@LAZYGLOBAL off.

run once "lib/equations".
run once "lib/node".
run once "lib/rocket".
run once "lib/util".
run once "lib/vectors".
run once "lib/warp".


function launch {
    // Launch to orbit of provided altitude and inclination.
    parameter target_orbit_altitude is 100_000.  // meters
    parameter target_orbit_inclination is 0.  // 0-180 degrees
    parameter pitchover_tilt is 20.  // how many degrees to tilt at pitchover maneuver
    parameter pitchover_altitude is 1000.  // perform pitchover at this altitude
    parameter pitchover_velocity is 100.  // or this velocity

    parameter auto_stage is true.
    parameter auto_deploy_solar_panels is true.
    parameter auto_deploy_fairings is true.
    parameter auto_extend_antennas is true.

    
    print "================ ASCEND GUIDANCE =================".
    print "Target orbital altitude:       " + target_orbit_altitude + "m".
    print "Target orbital inclination:    " + target_orbit_inclination + "°".
    print "Pitchover tilt:                " + pitchover_tilt + "°".
    print "Pitchover at:                  " + pitchover_altitude + "m or " + pitchover_velocity + " m/s".
    print " ".

    // Since the "center" of an orbit must be at the center of gravity of the body, the latitude of the launch site establishes the minimum absolute orbital inclination.
    // KSC is almost on the equator, so we're going to always round the latitude towards zero to allow any inclination from KSC and accept the inaccuracies it may introduce.
    local launch_site_latitude is int(LATITUDE).
    local target_orbit_inclination is max(target_orbit_inclination, launch_site_latitude).
    
    // Calculate the launch azimuth; the compass heading we head for when launching to achieve orbit of desired inclination
    local launch_azimuth is calculate_launch_azimuth(target_orbit_inclination, target_orbit_altitude, launch_site_latitude).
    
    // If the latitude of the launch site is negative (ship is in the southern hemisphere), launch southwards instead of northwards
    local southwards is launch_site_latitude < 0.
    if southwards {
        set launch_azimuth to 180 - launch_azimuth.
    }
    
    print "Launch site latitude:          " + round(LATITUDE, 3) + "°  (~" + launch_site_latitude + "°)".
    print "Available orbital inclination: " + target_orbit_inclination + "°".
    print "Launch azimuth:                " + round(launch_azimuth, 3) + "°".
    print "Launch southwards:             " + southwards.
    print " ".
    print "Auto-staging:                  " + auto_stage.
    print "Auto-deploy solar panels:      " + auto_deploy_solar_panels.
    print "Auto-deploy fairings:          " + auto_deploy_fairings.
    print "Auto-extend antennas:          " + auto_extend_antennas.
    print " ".


    // LAUNCH

    SAS off.
    RCS on.
    set NAVMODE to "SURFACE".
    lock STEERING to heading(launch_azimuth, 90).  // roll to launch azimuth
    lock THROTTLE to 1.0.

    when auto_stage and should_stage() then {
        stage_when_ready().
        wait 0.5.
        return true.  // preserve trigger
    }

    print "----------------- VERTICAL CLIMB -----------------".

    print "Waiting for pitchover altitude or velocity".
    wait until ALTITUDE > pitchover_altitude
            or VELOCITY:surface:mag > pitchover_velocity.
    
    
    print "------------------- PITCHOVER --------------------".
    // Once a certain altitude or velocity is reached, a slight turn is made, called the pitchover maneuver
    lock STEERING to heading(launch_azimuth, 90-pitchover_tilt).

    print "Waiting for prograde vector to catch up".
    wait until actual_prograde_pitch() > pitchover_tilt.
    
    
    print "------------------ GRAVITY TURN ------------------".
    // TODO: the angle of the launch azimuth will not account for the fact that our compass will change as we move north/south.
    lock STEERING to heading(launch_azimuth, 90-actual_prograde_pitch()).  // Follow prograde pitch to get 0 deg angle of attack, but force compass heading at launch azimuth.

    print "Waiting for apoapsis to match target altitude".
    wait until APOAPSIS > target_orbit_altitude.
    lock THROTTLE TO 0.
    
    
    print "------------------ CIRCULARIZE -------------------".
    // Don't create maneuver node until we are out of the atmosphere; otherwise the apoapsis' altitude and eta will change due to drag
    print "Waiting for ship to leave the atmosphere".
    warp_for(atmosphere_exit_eta()).  // we'll lose some velocity due to drag, so the warp will exit a few seconds before we actually exit the atmosphere
    wait until SHIP:dynamicpressure = 0.  // that's why we have this check as well

    if auto_deploy_fairings {
        deploy_fairings().
        wait 3.  // wait for fairings to clear the vessel
    }
    if auto_deploy_solar_panels {
        print "Deploying solar panels".
        PANELS on.
    }
    if auto_extend_antennas {
        extend_antennas().
    }
    
    // Create maneuver node that will circularize the orbit. TODO: create circulize()-function in lib/maneuvers.ks instead
    // NOTE: Potential errors in the inclination are not fixed, since we are most likely going to change our orbit, which will make the inclination change cheaper later on.
    local node is NODE(TIME:SECONDS + ETA:APOAPSIS, 0, 0, 0).

    // Burn magnitude in the prograde direction is the difference between the required velocity to acheive circular orbit at apoapsis altitude and our velocity at apoapsis
    local required_velocity is orbital_velocity_from_ap_pe(APOAPSIS, APOAPSIS, target_orbit_altitude).
    set node:prograde to required_velocity - velocityat(SHIP, TIME:seconds + ETA:apoapsis):orbit:mag.
    
    add node.
    execute_node().
    
    
    print "------------ LAUNCH SEQUENCE COMPLETE ------------".
    unlock_control().
}


function calculate_launch_azimuth {
    // Calculate the launch azimuth; the compass heading we head for when launching to achieve orbit of desired inclination.
    // http://www.orbiterwiki.org/wiki/Launch_Azimuth
    // https://www.princeton.edu/~stengel/MAE342Lecture4.pdf
    parameter target_orbit_inclination.
    parameter target_orbit_altitude.  // to compensate for the rotation of the body, we need to know the velocity of the target orbit, which is calculated from its altitude
    parameter launch_site_latitude.
    
    local inertial_azimuth is arcsin(cos(target_orbit_inclination) / cos(launch_site_latitude)).  // azimuth in inertial space, that is, disregarding the rotation of the body
    
    local equatorial_rotational_velocity is (2 * CONSTANT:PI * BODY:radius) / BODY:rotationperiod.
    local target_orbit_velocity is orbital_velocity_from_ap_pe(target_orbit_altitude, target_orbit_altitude, target_orbit_altitude).

    local launch_vector_x_component is target_orbit_velocity * sin(inertial_azimuth) - equatorial_rotational_velocity * cos(launch_site_latitude).
    local launch_vector_y_component is target_orbit_velocity * cos(inertial_azimuth).
    return arctan2(launch_vector_x_component, launch_vector_y_component).
}


launch(100_000, 45).