local M = {}

local wheelingMeasure = 0


local MAX_LEAN = 35
local MAX_SPEED = 400
local MAX_WHEELING = 35
local leanPID

local wheeliePID



local function brokeBeamsCheck()
    
	for _, b in pairs(v.data.beams) do
		if obj:beamIsBroken(b.cid) and b.breakGroup == "fall" then return true
		end
	end
	return false
	
end


local function init(jbeamData)
	electrics.values["currentWheelingDistance"] = 0
	electrics.values["balanceForceLR"] = 0
	electrics.values["balanceForceRL"] = 0
	electrics.values["kickstand"] = 0
	electrics.values["steering_adjusted"] = 0
	electrics.values["wheelie"] = 0
	electrics.values["toggle_wheelie"] = 0	
	electrics.values["wheelerForce"] = 0
	electrics.values["stopperForce"] = 0
	electrics.values["downPressure"] = 0	
		

	lastPosition = 0
	
	--function newPIDParallel(kP, 			kI, 		kD, 		minOutput, 	maxOutput, integralInCoef, 			integralOutCoef, 		minIntegral, 		maxIntegral)
	  leanPID = newPIDParallel(0.015, 		0.0,		0.0030, 	-1.0, 		1.0, 			1.0, 				1.0, 					-1.0, 				1.0)
	  wheeliePID = newPIDParallel(0.015, 	0.00100,	0.005, 		-1.0, 		1.0, 			1.0, 				1.0, 					-1.0, 				1.0)
	
end

local function reset()
	print("reset!")
	init()
end



local function getSpeedKmh()
	return wheels.wheels[0].wheelSpeed*3.6
end


local function limitMAXSpeed(speed)
	if speed > MAX_SPEED then
		electrics.values["throttle"] = 0
	end
end

local function manageAutoWheeling(dt)

	local pitchTarget = MAX_WHEELING
	local dirVectorUp = obj:getDirectionVectorUp()	--unit vector dir of ref->up
	local dirVectorRight = obj:getDirectionVectorRight()	--unit vector dir of ref->right
	local pitch = -dirVectorUp.y * dirVectorRight.x + dirVectorUp.x * dirVectorRight.y 	--cross product, only care about pitch, no z terms
	local truePitch = math.deg(math.asin(pitch))
	
	electrics.values["wheelerForce"] = wheeliePID:get(truePitch, pitchTarget, dt)

end

local function manageWheeling(trueLean, dt)

	if math.abs(trueLean) < (MAX_LEAN + 10) then 
		if electrics.values["wheelie"] == 1 then
			electrics.values["wheelerForce"] = 0.3
			electrics.values["stopperForce"] = 0
		else
			if electrics.values["toggle_wheelie"] == 1 then
				manageAutoWheeling(dt)
			else
				electrics.values["wheelerForce"] = 0
				if electrics.values["stoppie"] == 1 then
					electrics.values["stopperForce"] = 1
				else
					electrics.values["stopperForce"] = 0
				end
			end
		end
	else
		electrics.values["wheelerForce"] = 0
		electrics.values["stopperForce"] = 0
	end
	
end
	
local function manageWheelingMeasure(dt)

	if electrics.values["wheeling_count"] == 1 then
		if wheelingMeasure == 0 then
			wheelingMeasure = 1 
			gui.message("Start measuring wheeling ...")
		end
	
		local dirVectorUp = obj:getDirectionVectorUp()	--unit vector dir of ref->up
		local dirVectorRight = obj:getDirectionVectorRight()	--unit vector dir of ref->left
		local wheeling = -dirVectorUp.y * dirVectorRight.x + dirVectorUp.x * dirVectorRight.y 	--cross product, only care about pitch, no x terms
		local trueWheeling = math.deg(math.asin(wheeling))
	
	
		if trueWheeling > 10 then
			local newPosition = obj:getPosition()
			local distance = math.sqrt( (newPosition.x - lastPosition.x) * (newPosition.x - lastPosition.x) + (newPosition.y - lastPosition.y) * (newPosition.y - lastPosition.y))
			local currentWheelingDistance = electrics.values["currentWheelingDistance"] + distance
			electrics.values["currentWheelingDistance"] = currentWheelingDistance
			lastPosition = obj:getPosition()
		else
			lastPosition = obj:getPosition()
		end
	end
	
	if electrics.values["wheeling_count"] == 0 then
		if wheelingMeasure == 1 then
			wheelingMeasure = 0 
			gui.message("Stop measuring wheeling ...")
		end
	end		
		
end

local function computeTrueLean()

	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 trueLean = math.deg(math.asin(lean))
	
	return trueLean

end


local function manageDownPressure(trueLean, speed)

	if 	math.abs(trueLean) < (MAX_LEAN + 10) and 
		(wheels.wheels[1].contactMaterialID1>0 and wheels.wheels[0].contactMaterialID1>0) and
		electrics.values["wheelie"] == 0 and
		electrics.values["toggle_wheelie"] == 0
		then
		electrics.values["downPressure"] = 1
	else
		electrics.values["downPressure"] = 0
	end

end

local function manageLeanHighSpeed(speed, dt)
end

