local M = {}
local lastErrorLean  = 0 -- the TAS Delta ( the value of TAS - TargetTAS) of the last sample time, 
local IntegralSum = 0 --Integral sum, 


local function init()
    electrics.values["steeringBike"] = 0
	electrics.values["steeringBike2"] = 0
	electrics.values["steeringBike3"] = 0
	lastErrorLean  = 0 -- the TAS Delta ( the value of TAS - TargetTAS) of the last sample time, 
	IntegralSum = 0 --Integral sum, 
end

local function reset()
	init()
end

local function updateGFX(dt)

	local steering = (electrics.values["steering_input"] * 1)
	local dirVector = obj:getDirectionVector()		--unit vector direction of ref->back
	local dirVectorUp = obj:getDirectionVectorUp()	--unit vector dir of ref->up
	local lean = dirVectorUp.x * -dirVector.y + dirVectorUp.y * dirVector.x 	--cross product, only care about roll, no z terms
	local targetLean = -steering*45 - (math.deg(math.asin(lean)) / (electrics.values["airspeed"] * 0.15 + 0.1))--max lean angle is 55 degrees
	
	--pitch is between -1 (down) and 1 (pointing up), with 0 point at the horizon
	--roll is also between -1 and 1, with 0 being level, and -1 & 1 being opposing sides of the horizon 
	
	local trueLean = (math.deg(math.asin(lean)) * math.max((electrics.values["airspeed"] * 0.85), 8))
	local errorLean = targetLean - (trueLean / (electrics.values["airspeed"] + 2))

	--PID gains
	local okp = 0.012
	local okp2 = 0.02
	
	local steering_output = okp*(errorLean * (electrics.values["airspeed"]))
	local steering_output2 = okp2*errorLean

	electrics.values["steeringBike"] = steering_output
	electrics.values["steeringBike2"] = steering_output2
	
	--Integral controller
	
	local steering = steering * math.min(((electrics.values["airspeed"]) / 15),1)
	local Kp = 50
    local Ki = 1
    local Kd = 5
	local KiAttenuationFactor = 1
	
	local KiAttenuation = 1 - (math.abs(steering) * KiAttenuationFactor)
	KiAttenuation = math.min(KiAttenuation,1)  --cap multi at one
	KiAttenuation = math.max(KiAttenuation,0)  --min multi is zero
	Ki = Ki * KiAttenuation -- scale integral down as we lean
	
	local PID_P = Kp * errorLean   -- calculate the P part
    -- calculate the IntegralSum
    IntegralSum = IntegralSum + errorLean * dt
    -- integral  saturation
        if math.abs(IntegralSum) > 1000 then
        if IntegralSum > 0 then
            IntegralSum = 1000 
        elseif IntegralSum < 0 then
            IntegralSum = -1000
        end
    end


    local PID_I = Ki * IntegralSum
   
    -- the D part 
        local PID_D = Kd * (errorLean - lastErrorLean) / dt
     -- P+I+D
    local Pout = PID_P + PID_I + PID_D
     -- map the PID value to Thrust position value [ -1, 1] , -1 is the max throttle, 
    local steering_output = Pout / 2000
	--print("IN:" .. steering .. " OUT:" .. steering_output)
   --update some value
    lastErrorLean = errorLean
    	
	--log('I', "controller.lua", string.format('vec3(%s,%s,%s)', V.x, V.y, V.z))
	--log('I', "controller.lua", tostring(trueLean) .. tostring(steering_output))
	
	electrics.values["steeringBike3"] = steering_output
	
	
	
	
	
	
	--Kickstand
	if electrics.values["wheelspeed"] > 1.5 then 
		electrics.values["kickstand"] = 1 else
		electrics.values["kickstand"] = 0
	end

end

M.onInit = init
M.onReset = init
M.updateGFX = updateGFX

return M