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

local outsideCheck, insideCheck = -75, -65

function road_follower_init(e)
	roadFollowers[e] = {speed = 0, state = 'init'}  -- start stationary
end

local Q = require "scriptbank\\quatlib"

local sin    = math.sin
local cos    = math.cos
local rad	 = math.rad
local abs    = math.abs
local deg    = math.deg
local random = math.random

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 function TimerExpired(rf)
	return g_Time > rf.timer1
end

local function SetTimerSecs(rf, val)
	rf.timer1 = g_Time + val * 1000
end

local maxSpeed  = 5
local minSpeed  = 2

local function HandleSpeed(rf)
	if rf.subState == 'accel' and 
	   rf.speed < maxSpeed then 
		rf.speed = rf.speed + 0.02
			
	elseif rf.subState == 'decel' and 
		   rf.speed > minSpeed then 
		rf.speed = rf.speed - 0.01

	elseif rf.subState == 'brake' and 
		   rf.speed > 0 then 
		rf.speed = rf.speed - 0.4
	end
	if rf.speed < 0.01 then rf.speed = 0 end
	
	if rf.speed == 0 and 
	   rf.logicPos == 4 or 
	   rf.logicPos == 6 then 
		rf.state = 'wait' 
	end
end

local outQuat, inQuat = nil, nil

local lookAhead = 400
local lookLeft  = 250
local brakeDist = 100
local collisionList = {}

local function FollowRoad(rf, roadObj, bX, bY, bZ, vX, vZ)

 	local ox, oz = bX + vZ * outsideCheck, bZ - vX * outsideCheck 
	local ix, iz = bX + vZ * insideCheck,  bZ - vX * insideCheck

	local action = 'none'
	
	local outObj = IntersectAll(ox, bY + 10, oz, ox, bY, oz, 0)
	local inObj  = IntersectAll(ix, bY + 10, iz, ix, bY, iz, 0)
		
	if outObj == roadObj and inObj == roadObj then
		action = 'out'
		
		if rf.overRideTurn == nil and abs(rf.turnAngle) < 1 then
			-- check if road continues and randomly choose direction
			local lax, laz = rf.x + vX * lookAhead, rf.z + vZ * lookAhead
			local llx, llz = bX - vZ * lookLeft, bZ + vX * lookLeft

			if IntersectAll(lax, bY + 10, laz, lax, bY, laz, 0) == roadObj and
			   IntersectAll(llx, bY + 10, llz, llx, bY, llz, 0) == roadObj then
			    -- slightly favour straight on choice
				if random(1,3) ~= 2 then 
					rf.overRideTurn = 'yes' 
				else
					rf.overRideTurn = 'no' 
				end
			end
			
		elseif rf.overRideTurn == 'yes' then
			-- if no road ahead cancel override
			local lax, laz = rf.x + vX * lookAhead, rf.z + vZ * lookAhead

			if IntersectAll(lax, bY + 10, laz, lax, bY, laz, 0) ~= roadObj then
				rf.overRideTurn = 'no'
			end
		end
		
	elseif outObj ~= roadObj and inObj ~= roadObj then 
		rf.overRideTurn = nil
		action = 'in'
	end
	
	if rf.overRideTurn ~= 'yes' and action ~= 'none' then
		local turnAng =  0.1

		if (action == 'out' and outsideCheck < 0) or 
		   (action == 'in'  and outsideCheck > 0) then
		   
		   turnAng = -turnAng
		end
		
		if (rf.turnAngle > 0 and turnAng > 0) or
		   (rf.turnAngle < 0 and turnAng < 0) then
			turnAng = turnAng + rf.turnAngle
		end
		   
		rf.quat = Q.Mul(rf.quat, Q.FromEuler(0, rad(turnAng), 0))
		
		return turnAng
	end
	
	return 0
end

local function closerThan(x1, z1, x2, z2, dist)
	local dx, dz = x1 - x2, z1 - z2
	return dx*dx+dz*dz < dist*dist
end