local function manageLeanLowSpeed(trueLean, speed, dt)

	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 = 0
	local steering = 0
	local balanceForce = 0
	local Kp = 0
	local Ki = 0
	local Kd = 0
	
	
	if math.abs(speed) < 1 then speed = 0 end
	
	--reduce steering input in function of speed
	if (speed >= 0 and speed < 50) or speed < 0 then
		steering = -electrics.values["steering_input"]
	else
		steering = -electrics.values["steering_input"]*(50/speed)
	end	
	
	electrics.values["steering_adjusted"] = -steering
	
	
	if math.abs(trueLean) < MAX_LEAN + 10 and wheels.wheels[0].contactMaterialID1>0 then
		
		if speed == 0 and electrics.values["kickstand"] == 0 then
			targetLean = 10
		else
			targetLean = (math.min(1, speed/20))*(steering)*MAX_LEAN*(electrics.values.reverse == 1 and -1 or 1)
		end

		Kp = 0.015*math.max(0.3, (1 - (speed/140)))
		

		--local Ki = 0.5*(steering ~=0 and 1 or 0.1/0.5) --math.min(1, speed/100)
		--local Ki = speed > 50 and (0.4*math.min(1, math.max(0.1/0.4, speed/130))*(steering ~=0 and 1 or 0.1/(0.4*math.min(1, math.max(0.1/0.4, speed/130))))) or 0.1
		--local Ki = 0.5*math.min(1, math.abs(targetLean-trueLean)/10)
		
		if speed == 0 then
			Ki = 0.1
		else
			if (speed > 0 and speed < 50) or speed < 0 then
				Ki = 0.1*math.min(1, speed/50)*(steering ~=0 and 1 or 0.1/(0.1*math.min(1, speed/50)))
			else if speed >= 50 and speed < 100 then
					Ki = 0.3*math.min(1, speed/100)*(steering ~=0 and 1 or 0.1/(0.3*math.min(1, speed/100)))
				else -- >=100
					--Ki = 0.5*(steering ~=0 and 1 or 0.1/0.6)
					Ki = 0.5*(math.abs(steering) >=0 and math.max(0.1/0.5, math.abs(steering)) or 0.1/0.5)
				end
			end
		end
				
		--local Kd = 0.0015*math.max(1, (2.1)*speed/90)
		Kd = 0.0035*math.max(0.4, (1 - (speed/150)))
		
	  --leanPID:setConfig(kP, 	kI, 	kD, 	minOutput, 	maxOutput, 		integralInCoef, 	integralOutCoef, 		minIntegral, 		maxIntegral)
		leanPID:setConfig(Kp, 	Ki,		Kd, 	-1.0, 		1.0, 			1.0, 				1.0, 					-1.0, 				1.0)
		
		balanceForce = leanPID:get(trueLean, targetLean, dt)*(steering ~=0 and math.max(1, speed/100) or 1)
		
		electrics.values["balanceForceRL"] = balanceForce < 0 and -balanceForce or 0
		electrics.values["balanceForceLR"] = balanceForce > 0 and balanceForce or 0
		
		
		--[[
		dump(string.format("sp: %0.3f - TrL: %0.3f - TgL: %0.3f - bF: %0.3f", 
			speed,
			trueLean, 
			targetLean, 
			balanceForce))
		]]--

	else

		electrics.values["balanceForceRL"] =  0
		electrics.values["balanceForceLR"] =  0

	end
	
	

	
	--dump(string.format("sp:%0.4f - kp: %0.4f - ki: %0.4f - kd: %0.4f - bF: %0.4f - st: %0.4f", speed, Kp, Ki, Kd, balanceForce, electrics.values["steering_adjusted"]))
	
end



local function updateGFX(dt)

	local speed = getSpeedKmh()
	
	local trueLean = computeTrueLean()
	
	----------
	limitMAXSpeed(speed)
	----------
	
	-------------
	manageWheelingMeasure(dt)
	-------------
	
	-------------
	manageWheeling(trueLean, dt)
	-------------
		
	--------------
	manageLeanLowSpeed(trueLean, speed, dt)
	--------------
	
	--------------
	manageDownPressure(trueLean, speed)
	--------------
	
	-------------
	--Kickstand
	if speed > 3 or electrics.values.kickstand == 1 then 
		electrics.values["kickstand_retract"] = 1 else
		electrics.values["kickstand_retract"] = 0
	end
	
	--------------
	--manage throttle if rider fell
	if brokeBeamsCheck() then
		electrics.values["throttle"] = 0
		electrics.values["steering_adjusted"] = 0
		electrics.values["balanceForceRL"] =  0
		electrics.values["balanceForceLR"] =  0
		electrics.values["wheelerForce"] = 0
		electrics.values["stopperForce"] = 0
		electrics.values["downPressure"] = 0
	end
	
	--------------
	--Frame holder
	-- for _, beam in pairs(v.data.beams) do if beam.breakgroup == "frame_support_holder" then dump(beam.cid) end end
	if speed > 3 or electrics.values.frame_holder_retract == 1 then 
		electrics.values["frm_hld_retract"] = 1 
	else
		electrics.values["frm_hld_retract"] = 0 
	end
	--------------
	
	
end
	

M.init      = init
M.updateGFX = updateGFX

return M