local function iterate {
    parameter maneuver, no.
    // `maneuver` is a lexicon of:
    // "mass" - vehicle mass at start of maneuver
    // "mu" - gravitational constant
    // "hmax" - maximum step size
    // "itmax" - maximum number of iterations
    // "r0" - initial position
    // "v0" - initial velocity
    // "u0" - initial heading
    // "du0" - initial change in heading
    // "burns" - list of lexicon
    //   "start" - burn start time
    //   "end" - burn end time
    // "initial_coast" - either start time of coast, or `false` if none
    // "stages" - list of lexicon
    //   "thrust" - engine thrust
    //   "massflow" - fuel consumption of engine
    //   "fuel" - total amount of fuel in the stage
    //   "boiloff_rate" - how quickly fuel evaporates when not being used
    //   "dry_mass" - mass lost when vehicle stages
    // "c" - target orbit parameters (TODO)

    local num_legs is burns:length * 2.
    if initial_coast = false {
        set num_legs to num_legs - 1.
    }

    for iter in RANGE(itmax) {
        local q0 is list(u0:x, u0:y, u0:z, du0:x, du0:y, du0:z).
        local q01 is list(u0:x, u0:y, u0:z, du0:x, du0:y, dy0:z).
        local x0 is list(r0:x, r0:y, r0:z, v0:x, v0:y, v0:z).
        local xf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
        local qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).

        // Initialization of matrix of partials
        // z(i,j) partial of state and costate with respect to initial costate and switching times
        // e(i,j) partial of right end variables and switching conditions with respect to initial costate and switching times
        local z is list().
        local e is list().
        for i in RANGE(12) {
            z:add(list()).
            e:add(list()).
            for j in RANGE(6 + num_legs) {
                z[i]:add(0.0).
                e[i]:add(0.0).
            }
            e[i]:add(0.0).
        }
        local leg is 0.
        local leg5 is 5.
        local leg6 is 6.
        local leg7 is 7.
        local l7 is num_legs+6.
        local ump is 0.0.
        local uk is maneuver:mu.
        local prev_burn is false.
        for burn in maneuver:burns {
            if (leg <> 0) or (initial_coast <> false) {
                local phi is list().
                local dphi is list().
                for i in range(6) {
                    phi:add(list()).
                    dphi:add(list()).
                    for j in range(6) {
                        phi[i]:add(0.0).
                        dphi[i]:add(0.0).
                    }
                }

                local start is false.
                if prev_burn <> false {
                    set start to prev_burn:end.
                } else {
                    set start to maneuver:initial_coast.
                }
                local end is burn:start.
                local t is end - start.
                // phi(i,j) partial of state at end of coast with respect to state at start of coast
                // dphi(i,j) partial of costate at end of coast with respect to state at start of coast
                coast (x0, q0, xf, qf, phi, dphi, uk, t, no).

                local dz is list().
                for i in range(12) {
                    dz:add(list()).
                    for j in RANGE(leg6) {
                        dz[i]:add(0.0).
                    }
                }
                for i in range(6) {
                    local i6 is i + 6.
                    for j in range(leg6) {
                        for k in range(6) {
                            set dz[i][j] to dz[i][j] + phi[i][k] * z[k][j].
                            set dz[i6][j] to dz[i6][j] + phi[i][k] * z[k+6][j] + dphi[i][k] * z[k][j].
                        }
                    }
                }
                // Update matrix of partials Z
                for i in range(12) {
                    for j in range(leg6) {
                        set z[i][j] to dz[i][j].
                    }
                }
                local um is sqrt(qf[0]*qf[0]+qf[1]*qf[1]+qf[2]*qf[2]).
                local cbmu is burn:thrust / (mass*um).
                for i in range(3) {
                    set z[i+3][leg6] to -cbmu * qf[i].
                }
                if leg <> 0 {
                    // Calculation of switching condition and corresponding partial
                    set e[leg5][l7] to ump - um.
                    for j in range(leg5+1) {
                        set e[leg5][j] to -e[leg5][j].
                        for k in range(3) {
                            set e[leg5][j] to e[leg5][j] + (qf[k]/um)*z[k+6][j].
                        }
                    }
                }
                set leg to leg + 1.
                set leg5 to leg + 5.
                set leg6 to leg + 6.
                set leg7 to leg + 7.

                // Calculation of transversality condition and corresponding partial with respect to initial costate and switching times
                local r2 is xf[0]*xf[0] + xf[1]*xf[1] + xf[2]*xf[2].
                local rs is xf[0]*qf[0] + xf[1]*qf[1] + xf[2]*qf[2].
                local c3 is -uk / (r2 * sqrt(r2)).
                local c4 is -3.0 * c3 * rs / r2.
                set e[leg5][l7] to xf[3]*qf[3]+xf[4]*qf[4]+xf[5]*qf[5] - c3*rs - e[leg5][l7].
                set dummy to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
                for i in range(3) {
                    set dummy[i] to -c3*qf[i]-c4*xf[i].
                    set dummy[i+3] to qf[i+3].
                    set dummy[i+6] to - c3 * xf[i].
                    set dummy[i+9] to xf[i+3].
                }
                for j in range(leg7) {
                    for k in range(12) {
                        set e[leg5][j] to -dummy[k]*z[k][j] + e[leg5][j].
                    }
                }
                for j in range(leg7) {
                    set e[leg5][j] to -e[leg5][j].
                }
                set e[leg5][leg5] to 0.0.

                set x0 to xf.
                set q0 to qf.
                set xf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
                set qf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
            }
            // burn
            // Called to propagate burn arcs
            // x0(q0) - state(costate) at start of burn
            // xf(qf) - state(costate) at end of burn
            rkg031(x0, q0, xf, qf, z, evt, hmax, maneuver, no).
            if leg < (num_legs-1) {
                local um is sqrt(qf[0]*qf[0]+qf[1]*qf[1]+qf[2]*qf[2]).
                set ump to um.
                local cbmu is burn:thrust / (mass * um).
                for i in range(3) {
                    set z[i+3][leg6] to cbmu * qf[i].
                }
                for i in range(leg7) {
                    for k in range(3) {
                        set e[leg6][j] to e[leg6][j] + (qf[k]/um)*z[k+6][j].
                    }
                }
            }
            if leg <> 0 {
                local r2 is xf[0]*xf[0] + xf[1]*xf[1] + xf[2]*xf[2].
                local rs is xf[0]*qf[0] + xf[1]*qf[1] + xf[2]*qf[2].
                local c3 is -uk / (r2 * sqrt(r2)).
                local c4 is -3.0 * c3 * rs / r2.
                set e[leg5][l7] to xf[3]*qf[3]+xf[4]*qf[4]+xf[5]*qf[5] - c3*rs - e[leg5][l7].
                set dummy to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
                for i in range(3) {
                    set dummy[i] to -c3*qf[i]-c4*xf[i].
                    set dummy[i+3] to qf[i+3].
                    set dummy[i+6] to - c3 * xf[i].
                    set dummy[i+9] to xf[i+3].
                }
                for j in range(leg7) {
                    for k in range(12) {
                        set e[leg5][j] to -dummy[k]*z[k][j] + e[leg5][j].
                    }
                }
            }
            set leg to leg + 1.
            set leg5 to leg + 5.
            set leg6 to leg + 6.
            set leg7 to leg + 7.
            set prev_burn to burn.
            set x0 to xf.
            set q0 to qf.
            set xf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
            set qf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
        }
        // bveval(xf, qf, z, c, e, dc).
        // When we have convergeence, break
        for i in range(6) {
            local i6 is i+6.
            set e[i][l7] to dc[i].
            set dc[i+6] to e[i+7][l7].
            set e[leg6][i] to q01[i].
        }
        if no  {
            // called to solve simultaneous equations and determine new initial
            // costate and switching times. Q01- initial costate
            // adjust(q01, e, z, du, dudt, legmax, atp).

            if du <0.0001 or dudt < 0.001 {
                set no to true.
            }
        } else {
            break.
        }
    }
}

