local function iterate {
    parameter maneuver.
    // `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, y0:z, du0:x, du0:y, du0: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
            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.
        }
        // When we have convergeence, 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
}

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.
    coast(x0, q0, xf, qf, list(), list(), uk, t, false).
    print xf.
    print qf.
}

test_coast().