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

  insertionBurn(apoapsisTarget).

  print "Ascent Complete.".
  set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.
  unlock THROTTLE.
  unlock STEERING.
  SAS on.
}

// Calculate and perform the insertion burn for orbit.
// The reason to prefer this over CreateCircularizationNode() and ExecNode() is that the amount of RCS needed
// to vector into position during launch adjusts our orbit significantly. This gives us a "wait until" that
// serves the specific needs of launch.
// It also makes automated launches possible before maneuver nodes are unlocked.
//
// TODO: Refactor code reuse between this function and ExecNode().
//
// prerequisites:
//   * Vessel is still behind the Apoapsis
//   * Apoapsis is at the target height
function insertionBurn {
  parameter apoapsisTarget.

  // vector is prograde but only on the horizonal plane
  print "Adjusting heading.".
  local lock horizon is Vxcl(SHIP:UP:FOREVECTOR, SHIP:PROGRADE:FOREVECTOR).
  lock STEERING to LookDirUp(horizon, SHIP:FACING:TOPVECTOR).

  // calculate the deltaV to get us to the target velocity, for calculating burn start time.
  local t is TIME + SHIP:ORBIT:ETA:APOAPSIS.
  local Vc is sqrt(SHIP:BODY:MU/(PositionAt(SHIP, t) - SHIP:BODY:POSITION):MAG).
  local dV is Vc - VelocityAt(SHIP, t):ORBIT:MAG.

  if WillStage(dV) {
    when FlameOut() then {
      print "Flameout detected. Staging.".
      stage.
    }
  }
  
  // calculate the total burn time (Tb) and the burn start time (Ts)
  local Tb is BurnTime(dV).
  local Ts is TIME + SHIP:OBT:ETA:APOAPSIS - BurnTime(dV / 2).

  // warp to the burn point
  print "Waiting for burn window.".
  wait until (VAng(SHIP:FACING:FOREVECTOR, STEERINGMANAGER:TARGET:FOREVECTOR) <= 0.5) OR (TIME > Ts).
  if TIME < Ts - 2 {
    KUNIVERSE:TIMEWARP:WarpTo(Ts:SECONDS - 2).
  }
  wait until SHIP:UNPACKED.
  wait until TIME > Ts.

  // burn until periapsis is clear
  print "Circularizing orbit.".
  lock THROTTLE to 1.0.
  wait until SHIP:OBT:PERIAPSIS > apoapsisTarget.
  lock THROTTLE to 0.0.
}