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["steeringBike2"] = 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"]
	local targetLean = -steering*40 --max lean angle is 55 degrees
	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
	
	--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)) --What I think is the desired border at which the bike is able to lean
	local errorLean = targetLean - trueLean --What I think is the value at which the steering is out of the true lean border

	--PID gains
	--local okp = 0.012
	local okp2 = 0.023 --Steering Rate
	
	--local steering_output = okp*errorLean
	local steering_output2 = okp2*errorLean

	electrics.values["steeringBike2"] = steering_output2
	
	--Integral controller (Main PID stuff)
	
	local steering = steering * math.min((math.max(electrics.values["airspeed"],0) / 15),1) --Max/min value
	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))
	
	
	
	
	
	
	
	--Kickstand
	if electrics.values["airspeed"] > 7 then 
		electrics.values["r6_kickstand"] = 1 else
		electrics.values["r6_kickstand"] = 0
	end

end

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

return M