--Written by Shaxei, please ask permission before using

local M = {}

local engine

local maxstr=0.78

local function update(jbeamData)
    electrics.values['feetdown'] = 0
    engine = powertrain.getDevice("mainEngine") or powertrain.getDevicesByType("combustionEngine")[1]
    electrics.values['walkfwd'] = 0
    electrics.values['walkbackwards'] = 0
    electrics.values['sidestand'] = 1
    lmt=0
    electrics.values['riderTilt'] = 0.5
    strtrimlow=1
    strtrimmed=1
    strtrimhigh=1
end

local roll=0
local spd=0
local wspd=0
local pspd=0
local accel=0
local str=0
local strtgt=0
local stinp=0
local feet=0
local leanmult=0
local abs=math.abs
local min=math.min
local max=math.max
local clamp = clamp
local angvr
local pitch=0
local damp=2
local rideron=0

local accelsm = newTemporalSmoothingNonLinear(5)

local leandampsm = newTemporalSmoothingNonLinear(35)

local angvrsm=0

local strtrim=1 --  		1.5	1.0		0.2						0.2		0.2
--				kP, 	kI, 		kD, 	minOutput, 	maxOutput,  	integralInCoef,   integralOutCoef, 	minIntegral, maxIntegral
local steerPID = newPIDParallel(1.5, 	1.0,		0.2, 	-1, 		1, 			0.2, 		0.2, 			-0.1, 0.1)

