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 1.8.
  parameter gravityTurnStart is 8000.
  parameter leadAngle is 5.
  parameter minPitch is 5.
  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.
    }
  }

  // Hold throttle to maintain target TWR.
  // We do *not* use a PID Loop here because we can
  // calculate the correct value discretely.
  lock THROTTLE to ThrottleToTWR(atmoTWR).
  print "Throttling to maintain a TWR  of " + atmoTWR.

  // Main ascent control.
  lock STEERING to Heading(90,90,270).
  stage.

  wait until SHIP:ALTITUDE > gravityTurnStart.

  print "Turning gravity...".
  lock STEERING to getAscentDir(leadAngle, minPitch).
  wait until SHIP:ALTITUDE > 32000. // 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.

  lock THROTTLE to 1.0.
  wait until SHIP:ORBIT:APOAPSIS > apoapsisTarget.

  lock THROTTLE to 0.0.
  set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.
  wait 0.001. // make sure these control updates get applied

  print "Target apoapsis acquired. Creating maneuver node.".
  add CreateCircularizationNode().
  ExecNode().
  print "Orbit acquired. Releasing controls. Good luck, Kerman.".
  unlock THROTTLE.
  unlock STEERING.
  SAS on.
}