// This script assumes you have bound collective (aka deploy angle) to main throttle.
runoncepath("/debug/vecrender"). // debug

// SAS off. // debug: re-enable when adjusting heading
local pitchPID is PIDLoop(0.01, 0.001, 0.001, -45, 45).
set pitchPID:SETPOINT to 0.
local rollPID is PIDLoop(0.01, 0.001, 0.001, -70, 70).
set rollPID:SETPOINT to 0.
local collectivePID is PIDLoop(0.1, 0.1, 0.001, 0, 1).
set collectivePID:SETPOINT to 0.


// local x is 0. lock x to SHIP:FACING:FOREVECTOR * SHIP:VELOCITY:SURFACE.
// local y is 0. lock y to (SHIP:UP:FOREVECTOR * SHIP:VELOCITY:SURFACE) / (SHIP:VELOCITY:SURFACE:MAG * cos(VectorAngle(SHIP:UP:FOREVECTOR, SHIP:VELOCITY:SURFACE))).
// local z is 0. lock z to SHIP:FACING:RIGHTVECTOR * SHIP:VELOCITY:SURFACE.
// local spd is 0. lock spd to SHIP:VELOCITY:SURFACE:MAG.

local done is false.
on AG9 {
  set done to true.
}

until done {
  // local newPitch is pitchPID:Update(TIME:SECONDS, spd). // was x
  // local newRoll is rollPid:Update(TIME:SECONDS, spd). // was z
  local newThrot is collectivePID:Update(TIME:SECONDS, SHIP:VELOCITY:SURFACE:Y). // was y

  // debug
  print "Yaw = " + mod(360 - SHIP:BEARING, 360).
  print "Pitch = " + newPitch.
  print "Roll = " + newRoll.
  print "Throt = " + newThrot.
  print "Vertical Speed = " + SHIP:VELOCITY:SURFACE:Y.
  DrawVectorWithComponents(SHIP:VELOCITY:SURFACE).
  
  set SHIP:CONTROL:PILOTMAINTHROTTLE to newThrot.
  // lock STEERING to Heading(
  //   mod(360 - SHIP:BEARING, 360),
  //   newPitch,
  //   newRoll
  //   ).
  wait 0.001.
}

// Because helicopters can hover stably, once we achieve a hover we should be able to turn SAS on and call it good.
unlock STEERING.
SAS on.
print "Hover operation canceled. Returning control.".