local function coast {
    parameter x0, q0, xf, qf, phi, dphi, uk, t, no.
    local jump is false.
    local rm0 is sqrt(x0[0]*x0[0] + x0[1]*x0[1] + x0[2]*x0[2]).
    local drm0 is (x0[0]*q0[0] + x0[1]*q0[1] + x0[2]*q0[2])/rm0.
    local sig0 is x0[0]*x0[3] + x0[1]*x0[4] + x0[2]*q0[5].
    local dsig0 is x0[3]*q0[0] + x0[4]*q0[1] + x0[5]*q0[2] + x0[0]*q0[3] + x0[1]*q0[4] + x0[2]*q0[5].
    local alpha is x0[3]*x0[3] + x0[4]*x0[4] + x0[5]*x0[5] - 2.0 * uk / rm0.
    local h0 is list(
        x0[1]*x0[5] - x0[2]*x0[4],
        x0[2]*x0[3] - x0[0]*x0[5],
        x0[0]*x0[4] - x0[1]*x0[3]
    ).
    local p0 is (h0[0]*h0[0] + h0[1]*h0[1] + h0[2]*h0[2])/uk.
    local psy is t/p0.
    local alpsq is sqrt(-alpha). // This probably blows up for non-eliptical orbits?
    local s0 is 0.0.
    local s1 is 0.0.
    local s2 is 0.0.
    local s3 is 0.0.
    local ft is t.
    local rm is rm0.
    until false {
        local alpsy is psy * alpsq.
        set s0 to cos(alpsy*CONSTANT:RadTodeg).
        set s1 to sin(alpsy*CONSTANT:RadToDeg)/alpsq.
        set s2 to (s0 - 1.0) / alpha.
        set s3 to (s1 - psy) / alpha.
        set ft to rm0 * s1 + sig0 * s2 + uk * s3.
        set rm to rm0*s0 + sig0*s1 + uk*s2.
        if jump {
            break.
        }
        set psy to psy + (t-ft) / rm.

        if abs(t-ft) < abs(t) * 0.00001 {
            set jump to true.
        }
    }
    print psy.
    print alpha.
    print ft.
    local fm1 is -uk * s2 / rm0.
    local f is 1.0 + fm1.
    local fd is -uk * s1 / (rm * rm0).
    local g is ft - uk * s3.
    local gdm1 is -uk * s2 / rm.
    local gd is 1.0 + gdm1.
    local ukr3 is uk / (rm * rm * rm).
    local ukr03 is uk / (rm0 * rm0 * rm0).
    local dalph is 2.0 * (x0[3]*q0[3] + x0[4]*q0[4] + x0[5]*q0[5] + ukr03 * (x0[0]*q0[0] + x0[1]*q0[1] + x0[2]*q0[2])).
    local dapa is dalph/alpha.
    local dapa2 is dapa/alpha.
    local dpsy is -(drm0*s1 + dsig0*s2 + rm0*(psy*s0-s1)*dapa*0.5 + sig0*(psy*s1*0.5 - s2)*dapa + uk*(psy - 1.5*s1 + psy*s0*0.5)*dapa2)/rm.
    local ds0 is (alpha*dpsy + 0.5*psy*dalph)*s1.
    local ds1 is s0*dpsy + (psy*s0 - s1)*dapa*0.5.
    local ds2 is s1*dpsy + (0.5*psy*s1 - s2)*dapa.
    local ds3 is s2*dpsy + (psy - 1.5*s1 + 0.5*psy*s0)*dapa2.
    local s4 is (s2 - psy*psy*0.5)/alpha.
    local ds4 is s3*dpsy + (psy*psy*0.5 - 2.0*s2 + 0.5*psy*s1)*dapa2.
    local s5 is (s3 - psy*psy*psy/6.0)/alpha.
    local ds5 is s4*dpsy+(psy*psy*psy/6.0 + (2.0*psy - 2.5*s1 + 0.5*psy*20)/alpha)*dapa2.
    local u is s2*ft + uk*(psy*s4 - 3.0*s5).
    local du is ds2*ft + uk*(dpsy*s4 + psy*ds4 - 3.0*ds5).
    local drm is 20*drm0 + ds0*rm0 + s1*dsig0 + ds1*sig0 + uk*ds2.
    local df is (-uk*ds2 - fm1*drm0)/rm0.
    local dg is -uk*ds3.
    local dgd is (-uk*ds2 - gdm1*drm)/rm.
    local r01 is rm0*rm.
    local dr01 is rm*drm0 + drm*rm0.
    local dfd is (-uk*ds1 - fd*dr01)/r01.
    for i in range(3) {
        set xf[i] to x0[i]*f + x0[i+3]*g.
        set xf[i+3] to x0[i]*fd + x0[i+3]*gd.
        set qf[i] to x0[i]*df + x0[i+3]*dg + q0[i]*f + q0[i+3]*g.
        set qf[i+3] to x0[i]*dfd + x0[i+3]*dgd + q0[i]*fd + q0[i+3]*gd..
    }

    if no = false {
        return.
    }
    // calculation of partials
    set dukr3 to -3.0*ukr3*drm/rm.
    set dur03 to -3.0*ukr03*drm0/rm0.
    set s1r0 to s1/rm0.
    set ds1r0 to (ds1-s1r0*drm0)/rm0.
    set s1r to s1/rm.
    set ds1r to (ds1-s1r*drm)/rm.
    set r02 to 1.0/(rm0*rm0).
    set r2 to 1.0/(rm*rm).
    set uuk03 to -u*ukr03.
    set duuk3 to -du*ukr03-u*dur03.

    local ann is list(
        list(
            -fd*s1r0 - fm1*r02,
            -fd*s2
        ),
        list(
            fm1*s1r0 + uuk03,
            fm1*s2
        )
    ).
    local dumm1 is ann[0][0].
    local dan is list(
        list(
            -fd*ds1r0 - dfd*s1r0 + ukr03*ds2 + dur03*s2,
            -dfd*s2 - fd*ds2
        ),
        list(
            fm1*ds1r0 + df*s1r0 + duuk3,
            fm1*ds2 + df*s2
        )
    ).
    local dummy is dan[0][0].
    local bnn is list().
    local dbnn is list().
    for i in range(3) {
        bnn:add(list()).
        dbnn:add(list()).
        for j in range(3) {
            bnn[i]:add(0.0).
            dbnn[i]:add(0.0).
        }
    }

    amult(ann, dan, bnn, dbnn, x0, q0, xf, qf).
    for i in range(3) {
        for j in range(3) {
            set dphi[i][j] to dbnn[i][j].
            set phi[i][j] to bnn[i][j].
        }
    }
    for i in range(3) {
        set dphi[i][i] to dphi[i][i] + df.
        set phi[i][i] to phi[i][i] + f.
    }

    set ann[0][0] to ann[0][1].
    set ann[1][0] to ann[1][1].
    set ann[0][1] to -gdm1*ds2 - dgd*s2.
    set ann[1][1] to g*s2 - u.
    set dan[0][0] to dan[0][1].
    set dan[1][0] to dan[1][1].
    set dan[0][1] to -gdm1*ds2 - dgd*s2.
    set dan[1][1] to -du + dg*s2 + g*ds2.

    amult(ann, dan, bnn, dbnn, x0, q0, xf, qf).
    for i in range(3) {
        for j in range(3) {
            local j3 is j+3.
            set dphi[i][j3] to dbnn[i][j].
            set phi[i][j3] to bnn[i][j].
        }
    }
    for i in range(3) {
        local i3 is i + 3.
        set dphi[i][i3] to dphi[i][i3] + dg.
        set phi[i][i3] to phi[i][i3] + g.
    }

    set ann[1][0] to -ann[0][0].
    set ann[1][1] to -ann[0][1].
    set ann[0][0] to -fd*s1r - gdm1*r2.
    set ann[0][1] to u*ukr3 - gdm1*s1r.
    set dan[1][0] to -dan[0][0].
    set dan[1][1] to -dan[0][1].
    set dan[0][0] to -dfd*s1r -fd*ds1r + ukr3*ds2 + dukr3*s2.
    set dan[0][1] to -gdm1*ds1r - dgd*s1r + du*ukr3 + u*dukr3.

    amult(ann, dan, bnn, dbnn, x0, q0, xf, qf).
    for i in range(3) {
        local i3 is i+3.
        for j in range(3) {
            local j3 is j+3.
            set dphi[i3][j3] to dbnn[i][j].
            set phi[i3][j3] to bnn[i][j].
        }
    }
    for i in range(3) {
        local i3 is i + 3.
        set dphi[i3][i3] to dphi[i][i3] + dgd.
        set phi[i3][i3] to phi[i][i3] + gd.
    }

    set ann[0][1] to ann[0][0].
    set ann[1][1] to ann[1][0].
    set ann[1][0] to -dumm1.
    set ann[0][0] to -fd*(s0/r01 + r2 + r02) - uuk03*ukr3.
    set dan[0][1] to dan[0][0].
    set dan[1][1] to dan[1][0].
    set dan[1][0] to -dummy.
    set dan[0][0] to -uuk03*dukr3 - duuk3*ukr3 - dfd*(s0/r01 + r2 + r02) - fd*((ds0 - s0*dr01/r01)/r01 - 2.0*(r2*drm/rm + r02*drm0/rm0)).

    amult(ann, dan, bnn, dbnn, x0, q0, xf, qf).
    for i in range(3) {
        local i3 is i+3.
        for j in range(3) {
            set dphi[i3][j] to dbnn[i][j].
            set phi[i3][j] to bnn[i][j].
        }
    }
    for i in range(3) {
        local i3 is i + 3.
        set dphi[i3][i] to dphi[i][i3] + dfd.
        set phi[i3][i] to phi[i][i3] + fd.
    }
}

