// 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.

  assumeControl().
  alignForHover().
  if done {
    restoreControl().
    return.
  }
  local pid is PIDLoop(0.1, 0.01, 0.01, 0, 1).
  
  lock THROTTLE to pid:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
  local targetVel is 0.
  lock targetVel to Ceiling((-1 * (SHIP:ALTITUDE - SHIP:GEOPOSITION:TERRAINHEIGHT) / 10) - 5).
  until done or SHIP:STATUS = "LANDED" {
    set pid:SETPOINT to targetVel.
    wait 0.001.
  }

  restoreControl().
  set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0.
}

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

  assumeControl().
  alignForHover().
  if done {
    restoreControl().
    return.
  }
  local throttlePID is PIDLoop(0.1, 0.1, 0.001, 0, 1).
  set throttlePID:SETPOINT to vertSpeed.
  lock THROTTLE to throttlePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED).
  wait until done.

  // Unwind everything.
  restoreControl().
}

function hoverVectorVac {
  parameter top.
  if SHIP:VERTICALSPEED > 0 {
    return SHIP:UP.
  }
  return LookDirUp(SHIP:SRFRETROGRADE:FOREVECTOR, top).
}

function alignForHover {
  set top to SHIP:FACING:FOREVECTOR.
  if ReadSensor("PRES") = 0 {
    // if we're in a vacuum, align with retrograde for smoother horizontal control.
    lock STEERING to hoverDirVac(top).
    print "Aligning with retrograde.".
    wait until done or VAng(SHIP:FACING:FOREVECTOR, hoverDirVac():FOREVECTOR) < 1.
    if done {
      return.
    }
  } else {
    // ... otherwise just align vertically.
    lock STEERING to LookDirUp(SHIP:UP:FOREVECTOR, top).
    print "Aligning vertical.".
    wait until done or VAng(SHIP:FACING:FOREVECTOR, SHIP:UP:FOREVECTOR) < 1.
    if done {
      return.
    }
  }
}

function assumeControl {
  set done to false.
  SAS off.
  lock THROTTLE to 0.0.
}

function restoreControl {
  set SHIP:CONTROL:PILOTMAINTHROTTLE to THROTTLE.
  unlock THROTTLE.
  unlock STEERING.
  SAS on.
  print "Done.".
  set done to false.
}