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

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

  if SHIP:SENSORS:PRES = 0 {
    lock STEERING to SHIP:SRFRETROGRADE.
    wait until VAng(SHIP:FACING:FOREVECTOR, SHIP:SRFRETROGRADE:FOREVECTOR) < 10.
  } else {
    // debug
    print "Making vector visualization.".
    local d1 is VecDraw(V(0,0,0), V(0,0,0), BLUE, "", 1.0, true).
    set d1:VECUPDATER to {
      return atmoSafeRetrograde() * 100.
    }.
    print "Vector visualization created.".
    // end debug

    lock STEERING to atmoSafeRetrograde().
    wait until done or VAng(SHIP:FACING:FOREVECTOR, atmoSafeRetrograde()) < 10.
  }

  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:VELOCITY:SURFACE:MAG). // this is still wrong, we just burn hard and spin out of control.

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

// 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(-4).
  set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.0.
  set done to false.
}

function atmoSafeRetrograde {
  if VAng(SHIP:SRFRETROGRADE:FOREVECTOR, SHIP:UP:FOREVECTOR) < 45 {
    return SHIP:SRFRETROGRADE:FOREVECTOR.
  }
  local planeRetro is VXCL(SHIP:UP:FOREVECTOR, SHIP:SRFRETROGRADE:FOREVECTOR).
  return AngleAxis(45, SHIP:FACING:STARVECTOR) * planeRetro.
}