-- 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_racer_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(x, y, z, 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(x + xo, z + zo) + 24
		
		avgHeight = avgHeight + wheelHeights[i]
		
		wheelHeights[i] = (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 + 3, 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)
	CollisionOff(e)
	
	local xA, yA, zA = Q.ToEuler(v.quat)
	
	ResetPosition(e, v.x, v.y, v.z)
	ResetRotation(e, deg(xA), deg(yA), deg(zA))
	CollisionOn(e)
end

local waypoints = {}

local function FindWaypoints(e, v)
	-- find initial waypoint path to follow
	local pathIndex, pointIndex = -1, -1;
	local pointClosest = math.huge;
	
	local vDX, vDZ, vDist

	for path = 1, AIGetTotalPaths() do
		for point = 1, AIGetPathCountPoints(path) do

			vDX = v.x - AIPathGetPointX(path, point)
			vDZ = v.z - AIPathGetPointZ(path, point)

			vDist = vDX*vDX + vDZ*vDZ
			
			if vDist < pointClosest then
				pointClosest = vDist
				pathIndex = path
				pointIndex = point
			end
		end
	end
	
	-- only follow if waypoint within 1000 units of vehicle
	if pointClosest > 1000 * 1000 then pathIndex = -1 end
	
	if pathIndex ~= -1 then
		-- now we have found the closest path copy it
		waypoints[e] = {}
		
		local wp = waypoints[e]
		local rl = AIGetPathCountPoints(pathIndex)
		for i = 1, rl do
			wp[i] = {x = AIPathGetPointX(pathIndex, i), 
					 z = AIPathGetPointZ(pathIndex, i)};
		end
		v.driveTo = {x = wp[pointIndex].x, z = wp[pointIndex].z}
		v.wpt     = pointIndex
		
		if pointIndex == rl then v.rteDir = -1 end
	end
end

local function FollowWaypoints(e, v)
	local cw = waypoints[e][v.wpt]
	local dX, dZ = cw.x - v.x, cw.z - v.z
	-- if arriving at next waypoint
	if dX*dX + dZ*dZ < 50*50*v.speed then
		if v.rteDir == 1 then
			if v.wpt < #waypoints[e] then
				v.wpt = v.wpt + 1
				cw = waypoints[e][v.wpt]
				v.driveTo = {x = cw.x, z = cw.z}
			else
				v.wpt = 1
				cw = waypoints[e][v.wpt]
				v.driveTo = {x = cw.x, z = cw.z}
			end
		else
			if v.wpt > 1 then
				v.wpt = v.wpt -1
				cw = waypoints[e][v.wpt]
				v.driveTo = {x = cw.x, z = cw.z}
			else
				v.wpt = #waypoints[e]
				cw = waypoints[e][v.wpt]
				v.driveTo = {x = cw.x, z = cw.z}
			end			
		end
	end
end

local speedTarget      = 8
local acceleration     = 0.03

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

	if v.driveTo.x ~= v.x and v.driveTo.z ~= v.z then

		if v.speed < speedTarget then
			v.speed = v.speed + acceleration
		end
	else
		v.speed = 0
		return
	end
	
	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

	local vtX, vtZ = v.driveTo.x - v.x, v.driveTo.z - v.z
	local vtAng = deg(atan(vtX, vtZ))
	
	v.vmAng = deg(atan(mX, mZ))
	
	local turnAng = vtAng - v.vmAng
	
	if turnAng > 180 then 
		turnAng = turnAng - 360
	elseif turnAng < -180 then 
		turnAng = turnAng + 360
	end

	if turnAng >  30 then 
		turnAng =  30
		if v.speed > 1 then
			v.speed = v.speed - acceleration * 1.5
		end
	elseif turnAng < -30 then 
		turnAng = -30
		if v.speed > 1 then
			v.speed = v.speed - acceleration * 1.5
		end
	end
	
	v.turnAng = turnAng
	
	-- we need to turn the vehicle a small amount each
	-- frame until we are pointing in the right direction

	if 	turnAng > v.speed / 6 then 
		v.quat = Q.Mul(v.quat, Q.FromEuler(0, rad(v.speed / 6),0))
	elseif v.turnAng < -v.speed / 6 then 
		v.quat = Q.Mul(v.quat, Q.FromEuler(0, rad(-v.speed / 6),0))
	end

end

local function RandomPos(x, z, dist)
	x = x or g_PlayerPosX
	z = z or g_PlayerPosZ
	dist = dist or 100
		
	local angle = math.random() * 2 * math.pi

	return x + sin(angle) * dist, z + cos(angle) * dist
end		

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

function whacky_racer_main(e)

	local vehicle = whackyRacer[e]
	
	if vehicle == nil then
		local Ent = g_Entity[e]
		
		whackyRacer[e] = 
			{state = 'init',
			 wheels = {}, flames = {}, scanners = {},
			 base = nil, gun = nil, cap = nil,
			 x = Ent.x, y = Ent.y + 50, z = Ent.z,
			 quat = Q.FromEuler(rad(GetEntityAngleX(e)), 
								rad(GetEntityAngleY(e)), 
								rad(GetEntityAngleZ(e))),
			 driveTo = {x = Ent.x, z = Ent.z}, vmAng = 0,
			 scnrAng = 0, scnrCW = true,
			 gunAng = 0, gunReqAng = 0,
			 gunPitch = 0, gunReqPitch = 0,
			 wpt = nil, rteDir = 1,
			 rotAng = 0, turnAng = 0,
			 timer = math.huge, timer2 = 0,
			 speed = 0
			}
		return
	end
	
	if vehicle.state == 'init' and GotEnts(e, vehicle) then
	
		FindWaypoints(e, vehicle)
		
		vehicle.state = 'idle'
		vehicle.timer = g_Time + 5000
	end
	
	if vehicle.state == 'idle' then
		local xA, yA, zA = Q.ToEuler(vehicle.quat)
		local roll, pitch
		vehicle.y, pitch, roll = GetWheelHeights(vehicle.x, vehicle.y, vehicle.z, 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)
				
		if g_Time > vehicle.timer then
			local PX, PY, PZ = g_PlayerPosX, g_PlayerPosY, g_PlayerPosZ
		
			MoveVehicle(vehicle, xA, yA, zA)
			
			if waypoints[e] ~= nil and #waypoints[e] > 0 then
				FollowWaypoints(e, vehicle)
			else
				-- wander mode
				if g_Time > vehicle.timer2 then
					vehicle.driveTo.x,vehicle.driveTo.z = 
					    RandomPos(g_PlayerPosX, g_PlayerPosZ, 3000)
					vehicle.timer2 = g_Time + 12000
				end
			end
		end
	end
	
end