local function Navigate(rf, vX, vZ)

	local bX, bZ = rf.x + vX * brakeDist, rf.z + vZ * brakeDist
	local bvX, bvZ = rf.x + vX * brakeDist * 2.5, rf.z + vZ * brakeDist * 2.5
	local bY = GetTerrainHeight(bX,bZ)

	collisionList[rf.object] = {x = bX, z = bZ}
	
	local roadObj = IntersectAll(bX, bY + 10, bZ, bX, bY, bZ, rf.object)
	local cY = rf.y + 20

	-- first check for road running out or object in the way
	if roadObj == 0 then
		rf.logicPos = 1
		rf.subState = 'brake'
	elseif IntersectAll(rf.x, rf.y + 50, rf.z, bX, bY + 10, bZ, rf.object) ~= 0 then
		rf.logicPos = 2
		rf.subState = 'brake'
	elseif IntersectAll(rf.x, cY, rf.z, bvX, cY, bvZ, rf.object) ~= 0 or
		   IntersectAll(rf.x, cY + 20, rf.z, bvX, cY + 20, bvZ, rf.object) ~= 0 then
		rf.logicPos = 3
		rf.subState = 'brake'
			
	else
		
		-- execute basic road following code
		rf.turnAngle = FollowRoad(rf, roadObj, bX, bY, bZ, vX, vZ)
		
		-- now check if we need to slow down
		local slowDist = brakeDist * rf.speed
		local sX, sZ = rf.x + vX * slowDist, rf.z + vZ * slowDist
		local sY = GetTerrainHeight(sX,sZ)
		roadObj = IntersectAll(sX, sY + 10, sZ, sX, sY, sZ, rf.object)
		
		if IntersectAll(rf.x, rf.y + 50, rf.z, sX, sY, sZ, rf.object) ~= roadObj or
		   IntersectAll(rf.x, cY, rf.z, sX, cY, sZ, rf.object) ~= 0 then
			rf.subState = 'decel'
		else
			rf.subState = 'accel'
		end

		-- now give way to other road users
		if closerThan(g_PlayerPosX, g_PlayerPosZ, 
					  collisionList[rf.object].x, 
					  collisionList[rf.object].z, brakeDist) then
			rf.subState = 'brake'
			rf.logicPos = 4
		else
			local colObj = IntersectAll(bX, cY, bZ, bX + vZ * slowDist, cY, bZ - vX * slowDist, rf.object)

			if colObj ~= 0 and collisionList[colObj] ~= nil and 
			   closerThan(bX, bZ, collisionList[colObj].x, collisionList[colObj].z, brakeDist) then
				rf.subState = 'brake'
				rf.logicPos = 5
			end
		end
		
		-- finally brake if turn angle excessive
		if abs(rf.turnAngle) > 1 then rf.subState = 'decel' end
	end
end

local function updatePosition(e, rf)
	
	local ax, ay, az = Q.ToEuler(rf.quat)
	local vX, vY, vZ = Rotate3D (0, 0, 1, ax, ay, az)


	-- follow road, give way to other cars
	Navigate(rf, vX, vZ)
	
	HandleSpeed(rf)
	
	if rf.speed ~= 0 or rf.turnAngle ~= 0 then
		if rf.turnAngle ~= 0 then ax, ay, az = Q.ToEuler(rf.quat) end
		
		rf.x, rf.y, rf.z = rf.x + vX * rf.speed, rf.y + vY * rf.speed, rf.z + vZ * rf.speed

		CollisionOff(e)
		ResetPosition(e, rf.x, rf.y, rf.z)
		ResetRotation(e, deg(ax), deg(ay), deg(az))
		CollisionOn(e)
	end
	
end

local function showDebug(e, RF)

	PromptLocal(e, RF.state .. ", " .. 
				   RF.turnAngle .. ", " ..
				  (RF.subState or 'nil') .. ", " .. 
				  (RF.logicPos or 'nil') .. ", " ..
				  (RF.overRideTurn or 'nil')
				)
end
				
function road_follower_main(e)

	local RF = roadFollowers[e]
	
	if RF == nil then return end
			
	if RF.state == 'init' then
		-- initialise parameters for this follower
		local Ent = g_Entity[e]
		
		RF.x, RF.y, RF.z = Ent.x, Ent.y, Ent.z
		RF.quat          = Q.FromEuler(rad(GetEntityAngleX(e)),
									   rad(GetEntityAngleY(e)),
									   rad(GetEntityAngleZ(e)))
		RF.state 		 = 'idle'
		RF.subState		 = 'accel'
		RF.timer1		 = math.huge
		RF.object		 = Ent.obj
		RF.turnAngle     = 0
		RF.overRideTurn	 = nil
		RF.logicPos      = 0
		
		collisionList[RF.object] = {x = RF.x, z = RF.y}

		SetTimerSecs(RF, 5)		
		
	elseif RF.state == 'idle' then
		if TimerExpired(RF) then	
			RF.state = 'drive'
		end
		
	elseif RF.state == 'drive' then		
		updatePosition(e, RF)
		
	elseif RF.state == 'wait' then
		SetTimerSecs(RF, 3)

		local Ent = g_Entity[e]		
		RF.x, RF.y, RF.z = Ent.x, Ent.y, Ent.z

		RF.state = 'idle'
	end
	
	-- showDebug(e, RF)

end