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

// 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 vel is 0. lock vel to SHIP:BODY:UP * SHIP:VELOCITY:SURFACE.
set vecDraw(V(0,0,0), vel):show to true. // debug

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 {
  // debug
  // print x + " " + y + " " + z.
  print "Vertical Speed = " + y.

  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, y). // was y

  // debug
  print "Yaw = " + mod(360 - SHIP:BEARING, 360).
  print "Pitch = " + newPitch.
  print "Roll = " + newRoll.
  print "Throt = " + newThrot.
  print "Spd = " + spd.
  
  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.".