@lazyglobal off.

runoncepath("0:/lib/common.ks").

global atlas is lex(
        "launch", launch@
        ).

local function launch {
    parameter hdg, target_ap, target_pitch is 6.

    lock throttle to 1.0.

    local engine is ship:partsdubbed("ROE-LR89.2")[0].
    stage.
    wait until stage:ready.
    wait until engine:thrust > (engine:maxthrust * 0.9).
    stage.
    local booster_drop is time:seconds + 160.

    local target_vel is 150.
    lock steering to heading(hdg, 90 - min(target_pitch, target_pitch * (ship:velocity:surface:mag / target_vel)), -hdg).
    wait until ship:velocity:surface:mag > target_vel.
    print "Heading stable, switching to gravity turn".
    set warpmode to "physics".
    set warp to 3.

    lock steering to lookdirup(ship:srfprograde:forevector, ship:up:forevector).

    wait until time:seconds > booster_drop.
    set warp to 0.
    wait until kuniverse:timewarp:issettled.
    stage.
    wait 2.
    set warp to 3.

    wait until ship:altitude > 100000.
    print "Dropping Fairings".
    if not ship:partsdubbed("SSTUHollowRing"):empty {
        set warp to 0.
        wait until kuniverse:timewarp:issettled.
        print "Dropping Fairings".
        stage.
        wait 2.
        set warp to 3.
    }
    print "Switching to target PE mode".
    lock steering to insert_orbit_pe(hdg, target_ap).

    wait until ship:thrust = 0.
    unlock steering.
    lock throttle to 0.0.
    set warp to 0.
    wait until kuniverse:timewarp:issettled.
}

local function drawvec {
    parameter vec, color, desc.
    local origin is ship:position.
    return vecdraw(origin, origin + vec:normalized, color, desc, 5.0, TRUE, 0.2, TRUE, FALSE).
}

local function insert_orbit_pe {
    parameter azimuth, target_ap.
    //clearvecdraws().
    local ap_hdg is ap_prograde().
    local origin is ship:position.

    //drawvec(ship:facing:forevector, RGB(0, 1, 0), "Ship Facing").
    //drawvec(ap_hdg:forevector, RGB(1, 1, 0), "AP Prograde").

    local ap_delta is ship:orbit:apoapsis - target_ap.
    local cur_burn_angle is mod(vang(vxcl(ship:facing:forevector, ap_hdg:starvector), ap_hdg:forevector) + 360, 360).
    local ap_burn_ang is symmetric_clamp(ap_delta/3000, 90).
    local target_burn_hdg is angleaxis(ap_burn_ang, ap_hdg:starvector) * ap_hdg.

    //drawvec(target_burn_hdg:forevector, RGB(1, 0, 0), "Non-azimuth-corrected burn vector").

    // Vector pointing exactly towards our azimuth, flat with our orbital position
    local azimuth_vector is heading(azimuth, 0, 0):forevector.
    //drawvec(azimuth_vector, RGB(0, 1, 1), "Azimuth").

    // Vector pointing exactly towards AP, flat with our orbital position
    local steering_vector is vxcl(ship:up:forevector, target_burn_hdg:forevector).
    //drawvec(steering_vector, RGB(1, 0, 1), "AP Steering Vector").

    // Angle to rotate to bring our AP vector to our azimuth
    local azimuth_ang is vang(steering_vector, azimuth_vector).

    local azimuth_adjustment is angleaxis(-azimuth_ang, ship:up:forevector).
    local azimuth_burn_hdg is azimuth_adjustment * target_burn_hdg.
    //drawvec(azimuth_burn_hdg:forevector, RGB(0, 1, 0), "Azimuth-corrected burn vector").

    local steering_ang is symmetric_clamp(vang(ship:facing:forevector, azimuth_burn_hdg:forevector), 10).
    local steering_hdg is angleaxis(steering_ang, vcrs(ship:facing:forevector, azimuth_burn_hdg:forevector)) * ship:facing.
    return steering_hdg.
}

local function symmetric_clamp {
    parameter val, limit.
    return min(max(val, -limit), limit).
}