local pid = require('vehicles/A380-800/pid/pid')
-- This --Source Code Form is subject to the terms of the bCDDL, v. 1.1.
-- If a copy of the bCDDL was not distributed with this
-- file, You can obtain one at http://beamng.com/bCDDL-1.1.txt

local M = {}
--Mandatory controller parameters
M.type = "auxiliary"
M.relevantDevice = nil

M.enabled = false
M.holdAltitude = false
M.holdAltitudeValue = 0

M.holdVS = false
M.holdVSValue = 0

local frameRate = 60
local fixedDt = 16.66
local framesBeforeUpdate = 33

local abs = math.abs
local min = math.min
local max = math.max

local elevatorSmoothing = newExponentialSmoothing(10)

local vsPid = pid:new()
vsPid.kp = 0.025
vsPid.ki = 100
vsPid.kd = 10

local function updateHoldVS(dt)
  vsPid.target = M.holdVSValue
  local output = vsPid:get(electrics.values.vsi_feetPerMinute, dt)
  electrics.values.elevator = elevatorSmoothing:get(output) 
end

local frameCount = 0
local function update()
  frameCount = frameCount + 1
  if frameCount <= framesBeforeUpdate then return end
  
  --altitude
  if M.holdAltitude then
    M.holdVS = true
    
    local alt = M.holdAltitudeValue
    
    --set elevator 
    local altDiff = electrics.values.altitudeMT - alt
    M.holdVSValue = math.max(math.min(altDiff / 100, 1000), -1000)
  end
  
  --hold VS
  if M.holdVS then
    updateHoldVS(fixedDt)
    do return end 
    
    local currentVS = electrics.values.vsi_feetPerMinute
    local difference = M.holdVSValue - currentVS
    
    
    local surfaceFactor = 0
    print(difference)
    if math.abs(difference) < M.holdVSValue then
        --minor adjustment
        surfaceFactor = difference / M.holdVSValue
    else
        if difference > 0 then surfaceFactor = 0.5 end
        if difference < 0 then surfaceFactor = -0.5 end
    end
    electrics.values.elevator = elevatorSmoothing:get(surfaceFactor * 1)
  end
  
  frameCount = 0
end

local function init(jbeamData)
  frameRate = jbeamData.frameRate or 120
  fixedDt = 1000 / frameRate
  framesBeforeUpdate = math.floor(fixedDt * 2)
end

-- public interface
M.init = init
M.updateGFX = updateGFX
M.update = update
return M



