-- LUA Script - precede every function and global member with lowercase name of script + '_main'
local whackyRacer = {}

local Q = require "scriptbank\\quatlib"

local sin  = math.sin
local cos  = math.cos 
local rad  = math.rad
local deg  = math.deg
local abs  = math.abs
local asin = math.asin
local atan = math.atan2
local modf = math.modf
local sqrt = math.sqrt

local random = math.random

function whacky_racer2_init(e)
	Include("quatlib.lua")
end

function WhackyRacerAddWheel(e)
	for k, v in pairs(whackyRacer) do
		if #v.wheels < 4 then
			v.wheels[#v.wheels + 1] = {Ent = e, quat = nil}
			return
		end
	end	
end

local function Rotate3D (x, y, z, xrot, yrot, zrot) 

	function RotatePoint2D (x, y, Ang)  -- Ang in radians
		local Sa, Ca = sin(Ang), cos(Ang)
		return x*Ca - y*Sa, x*Sa + y*Ca
	end
 
	local NX, NY, NZ = x, y, z

	-- X
	NZ, NY = RotatePoint2D (NZ, NY, -xrot)
	
	-- Y
	NX, NZ = RotatePoint2D (NX, NZ, -yrot)
	
	-- Z
	NY, NX = RotatePoint2D (NY, NX, -zrot)

	return NX, NY, NZ
end

local wheelOffsets = 
		{{x = -62.7, y = -3.7, z = 136.4},
		 {x = -62.7, y = -3.7, z =   0.64},
		 {x =  62.7, y = -3.7, z = 136.4},
		 {x =  62.7, y = -3.7, z =   0.64}
		}

local wheelHeights = {}

local function calcRollAngle(h1, h2)
	return asin((h2 - h1) / 126)
end

local function calcPitchAngle(h1, h2)
	return asin((h2 - h1) / 136)
end

local function GetWheelHeights(v, xA, yA, zA)

	local xo, yo, zo
	-- given proposed vehicle position and current angles
	-- calculate the terrain heights under the eight wheels
	local avgHeight, avgRoll, avgPitch = 0, 0, 0
	
	for i = 1, 4 do
		local wo = wheelOffsets[i]
	
		xo, yo, zo = Rotate3D(wo.x, wo.y, wo.z, xA, yA, zA)

		wheelHeights[i] = GetTerrainHeight(v.x + xo, v.z + zo) + 25
		
		avgHeight = avgHeight + wheelHeights[i]
		
		wheelHeights[i] = (v.y + yo) - wheelHeights[i] 
	end
	
	avgHeight = avgHeight / 4
	
	-- now work out vehicle angles
	local ax1 = calcRollAngle(wheelHeights[1], wheelHeights[3])
	local ax2 = calcRollAngle(wheelHeights[2], wheelHeights[4])
	
	avgRoll = (ax1 + ax2) / 2
	
	local p1 = calcPitchAngle(wheelHeights[1], wheelHeights[2])
	local p2 = calcPitchAngle(wheelHeights[3], wheelHeights[4])
	
	avgPitch = (p1 + p2) / 2

	return avgHeight + 5, avgPitch, avgRoll
end

local turnDir = {'F','R','F','R'}

local rotQuat = nil
local finQuat = nil

local function PositionWheel(num, v, xA, yA, zA)
	local wo = wheelOffsets[num]
	
	local xo, yo, zo = Rotate3D(wo.x, wo.y, wo.z, xA, yA, zA)
	
	local suspensionOffset = wheelHeights[num]
	
	if suspensionOffset > 4 then suspensionOffset = 4 end
	if suspensionOffset < -4 then suspensionOffset = -4 end
	
	ResetPosition(v.wheels[num].Ent, v.x + xo, 
	                                 v.y + yo - suspensionOffset, 
									 v.z + zo);
		
	if turnDir[num] == 'F' then
		if num == 1 then
			finQuat = Q.FromEuler(0, rad( v.turnAng), 0)
		else
			finQuat = Q.FromEuler(0, rad( -v.turnAng), rad(180))
		end
	else
		if num == 2 then
			finQuat = Q.FromEuler(0, 0, 0)
		else
			finQuat = Q.FromEuler(0, 0, rad(180))
		end
	end
	
	if num < 3 then suspensionOffset = -suspensionOffset end
	
	finQuat = Q.Mul(v.quat, finQuat)
	
	if num > 2 then
		rotQuat = Q.FromEuler(rad(-v.rotAng), 0, rad(suspensionOffset * 2))
	else
		rotQuat = Q.FromEuler(rad(v.rotAng), 0, rad(suspensionOffset * 2))
	end
	
	v.rotAng = v.rotAng - v.speed / 10

	if v.rotAng < -180 then 
		v.rotAng = v.rotAng + 360
	elseif v.rotAng > 180 then
		v.rotAng = v.rotAng - 360
	end
	
	finQuat = Q.Mul(finQuat, rotQuat)

	local xA, yA, zA = Q.ToEuler(finQuat)
	
	ResetRotation(v.wheels[num].Ent, deg(xA), deg(yA), deg(zA))
end

local function PositionWheels(v, xA, yA, zA)
	for i = 1, 4 do
		PositionWheel(i, v, xA, yA, zA)
	end
end

local function DistanceTo(x1,z1,x2,z2)
	local dX, dZ = x1 - x2, z1 - z2
	
	return sqrt(dX*dX + dZ*dZ)
end

local function PositionVehicle(e, v, xA, yA, zA)
	
	ResetPosition(e, v.x, v.y, v.z)
	ResetRotation(e, deg(xA), deg(yA), deg(zA))

end

local maxSpeed      = 20
local acceleration  = 0.03

local function PlayerControl(v)
	if v.speed < maxSpeed and g_KeyPressW == 1 then
		v.speed = v.speed + acceleration
	end
	if v.speed > 0 and g_KeyPressS == 1 then
		v.speed = v.speed - acceleration * 2
	end
	if g_KeyPressR == 1 then
		if v.turnAng < 30 then
			v.turnAng = v.turnAng + 0.3
		end
	elseif v.turnAng > 0 then
		v.turnAng = v.turnAng - 0.1
	end
	if g_KeyPressE == 1 then
		if v.turnAng > -30 then
			v.turnAng = v.turnAng - 0.3
		end
	elseif v.turnAng < 0 then
		v.turnAng = v.turnAng + 0.1
	end
end

local function MoveVehicle(v, xA, yA, zA)
	
	if v.speed > 0 then
		local mX, mY, mZ = Rotate3D(0, 0, v.speed, xA, yA, zA)
	
		v.x, v.y, v.z = v.x + mX, v.y + mY, v.z + mZ

		v.vmAng = deg(atan(mX, mZ))
	
		-- we need to turn the vehicle a small amount each
		-- frame until we are pointing in the right direction

		v.quat = Q.Mul(v.quat, Q.FromEuler(0, rad(v.turnAng / 20),0))
	end
end	

local function GotEnts(e, v)
	if #v.wheels ~= 4 then
		PromptLocal(e, "Not enough wheel entities!")
		return false
	end
	return true
end

local function AttachPlayer(v, xA, yA, zA)
		
	local xo, yo, zo = Rotate3D(0, 0, -400, xA, yA, zA)

	local y = GetTerrainHeight(v.x + xo, v.z + zo)
	
	SetFreezePosition(v.x + xo, y + 300, v.z + zo)
	SetFreezeAngle(20, v.vmAng, 0)
	TransportToFreezePosition()
end

function whacky_racer2_main(e)

	local vehicle = whackyRacer[e]
	
	if vehicle == nil then
		local Ent = g_Entity[e]
		
		whackyRacer[e] = 
			{state = 'init',
			 wheels = {},
			 x = Ent.x, y = Ent.y + 50, z = Ent.z,
			 quat = Q.FromEuler(rad(GetEntityAngleX(e)), 
								rad(GetEntityAngleY(e)), 
								rad(GetEntityAngleZ(e))),
			 vmAng = 0, rotAng = 0, turnAng = 0,
			 speed = 0
			}
		return
	end
	
	if vehicle.state == 'init' and GotEnts(e, vehicle) then

		vehicle.state = 'idle'
	end
	
	if vehicle.state == 'idle' then
		local xA, yA, zA = Q.ToEuler(vehicle.quat)
		local roll, pitch
		
		vehicle.y, pitch, roll = GetWheelHeights(vehicle, xA, yA, zA)
		
		vehicle.quat = Q.Mul(vehicle.quat, Q.FromEuler(pitch, 0, roll))
		
		xA, yA, zA = Q.ToEuler(vehicle.quat)
				
		PositionVehicle(e, vehicle, xA, yA, zA)

		PositionWheels(vehicle, xA, yA, zA)

		AttachPlayer(vehicle, xA, yA, zA)
		
		MoveVehicle(vehicle, xA, yA, zA)
		
		PlayerControl(vehicle)
			
	end
	
end