// Provides stabilization functions, including Hover.


// Stabilizes a rocket relative to the surface, with optional vertical
// velocity. Do not include both this and helicopter_stabilize.ks in one
// program.
function Hover {
  parameter vertSpeed is 0.0.

  set done to false.
  SAS off.

  lock THROTTLE to 0.0.

  if SHIP:SENSORS:PRES = 0 {
    // In a vacuum, perform a more efficient vacuum maneuver.
    // This will call hoverAtmo after initial stabilization, to make sure
    // we don't flip upside down.
    hoverVac(vertSpeed).
  } else {
    hoverAtmo(vertSpeed).
  }

  set SHIP:CONTROL:PILOTMAINTHROTTLE to THROTTLE.
  unlock THROTTLE.
  unlock STEERING.
  SAS on.
  print "Stabilized operation ended. Returning control to pilot.".
  set done to false.
}

function hoverAtmo {
  parameter vertSpeed.

  local headingV is V(0,0,0).
  lock headingV to VectorExcl(SHIP:UP:FOREVECTOR, SHIP:SRFRETROGRADE).

  // set up PID controllers
  local pitchPID is PIDLoop(5, 0.1, 0.01, 0, 90).
  set pitchPID:SETPOINT to 0.

  local throttlePID is PIDLoop(0.1, 0.1, 0.001, 0, 1).
  set throttlePID:SETPOINT to vertSpeed.

  // we use the inverse of GROUNDSPEED because pitch and acceleration are
  // inversely proportional here.
  local newPitch is 0.0.
  lock newPitch to pitchPID:Update(TIME:SECONDS, -SHIP:GROUNDSPEED).
  lock STEERING to AngleAxis(-newPitch, SHIP:FACING:STARVECTOR) * headingV.
  lock THROTTLE to throttlePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
  wait until done.
}

function hoverVac {
  parameter vertSpeed.
  // TODO: implement me!
  hoverAtmo(vertSpeed).
}

// Convenience function for landing operations. Hover over a point with a negative velocity, shutting down on landing.
function Land {
  set done to false.
  when SHIP:STATUS = "LANDED" then {
    set done to true.
  }
  Hover(-0.5).
  set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0.
  set done to false.
}

// Rocket-style point stability control.
// Hover, ascend, or descend at a fixed rate.
// Adjusts the throttle to control target vertical speed.
// Uses retrograde lock to cancel horizontal velocity.
// TODO: how does this work
// function PointStabilizeR {
//   parameter vertSpeed is 0.0.

//   set done to false.
//   SAS off.

//   lock STEERING to SHIP:RETROGRADE.
//   wait until VectorAngle(
//     SHIP:FACING:FOREVECTOR,
//     SHIP:SRFRETROGRADE:FOREVECTOR) < 0.05.

//   local throttlePID is PIDLoop(0.1, 0.001, 0.001, 0, 1).
//   set throttlePID:SETPOINT to vertSpeed.
//   lock THROTTLE to throttlePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
//   wait until done.

//   set SHIP:CONTROL:PILOTMAINTHROTTLE to THROTTLE.
//   unlock THROTTLE.
//   unlock STEERING.
//   SAS on.
//   set done to false.
//   print "Stabilized operation ended. Returning control to pilot.".
// }