// Provides stabilization functions, including Hover.
//
// Must define a global variable 'done', which can be set to true to cancel
// control functions.
//
// Do not include both this and stabilize_helicopter.ks in one program.

runoncepath("/lib/sensors").

// Convenience function for landing operations. Hover over a point with a negative velocity, shutting down on landing.
function Land {
  set done to false.

  alignForHover().
  local pid is hoverPID().
  lock THROTTLE to pid:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
  until done or SHIP:STATUS = "LANDED" {
    set pid:SETPOINT to (-1 * (SHIP:ALTITUDE - SHIP:GEOPOSITION:TERRAINHEIGHT) / 10) - 5.
    wait 0.001.
  }
  
  set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0.
  set done to false.
}

// Stabilizes a rocket relative to the surface, with optional vertical
// velocity.
function Hover {
  parameter vertSpeed is 0.0.

  set done to false.
  SAS off.
  lock THROTTLE to 0.0.

  alignForHover().
  local throttlePID is hoverPID(vertSpeed).
  lock THROTTLE to throttlePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
  wait until done.

  // Unwind everything.
  restoreControls().
}

function hoverPID {
  parameter initSetpoint is 0.
  local pid is PIDLoop(0.01, 0.01, 0.001, 0, 1).
  set pid:SETPOINT to initSetpoint.
  return pid.
}

function alignForHover {
  if ReadSensor("pres") = 0 {
    // if we're in a vacuum, cancel all velocity first...
    lock STEERING to SHIP:SRFRETROGRADE.
    print "Aligning with retrograde.".
    wait until done or VAng(SHIP:FACING:FOREVECTOR, SHIP:SRFRETROGRADE:FOREVECTOR) < 0.1.
    if done {
      restoreControls().
      return.
    }
  } else {
    // ... otherwise just cancel vertical velocity
    lock STEERING to SHIP:UP.
    print "Aligning vertical.".
    wait until done or VAng(SHIP:FACING:FOREVECTOR, SHIP:UP:FOREVECTOR) < 1.
    if done {
      restoreControls().
      return.
    }
  }
}

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