local function updateGFX(dt)
	roll,pitch = obj:getRollPitchYaw()
	angvr = obj:getRollPitchYawAngularVelocity()
	spd = electrics.values.airspeed
	if electrics.values['rideron']==1 then									
		if spd < 6 then			--10m/s = 22mph	
			if spd > 2.8 then  --//////////////////////////////////////////////////////////////////////////////////////// 6-10mph counterlean mode
				--				           2.5
				str=str-( clamp((str-(input.steering))*100,-2.0,2.0) *dt)

				if spd<5 and input.throttle < 0.02 then 
					electrics.values['bikesteer'] = (roll*-2.4) + (angvr*0.24) - (str*spd*0.11) 	-- come-to-a-stop mode, stand bike upright if at <2% throttle and <10mph ready to put feet down
				else --                                                    0.8
					electrics.values['bikesteer'] = (roll*-2.4) - (str*0.9) + (angvr*0.24) 		--normal 6-10mph counterlean mode
				end
				--				      1.8
				electrics.values['riderlean'] = (roll*1.2) --counterlean the rider

				feet= clamp(1-((spd-2.2)*0.4), 0, 1)

				electrics.values['lfootdown'] = feet 
				electrics.values['rfootdown'] = feet
				electrics.values['walkbackwards'] = 0
				electrics.values['sidestand'] = 0

			else   --////////////////////////////////////////////////////////////////////////////////////////////////////// <6mph feet-down walking mode.

				str=input.steering*-0.5
				electrics.values['bikesteer'] = input.steering / (1+abs(roll*10))

				if input.parkingbrake > 0.5 and spd < 0.5 and abs(roll)<0.6 then
					electrics.values['sidestand'] = 1
				else
					electrics.values['sidestand'] = 0
				end

				if spd < 0.3 and input.brake > 0.95 and input.clutch < 0.1 and abs(pitch) < 0.1 and input.parkingbrake < 0.5 then --lean onto one leg
					electrics.values['rfootdown'] = 0.8 - (roll*0.1)
					if roll < -0.05 then
						electrics.values['lfootdown'] =	0
						electrics.values['riderlean'] = roll*2.2
					else
						electrics.values['lfootdown'] =	1.6
						electrics.values['riderlean'] = 0.6
					end
					electrics.values['walkbackwards'] = 0
				else		--else stay upright
					if abs(roll)<0.1 then
						electrics.values['lfootdown'] = 1.1
						electrics.values['rfootdown'] =	1.1
						electrics.values['riderlean'] = roll*1.8
						electrics.values['ridertuck'] = ((electrics.values['riderTilt']-0.5))*0.5
					elseif abs(roll)<0.6 then
						electrics.values['lfootdown'] = 1+(roll*3)+(angvr*-2)
						electrics.values['rfootdown'] = 1+(roll*-3)+(angvr*2)
						electrics.values['riderlean'] = roll*1.8
						electrics.values['ridertuck'] = ((electrics.values['riderTilt']-0.5))*0.5
					else
						electrics.values['lfootdown'] = 0
						electrics.values['rfootdown'] = 0
						electrics.values['riderlean'] = 0
						electrics.values['ridertuck'] = 1
					end
					
					if electrics.values.gear == 0 then --if in upright mode and in neutral use the clutch to move backwards
						electrics.values['walkbackwards'] = input.clutch - (spd*0.6)
					else
						electrics.values['walkbackwards'] = 0
					end
				end
			end
			leanmult=-0.2
			electrics.values['rightknee'] = 0
			electrics.values['leftknee'] = 0
			
			electrics.values['riderheadtilt'] = str*0.2

		else  --////////////////////////////////////////////////////////////////////////////////////////////////////////////// >10mph MEDIUM-HIGH SPEED SELF BALANCING

			--if abs(str)>0.8 then
			--	str=str-( clamp((str-(input.steering))*10,-1,1) *dt)
			--else
				str=str-( clamp((str-(input.steering))*10,-damp,damp) *dt)
			--end
			electrics.values['rfootdown'] = 0							
			electrics.values['lfootdown'] = 0
			electrics.values['sidestand'] = 0
			electrics.values['riderheadtilt'] = roll*-2

			if spd < 14 then --  <24mph        RIDER LEAN CONTROL
				electrics.values['riderlean'] = ( str * 0.2 * min(spd-4,8)) 
			else             --  >24mph
				electrics.values['riderlean'] = ( str * 1.1) 
			end

			electrics.values['bikesteer'] = steerPID:get(roll, str*-maxstr*clamp(spd*0.073,0.54,0.9), dt)


			electrics.values['rightknee'] = max(str + (min(roll+0.4,0)*1.5),-0.2)
			electrics.values['leftknee'] = min(str + (max(roll-0.4,0)*1.5),0.2)

			electrics.values['walkfwd'] = 0
			electrics.values['walkbackwards'] = 0

			--electrics.values['ridertuck'] = max( ((electrics.values['riderTilt']-0.5) + min(spd/60,0.5))*0.5, abs(str*0.5))


			electrics.values['ridertuck'] = ((electrics.values['riderTilt']-0.5) + min(spd/60,0.5))*0.5
			
		end	

		electrics.values['riderpresent'] = 0

		if electrics.values.modeswitchv == 1 then --mode switching
			electrics.values.modeswitchv = 0

			maxstr = maxstr + 0.02

			if maxstr >= 0.85 then
				maxstr = 0.70
			end
			if maxstr < 0.75 then
				guihooks.message("Maximum lean set: " .. (maxstr) .. " (keyboard)")
				damp=1.5
			elseif maxstr < 0.81 then
				guihooks.message("Maximum lean set: " .. (maxstr) .. " (controller)")
				damp=2
			else
				guihooks.message("Maximum lean set: " .. (maxstr) .. " (mouse steer/wheel)")
				damp=2.5
			end
			
		end

		if spd > 2.5 and spd < 4.0 and electrics.values.gear == 1 and input.throttle > 0.02 and engine.outputAV1 > 100 and input.clutch < 0.2 then --experimental helper prevent the bike dropping below 8mph when doing slow maneuvers
			electrics.values['walkfwd'] = 1
		else
			electrics.values['walkfwd'] = 0
		end

	elseif electrics.values['rideron']==0 then	
		electrics.values['riderpresent'] = 1
		if input.parkingbrake > 0.5 and abs(roll)<0.6 then --To make picking the bike up with nodegrabbed in walking mode possible
			electrics.values['sidestand'] = 1
			electrics.values['rfootdown'] = 2
			electrics.values['ridertuck'] = 2
		else
			electrics.values['sidestand'] = 0
			electrics.values['rfootdown'] = 0
			electrics.values['ridertuck'] = 0.6
		end
		electrics.values['lfootdown'] = 0 --1.1
		electrics.values['riderlean'] = -0.2
		electrics.values['bikesteer'] = -1
		electrics.values['rightknee'] = 0
		electrics.values['leftknee'] = 0
		electrics.values['ridertuck'] = 0.6
		electrics.values['walkfwd'] = 0
		electrics.values['walkbackwards'] = 0
	else --multiplayer other player mode
		electrics.values['riderpresent'] = 0
		if spd<4 then
			if abs(roll)<0.1 then
				electrics.values['lfootdown'] = 1.1
				electrics.values['rfootdown'] =	1.1
				electrics.values['riderlean'] = roll*1.8
			else
				electrics.values['lfootdown'] = 1+(roll*3)
				electrics.values['rfootdown'] = 1+(roll*-3)
				electrics.values['riderlean'] = roll*1.8
			end
		else
			electrics.values['lfootdown'] = 0
			electrics.values['rfootdown'] = 0
			electrics.values['riderlean'] = roll*-1.3
		end
		electrics.values['bikesteer'] = 0
		electrics.values['rightknee'] = 0
		electrics.values['leftknee'] = 0
		electrics.values['ridertuck'] = 0
		electrics.values['walkfwd'] = 0
		electrics.values['walkbackwards'] = 0
	end

	--if pitch > 0.3 then 
	--	controller.mainController.setEngineIgnition(false)
	--else
	--	controller.mainController.setEngineIgnition(true)
	--end

	if abs(roll) > 0.95 then --if we've rolled past ~70 degrees then break the rider away and stop the engine
    		if spd > 10 and electrics.values['rideron']==1 then
			beamstate.breakBreakGroup("riderfall")
		end
		controller.mainController.setEngineIgnition(false)
		--electrics.values['riderpresent'] = 1
	end
end

-- public interface
M.init = update
M.initSecondStage = update
M.updateData = update
M.updateGFX = updateGFX

return M
