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

parameter APOAPSIS_TARGET is 80000.
parameter GRAVITY_TURN_START is 8000.
parameter GRAVITY_PITCH is 75.
parameter INITIAL_PITCH is 85.
parameter MINIMUM_PITCH is 40.
SensorCheck().

// Configure subsystems.
CORE:PART:GETMODULE("kOSProcessor"):DOEVENT("Open Terminal").
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.
}

// Hold throttle to maintain 1.5 TWR.
// We do *not* use a PID Loop here because we can
// calculate the correct value discretely.
lock THROTTLE to ThrottleToTWR(1.5).

// Main ascent control.
lock THROTTLE to 1.0.
lock STEERING to heading(90,90,270).
stage.

wait until SHIP:ALTITUDE > 200.
lock STEERING to heading(90, INITIAL_PITCH, 270).

wait until SHIP:ALTITUDE > GRAVITY_TURN_START.

print "Pitching for gravity turn".

// Pitch over...
lock STEERING to heading(90, GRAVITY_PITCH, 270).
print ("Locked to heading " + STEERINGMANAGER:TARGET). // debug?
// Wait until we have rotated to (approximately) that pitch...
wait until vectorangle(
  SHIP:FACING:FOREVECTOR,
  STEERINGMANAGER:TARGET:FOREVECTOR)
  < 0.5.
// then wait until Prograde catches up.
wait until vectorangle(
  SHIP:SRFPROGRADE:FOREVECTOR,
  STEERINGMANAGER:TARGET:FOREVECTOR)
  < 0.5.

print "Locking to prograde".

until SHIP:ORBIT:APOAPSIS > 80000 {
  // todo: we may need different values for bodies other than Kerbin.
  local newHeading is lookdirup(SHIP:SRFPROGRADE:FOREVECTOR,
    heading(90, 0, 270):TOPVECTOR).
  if GetPitch(newHeading:FOREVECTOR) < MINIMUM_PITCH {
    set newHeading to heading(90, MINIMUM_PITCH, 270).
  }
  lock STEERING to newHeading.
  wait 0.001.
}

lock THROTTLE to 0.0.
set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.
wait 0.001.
print "Target apoapsis acquired. Creating maneuver node".

AddCircularizationNode().

print "Releasing controls. Circularization maneuver added. Good luck, Kerman!".
unlock THROTTLE.
unlock STEERING.
SAS on.
wait 5.
CORE:PART:GETMODULE("kOSProcessor"):DOEVENT("Close Terminal").