kspkoscode

wait 3.
    clearScreen.
    local tel is true.
    set line to 0.
    set width to 26.
    when tel = true then {
        terminaldisplay (line, width).
        preserve.
    }
    wait 0.

    actguid(orbitdata).

/////////////////////////////////////

global function actguid {
    parameter orbdata.

    wait 0.
    set actmode to 1.
    set brnmode to actmode.
    local met is 45.
    local radalt is alt:radar.
    local canstage is false.
    local finalp is 2.5.
    local desiredpitch is 90.
    local azimuth is 0.
    local rcsact is true.
    local orbalt is orbdata[1].
    local gravityturnvel is orbdata[2].
    local tarinc is orbdata[3].
    local jtsfar is orbdata[4].
    local stgrcs is orbdata [5].
    local corrections is true.
    local shipmaxq is false.


    lock steering to heading (azimuth,desiredpitch).

    wait until alt:radar > radalt*3.

    set azimuth to flightaz(launchdata).

    set tarp to desiredpitch.
    set message to round(azimuth) + "° Launch Azimuth".

    wait until missionTime>10.
    set desiredpitch to 85.
    set tarp to desiredpitch.

    wait until ship:velocity:surface:mag >= gravityturnvel.

    set message to "Starting guideance".


    local wantthrottle is 1.
    lock throttle to wantthrottle.
    local tgtpitch is desiredpitch.
    set jtsfar to true.
    local oldthrust is ship:availablethrust.
    local q1 is ship:q.
    local time1 is time:seconds.

    set actmode to 2.
    set brnmode to actmode.

    until actmode <>2 {

        if missionTime>=met and canstage=false{
            set oldthrust to ship:availablethrust.
            set canstage to true.
        }

        if ship:availablethrust < (oldthrust-5) and canstage = true and missionTime>met{
            dostage().
            set message to "Staging "+stage:number.
            wait 0.
            set oldthrust to ship:availablethrust.
            wait 0.
        }

        local time2 is time:seconds.
        local q2 is ship:q.
        local a is ship:thrust/ship:mass.
        local vs is verticalSpeed.
        local h is (ship:body:atm:height+5000)-ship:altitude.
        local av is max(0.0000001,(a*(sin(tgtpitch)))).
        local timetofinalalt is ((-vs) + sqrt(max(0,vs^2 +2*av*h)))/(av).
        local pitchrate is (finalp-tgtpitch)/timetofinalalt.
        local dt is time2-time1.
        set deltatime to round(dt).

        set tgtpitch to min(85,max(finalp,(tgtpitch + dt*pitchrate))).
        set desiredpitch to min(85,max((90-(vang(srfprograde:vector,up:vector))-4),(tgtpitch + dt*pitchrate))).
        set tarp to round(tgtpitch).

        if q1 > q2 and shipmaxq=false{
            set message to "Max Q " + round(ship:q*constant:atmtokpa,3)+ " kPa".
            set shipmaxq to true.
        }

        if abs(orbit:inclination)<tarinc and corrections=true {
            set azimuth to flightaz(launchdata).
        }
        else {
            wait 0.
            set corrections to false.
            wait 0.
            set azimuth to heading_of_vector(ship:prograde:vector).
            wait 0.

        }

        if ship:q*constant:atmtokpa<5.5 and shipmaxq=true{
            set actmode to 3.
        }
        set time1 to time2.
        set q1 to q2.
        wait 0.
    }
    set brnmode to actmode.
    set throttlepid to pidLoop(0.1, 0.1, 0.01, (2/3), 1).
    set throttlepid:setpoint to 60.
    set wantthrottle to throttlepid:update(time:seconds, eta:apoapsis).
    until actmode <>3 {

        if ship:availablethrust < (oldthrust-5) and canstage = true and missionTime>met{
            dostage().
            set message to "Staging "+stage:number.
            wait 0.
            set oldthrust to ship:availablethrust.
            wait 0.
        }

        local time2 is time:seconds.
        local currentpitch is (90-(vang(up:vector,facing:vector))).
        local a is ship:thrust/ship:mass.
        local vs is verticalSpeed.
        local h is (ship:body:atm:height+5000)-ship:altitude.
        local av is max(0.0000001,(a*(sin(currentpitch)))).
        local timetofinalalt is ((-vs) + sqrt(max(0,vs^2 +2*av*h)))/(av).
        local pitchrate is (finalp-desiredpitch)/timetofinalalt.
        local dt is time2-time1.
        set deltatime to round(dt).

        set desiredpitch to max(finalp,(desiredpitch + dt*pitchrate)).
        set tarp to round(desiredpitch).
        set wantthrottle to throttlepid:update(time:seconds, eta:apoapsis).

        if currentpitch <= finalp+1 or eta:apoapsis < 45 set actmode to 4.
        if ship:periapsis >=0 set actmode to 5.

        if abs(orbit:inclination)<tarinc  and corrections=true and corrections=true {
            set azimuth to flightaz(launchdata).
        }
        else {
            wait 0.
            set corrections to false.
            wait 0.
            set azimuth to heading_of_vector(ship:prograde:vector).
            wait 0.

        }

        if ship:altitude >= 60_000 and jtsfar = true and ship:q*constant:atmtokpa <0.4{
            wait until stage:ready.
            jetfairing("far").
            extendpanels("far").
            deployantenna("far").
            set jtsfar to false.
        }

        if stage:number=stgrcs and rcsact=true{
            rcs on.
            set rcsact to false.
        }

        set time1 to time2.
        wait 0.
    }

    set brnmode to actmode.
    set wantthrottle to throttlepid:update(time:seconds, eta:apoapsis).
    until actmode <>4 {

        local time2 to time:seconds.
        local g is body:mu / (altitude + body:radius)^2.
        local dt is time2-time1.
        set deltatime to round(dt).

        if ship:availablethrust < (oldthrust-5) and canstage = true and missionTime>met{
            dostage().
            set message to "Staging "+stage:number.
            wait 0.
            set oldthrust to ship:availablethrust.
            wait 0.
        }

        set desiredpitch to min(max(((arcsin(g/velocity:orbit:mag)+2*(60-eta:apoapsis))+finalp),0),45).

        set tarp to round(desiredpitch).
        set wantthrottle to throttlepid:update(time:seconds, eta:apoapsis).

        if ship:periapsis >=0 set actmode to 5.

        if abs(orbit:inclination)<tarinc and corrections=true and corrections=true {
            set azimuth to flightaz(launchdata).
        }
        else {
            wait 0.
            set corrections to false.
            wait 0.
            set azimuth to heading_of_vector(ship:prograde:vector).
            wait 0.
            set message to corrections.

        }

        if ship:altitude >= 60_000 and jtsfar = true and ship:q*constant:atmtokpa <0.4{
            wait until stage:ready.
            jetfairing("far").
            extendpanels("far").
            deployantenna("far").
            set jtsfar to false.
        }

        if stage:number=stgrcs and rcsact=true{
            rcs on.
            set rcsact to false.
        }

        set time1 to time2.
        wait 0.
    }


    set brnmode to actmode.
    set throttlepid to pidLoop(0.1, 0.1, 0.01, 1/3, 1).
    set throttlepid:setpoint to 30.
    until actmode <>5 {

        if ship:availablethrust < (oldthrust-5) and canstage = true and missionTime>met{
            dostage().
            set message to "Staging "+stage:number.
            wait 0.
            set oldthrust to ship:availablethrust.
            wait 0.
        }

        set time2 to time:seconds.
        set dt to time2-time1.
        set deltatime to round(dt).

        set desiredpitch to (90-vang(prograde:vector,up:vector)).
        set wantthrottle to throttlepid:update(time:seconds, eta:apoapsis).

        if abs(orbit:inclination)<tarinc   and corrections=true and corrections=true {
            set azimuth to flightaz(launchdata).
        }
        else {
            wait 0.
            set corrections to false.
            wait 0.
            set azimuth to heading_of_vector(ship:prograde:vector).
            wait 0.

        }

        if ship:altitude >= 60_000 and jtsfar = true and ship:q*constant:atmtokpa <0.4{
            wait until stage:ready.
            jetfairing("far").
            extendpanels("far").
            deployantenna("far").
            set jtsfar to false.
        }

        if stage:number=stgrcs and rcsact=true{
            rcs on.
            set rcsact to false.
        }

        if ship:altitude>ship:orbit:body:atm:height+100 and ship:orbit:apoapsis>=orbalt-100{
            set actmode to 6.
        }

        set time1 to time2.
        wait 0.

    }
    set bnrmode to "Complete".
    lock throttle to 0.
    set canstage to false.
}