launch_ssm_x2

set lorb to 80000.
set incl to 0.
set g_roll_correction to -90.

set ha to 59000.
set vac to 70000.
set angle to 5.
set gt0 to 800.
set gt1 to 45000.//48
set maxq to 8000.


lock correctRoll to R(0,0,g_roll_correction).
lock t_h to SHIP:GEOPOSITION:TERRAINHEIGHT.
lock radar_alt to SHIP:ALTITUDE-( (t_h + sqrt(t_h*t_h)) / 2 ).
set prog_mode to 1.
set tset to 1.
lock throttle to tset.
set pitch to 0.

FUNCTION eng_out {

    local numOut to 0.
    LIST ENGINES IN eng_list.
    FOR eng IN eng_list
        IF eng:FLAMEOUT
            SET numOut TO numOut + 1.
    return numOut.
}

lock angle1 to arcsin(max(-1,min(1,cos(180+incl)/cos(ship:latitude)))).
lock vlaunchx to (1600 * sin(angle1*-1))-(174.9422*sin(90)).
lock vlaunchy to (1600 * cos(angle1*-1))-(174.9422*cos(90)).
lock newangle to 90-arctan(vlaunchx/vlaunchy).
lock steering to HEADING(arcsin(max(-1,min(1,cos(180+newangle)/max(0.001,cos(ship:latitude))))), 90-pitch ) + R(0,0,90)+ correctRoll.

print "Ignition".
stage.

when radar_alt > gt0 or not (prog_mode = 1) then {
    SET WARP TO 2.
    if not ( prog_mode = 1) {return false.}
    print "T+" + round(missiontime) + " begin turn.".
    lock pitch to max(-90,min(-angle,((altitude-gt0)*19.9/body:atm:height)^0.55*-23)).

    when radar_alt > gt1 or altitude > ha or apoapsis > lorb or not (prog_mode = 1) then {
        if not ( prog_mode = 1) {return false.}
        print "T+" + round(missiontime) + " end turn.".
        set pitch to -90.
    }
}

when not((eng_out() = 0)) or ( not ( prog_mode = 1)) then {
    if not(prog_mode = 1) return false.

    if not((eng_out() = 0)) {
        print eng_out().
        stage.
    }
    return true.
}

on round(time:seconds,1) {

    if not ( prog_mode = 1) {return false.}

    if altitude > ha or apoapsis > lorb {
        return false.
    }

    // dynamic pressure q
    set vsm to velocity:surface:mag.
    set exp to -altitude/5000.
    set ad to 1.2230948554874 * 2.718281828^exp.    // atmospheric density
    set q to 0.5 * ad * vsm^2.
    // calculate target velocity
    set vl to maxq*0.9.
    set vh to maxq*1.1.
    if q < vl { set tset to 1. }
    if q > vl and q < vh { set tset to (vh-q)/(vh-vl). }
    if q > vh { set tset to 0.25. }

    if abs(STEERINGMANAGER:ANGLEERROR) > 1 {set tset to tset*abs(STEERINGMANAGER:ANGLEERROR). }
    if tset < 0.25 { set tset to 0.25. } //only in lower atmo

    if ship:velocity:surface:mag < 520 { set tset to 1. }
    print "pitch: " + round(pitch,2) + "  " at (0,25).
    print "alt:radar: " + round(radar_alt) + "  " at (0,26).
    print "q: " + round(q) + "  " at (0,27).
    print "throttle: " + round(tset,2) + "   " at (0,28).
    print "apoapis: " + round(apoapsis/1000,2) at (0,29).
    print "periapis: " + round(periapsis/1000,2) at (0,30).

    return true.
}

when altitude > 16000 or not (prog_mode = 1) then {

    if not ( prog_mode = 1) {return false.}

    set maxq to 7000.

    when altitude > 26000 or not (prog_mode = 1) then {

        if not ( prog_mode = 1) {return false.}

        lock steering to lookdirup(ship:srfprograde:vector, ship:facing:topvector).

    }
}

when (altitude > ha or apoapsis > lorb) or not (prog_mode = 1) then {

    if not ( prog_mode = 1) {return false.}

    print "stage2-part".
    set tset to 0.

    if altitude < vac {

        print "T+" + round(missiontime) + " Waiting to leave atmosphere".

        LOCK STEERING TO SHIP:PROGRADE.

        on round(time:seconds,1) {

            if (altitude > vac) or not (prog_mode = 1) {
                return false.
            }

            if apoapsis < lorb { set tset to (lorb-apoapsis)/(lorb*0.01). }
            else
            {
                set tset to 0.
            }
            set vsm to velocity:surface:mag.
            set exp to -altitude/5000.
            set ad to 1.2230948554874 * 2.718281828^exp.    // atmospheric density
            set q to 0.5 * ad * vsm^2.
            print "pitch: " + round(pitch,2) + "  " at (0,25).
            print "alt:radar: " + round(radar_alt) + "  " at (0,26).
            print "q: " + round(q) + "  " at (0,27).
            print "throttle: " + round(tset,2) + "   " at (0,28).
            print "apoapis: " + round(apoapsis/1000,2) at (0,29).
            print "periapis: " + round(periapsis/1000,2) at (0,30).

            return true.
        }
    }
}

when (altitude > vac) or not ( prog_mode = 1) then {

    if not ( prog_mode = 1) {return false.}

    print "stage3-part".

    unlock steering.
    SET WARP TO 0.
    set tset to 0.
    SET SHIP:CONTROL:PILOTMAINTHROTTLE TO 0.
    unlock throttle.

    set mu to 3.5316000*10^12.  // gravitational parameter, mu = G mass
    set rb to 600000.           // radius of body [m]
    set vom to velocity:orbit:mag.  // actual velocity
    set r to rb + altitude.         // actual distance to body
    set ra to rb + apoapsis.        // radius in apoapsis
    set va to sqrt( vom^2 + 2*mu*(1/ra - 1/r) ). // velocity in apoapsis
    set a to (periapsis + 2*rb + apoapsis)/2. // semi major axis present orbit
    // future orbit properties
    set r2 to rb + apoapsis.    // distance after burn at apoapsis
    set a2 to (lorb + 2*rb + apoapsis)/2. // semi major axis target orbit
    set v2 to sqrt( vom^2 + (mu * (2/r2 - 2/r + 1/a - 1/a2 ) ) ).
    // setup node
    set deltav to v2 - va.
    print "T+" + round(missiontime) + " Apoapsis burn: " + round(va) + ", dv:" + round(deltav) + " -> " + round(v2) + "m/s".
    set nd to node(time:seconds + eta:apoapsis, 0, 0, deltav).
    add nd.

}

wait until false.