-- Author: stefan750

local M = {}

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

local accel = 3                          -- Acceleration while airborne
local brake = 5                          -- Braking strength while airborne
local sideDamping = 5                    -- Resistance to sideways movement while airborne depending on speed
local maxSideDamping = 5                 -- Maximum resistance to sideways movement while airborne
local airYaw = 5                         -- Yaw control strength while airborne
local yawDamping = 5                     -- Yaw resistance while airborne
local pitchPID = newPIDParallel(2, 0, 2) -- Pitch stabilization PID
local rollPID = newPIDParallel(2, 0, 2)  -- Roll stabilization PID
local autoStraight = 0.05                -- How strongly the vehicle should try to point in the move direction

local fallDamping = 70
local fallDampingHeightMul = 0.05

local function onInit()
	enablePhysicsStepHook()
end

local function onReset()
	pitchPID:reset()
	rollPID:reset()
end

local function onPhysicsStep(dt)
	--obj.debugDrawProxy:drawSphere(0.5, obj:getCenterPosition(), color(255,0,0,255))

	-- If any wheel is on the ground, exit the function
	local lwheels = wheels.wheels
	local wheelCount = tableSizeC(lwheels) - 1
	for i = 0, wheelCount - 1 do
		local wd = lwheels[i]
		if not wd.isBroken and wd.downForceRaw > 0 then
			return
		end
	end

	-- Vehicle data
	local dirFwd = obj:getDirectionVector()
	local dirUp = obj:getDirectionVectorUp()

	local vehRot = quatFromDir(-dirFwd, dirUp)
	local vehRotInv = vehRot:inversed()

	local vehPos = obj:getCenterPosition()
	local cog = vehPos - obj:getPosition()

	local vehAngVelLocal = vec3(obj:getPitchAngularVelocity(), obj:getRollAngularVelocity(), obj:getYawAngularVelocity())
	local vehAngVel = vehAngVelLocal:rotated(vehRot)

	local vehVel = obj:getVelocity() + cog:cross(vehAngVel)
	local vehVelLocal = vehVel:rotated(vehRotInv)

	local roll, pitch, yaw = obj:getRollPitchYaw()

	-- Acceleration, braking, and side damping
	local throttle = (electrics.values.gearIndex or 0) >= 0 and (electrics.values.throttle or 0) or -(electrics.values.throttle or 0)
	local fwdImpulse = throttle*accel + (electrics.values.brake or 0)*fsign(vehVelLocal.y)*brake
	local sideImpulse = clamp(-vehVelLocal.x * sideDamping, -maxSideDamping, maxSideDamping)

	-- Fall damping
	local velLength = vehVel:length()
	local velNorm = vehVel:normalized()
	local fallDampForce = 0
	for i = 0, wheelCount - 1 do
		local wd = lwheels[i]
		if not wd.isBroken then
			local rayStart = vehPos - cog + obj:getNodePosition(wd.node1) + vec3(0, 0, -wd.radius+0.05)
			local rayLen = velLength*fallDampingHeightMul
			local height = obj:castRayStatic(rayStart, velNorm, rayLen)
			fallDampForce = max(fallDampForce, (rayLen - height)/guardZero(rayLen))

			--obj.debugDrawProxy:drawLine(rayStart, rayStart + velNorm*rayLen, color(0, 0, 255, 255))
		end
	end
	--fallDampForce = fallDampForce / wheelCount

	-- Calculate final impulse
	local impulse = (vec3(sideImpulse, -fwdImpulse, 0)):rotated(vehRot) + vec3(0, 0, max(fallDampForce*fallDamping*-vehVel.z, 0))
	
	-- Stabilize pitch and roll
	local pitchTorque = pitchPID:get(pitch, 0, dt)
	local rollTorque = rollPID:get(-roll, 0, dt)

	-- Yaw air steering
	local yawInputTorque = -input.steering*fsign(vehVelLocal.y)*airYaw
	local yawDampingTorque = -vehAngVelLocal.z*yawDamping
	local autoStraightTorque = vehVelLocal.x*fsign(vehVelLocal.y)*autoStraight
	
	-- Calculate final torque
	local torque = vec3(pitchTorque, rollTorque, yawInputTorque + yawDampingTorque + autoStraightTorque):rotated(vehRot)

	--thrusters.applyAccel(impulse + cog:cross(-torque), dt, v.data.refNodes and v.data.refNodes[0].ref or 0, -torque)
	obj:applyClusterLinearAngularAccel(v.data.refNodes and v.data.refNodes[0].ref or 0, impulse + cog:cross(-torque), -torque)
end

-- public interface
M.onInit            = onInit
M.onExtensionLoaded = onInit
M.onReset           = onReset
M.onPhysicsStep     = onPhysicsStep

return M
