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

// Calculate the direction to lock during ascent.
function getClampedDir {
  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).
  if GetPitch(newHeading:FOREVECTOR) < minPitch {
    set newHeading to heading(90, minPitch, 270).
  }
  return newHeading.
}

// Given a target (end) pitch, a target duration, and a start time,
// returns the correct current heading.
function pitchProgram {
  parameter endPitch, duration, startTime.

  // what percent through the duration are we?
  local elapsedP is (TIME:SECONDS - startTime) / duration.
  local p is endPitch * elapsedP.
  return Heading(90, 90 - p, 270).
}

function Launch {
  parameter apoapsisTarget is 80000.
  parameter atmoTWR is 2.0.
  parameter minPitch is 5.
  parameter kickAngle is 20.
  parameter kickTime is 30.
  parameter kickStart is 100.
  parameter autoStage is true.

  // Configure subsystems.
  RCS off.
  SAS off.

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

  // 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).
  NoFuelResources(true).
  PreLaunchCrossfeed(false).
  stage.
  wait until SHIP:VERTICALSPEED > kickStart.
  
  print "Phase 2: Initial Pitch.".
  local startTime is TIME:SECONDS.
  lock STEERING to pitchProgram(kickAngle, kickTime, startTime).
  wait kickTime.

  print "Phase 3: Stable Prograde Boost.".
  lock STEERING to getClampedDir(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.".
  set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.
  unlock THROTTLE.
  unlock STEERING.
  SAS on.
}