local function amult {
    parameter ann, dan, bnn, dbnn, x0, q0, xf, qf.
    local da is list().
    local a is list().
    for i in range(3) {
        da:add(list()).
        a:add(list()).
        for j in range(2) {
            da[i]:add(qf[i]*ann[0][j] + qf[i+3]*ann[1][j] + xf[i]*dan[0][j] + xf[i+3]*dan[1][j]).
            a[i]:add(ann[0][j]*xf[i] + ann[1][j]*xf[i+3]).
        }
    }
    for i in range(3) {
        for j in range(3) {
            set dbnn[i][j] to a[i][0]*q0[j] + a[i][1]*q0[j+3] + da[i][0]*x0[j] + da[i][1]*x0[j+3].
            set bnn[i][j] to a[i][0]*x0[j] + a[i][1]*x0[j+3].
        }
    }
}

local function test_coast {
    local x0 is list(4551.3085900, 4719.8398400, 25.0576324, 5.5990610, -5.4170895, -0.0118389).
    local q0 is list(0.4601788, -0.8868545, 0.0415273, -0.0004394, -0.0010504, -0.0004081).
    local uk is 398601.5.
    local xf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
    local qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
    local t is 2047.6868 - 934.
    local phi is list().
    local dphi is list().
    for i in range(6) {
        phi:add(list()).
        dphi:add(list()).
        for j in range(6) {
            phi[i]:add(0.0).
            dphi[i]:add(0.0).
        }
    }
    coast(x0, q0, xf, qf, phi, dphi, uk, t, true).
    print xf.
    print qf.
}

test_coast().