RunOncePath("lib/throttle").
RunOncePath("lib/navigation").

// Calculate the direction to lock during ascent.
function getAscentDir {
  parameter leadAngle is 5.
  parameter minPitch is 5.

  // face just beneath prograde, but hold a solid eastern heading and don't
  // rotate the ship
  local newHeading is lookdirup(SHIP:SRFPROGRADE:FOREVECTOR, heading(90, 0, 270):TOPVECTOR).
  set newHeading to angleaxis(leadAngle, newHeading:TOPVECTOR)*newHeading.
  if getPitch(newHeading:FOREVECTOR) < minPitch {
    set newHeading to heading(90, minPitch, 270).
  }
  return newHeading.
}

// Returns the navball pitch of a given vector.
function getPitch {
  parameter v is SHIP:FACING:FOREVECTOR.
  return 90 - vectorangle(SHIP:UP:FOREVECTOR, v).
}

function Launch {
  parameter apoapsisTarget is 80000.
  parameter atmoTWR is 2.0.
  parameter leadAngle is 2.
  parameter minPitch is 5.
  parameter initialPitch is 10.
  parameter autoStage is true.

  // Configure subsystems.
  RCS off.
  SAS off.

  // Countdowns are cute.
  print "Initiating automated launch sequence.".
  from { local x is 5. } until x = 0 step { set x to x - 1. } do {
    print "..." + x.
    wait 0.5.
  }

  // staging logic. Stage as many times as needed until we finish ascent.
  // Once Apo target is attained just drop this trigger.
  if autoStage {
    when FlameOut() or SHIP:ORBIT:APOAPSIS > apoapsisTarget then {
      if SHIP:ORBIT:APOAPSIS > apoapsisTarget {
        return false.
      }
      stage.
      return true.
    }
  }

  // Drag controls
  when SHIP:VERTICALSPEED > 340 then {
    print "Throttling for drag control.".
    lock THROTTLE to ThrottleToTWR(atmoTWR).
    // TODO: if we have a pressure sensor we can use it to decide when to kick the throttle up instead, neat solution for e.g. Duna and Eve.
    when SHIP:ALTITUDE > 32000 then {
      lock THROTTLE to 1.0.
    }
  }

  
  print "Phase 1: Vertical Ascent.".
  lock THROTTLE to 1.0.
  lock STEERING to Heading(90,90,270).
  stage.
  wait until SHIP:VERTICALSPEED > 100.
  
  print "Phase 2: Initial Pitch.".
  lock STEERING to getAscentDir(leadAngle, minPitch).
  wait until getPitch(SHIP:SRFPROGRADE:FOREVECTOR) <= 90-initialPitch. 

  print "Phase 3: Stable Prograde Boost.".
  lock STEERING to getAscentDir(0, minPitch).
  wait until SHIP:ORBIT:APOAPSIS > apoapsisTarget.

  // TODO: A smoother approach based on target orbital velocity should be considered.
  print "Phase 4: Circularization Maneuver.".
  lock THROTTLE to 0.0.
  set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.
  wait 0.001. // make sure these control updates get applied
  add CreateCircularizationNode().
  ExecNode().

  print "Ascent Complete.".
  unlock THROTTLE.
  unlock STEERING.
  SAS on.
}