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

function minecart_init(e)
	mineCarts[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 atan	 = math.atan2

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 FW_Offsetx, FW_Offsetz = -11.67,  14
local RW_Offsetx, RW_Offsetz = -11.67, -14

local function DetectObject (x, y, z, xo, zo, ax, ay, az)
	local xo, yo, zo = Rotate3D (xo, 0, zo, ax, ay, az)
	local nx, nz = x + xo, z + zo
	return IntersectAll(nx, y + 3, nz, nx, y -0.5, nz, 0)
end

local function PositionOnRail(rf)
	-- So the principle here is that we detect the object that
	-- the front wheel is on, then check that the back wheel is 
	-- on the same object.
	local ax, ay, az = Q.ToEuler(rf.quat)

	local FW_Obj = DetectObject(rf.x, rf.y, rf.z, FW_Offsetx, FW_Offsetz, ax, ay, az)
	
	local RW_Obj = DetectObject(rf.x, rf.y, rf.z, RW_Offsetx, RW_Offsetz, ax, ay, az)
	
	if FW_Obj == RW_Obj then
	
		local reposSuccess, attempts = false, 8
		local FWl_Obj, FWr_Obj, RWl_Obj, RWr_Obj
		
		local FWOx, RWOx = FW_Offsetx, RW_Offsetx
		
		while not reposSuccess and attempts > 0 do
		
			FWl_Obj = DetectObject(rf.x, rf.y, rf.z, FWOx - 0.25, FW_Offsetz, ax, ay, az)
			FWr_Obj = DetectObject(rf.x, rf.y, rf.z, FWOx + 0.25, FW_Offsetz, ax, ay, az)
		
			-- Then check if 1/4 of a unit each side of the front wheel 
			-- is also on the same object.
			-- If so then a similar check for the back wheel.
	
			RWl_Obj = DetectObject(rf.x, rf.y, rf.z, RWOx - 0.25, RW_Offsetz, ax, ay, az)
			RWr_Obj = DetectObject(rf.x, rf.y, rf.z, RWOx + 0.25, RW_Offsetz, ax, ay, az)
		
			if FWl_Obj == FW_Obj then
				if FWr_Obj == FW_Obj then
					if RWl_Obj == RW_Obj then
						if RWr_Obj == RW_Obj then
							reposSuccess = true
						else
							RWOx = RWOx - 0.125
						end
					else
						-- bump right a bit
						RWOx = RWOx + 0.125
					end
				else
					-- bump left a bit
					FWOx = FWOx - 0.125
				end
			else
				-- bump right a bit
				FWOx = FWOx + 0.125
			end
			
			if not reposSuccess then attempts = attempts - 1 end
		end

		-- at this point FWOx and RWOx indicate the front and rear 
		-- wheel points to centre the cart on track
		-- the difference between them represents the angle we need
		-- to tweak by 
		local tweakAng = atan((FWOx - RWOx), (FW_Offsetz - RW_Offsetz))

		rf.quat = Q.Mul(rf.quat, Q.FromEuler(0, tweakAng, 0))
	
--		Prompt(FW_Obj  .. ", " .. 
--			   RW_Obj  .. ", " .. 
--			   FWOx .. ", " .. 
--			   RWOx .. ", " .. 
--			   8 - attempts
--			   )
	end
end

local maxSpeed  = 5
local minSpeed  = 2

local function Navigate(rf, ax, ay, az)

	local vX, vY, vZ = Rotate3D (0, 0, rf.speed, ax, ay, az)
	
	-- get object under front wheel (assume it is the track!)
	local railObj = DetectObject(rf.x, rf.y, rf.z, FW_Offsetx, FW_Offsetz, ax, ay, az)
	
	if railObj == 0 then
		rf.logicPos = 1	
 		-- try to the left
		railObj = DetectObject(rf.x, rf.y, rf.z, FW_Offsetx - 1, FW_Offsetz, ax, ay, az)
		if railObj ~= 0 then
			rf.quat = Q.Mul(rf.quat, Q.FromEuler(0, rad(-1), 0))
		else
			rf.quat = Q.Mul(rf.quat, Q.FromEuler(0, rad(1), 0))
		end
	else
		-- now check ahead to the next position
		local nX, nZ = rf.x + vX, rf.z + vZ
		
		newObj = DetectObject(nX, rf.y, nZ, FW_Offsetx, FW_Offsetz, ax, ay, az)
		
		if newObj ~= railObj then
			-- track has either changed, ended or curved
			if newObj == 0 then
				-- track has either ended or curved
				-- look to left
				newObj = DetectObject(nX, rf.y, nZ, FW_Offsetx - 1, FW_Offsetz, ax, ay, az)
				if newObj == 0 then
					-- look to right
					newObj = DetectObject(nX, rf.y, nZ, FW_Offsetx + 1, FW_Offsetz, ax, ay, az)
					if newObj == 0 then
						rf.logicPos = 2
						rf.subState = 'brake'
					else
						-- rotate right
						rf.x, rf.z = rf.x + vX, rf.z + vZ
						rf.quat = Q.Mul(rf.quat, Q.FromEuler(0, rad(1), 0))
					end
				else
					-- rotate left
					rf.x, rf.z = rf.x + vX, rf.z + vZ
					rf.quat = Q.Mul(rf.quat, Q.FromEuler(0, rad(-1), 0))
				end
			else
				rf.x, rf.z = rf.x + vX, rf.z + vZ
			end
		else
			rf.x, rf.z = rf.x + vX, rf.z + vZ
		end
	end
	PositionOnRail(rf)
end

local function updatePosition(e, rf)
	
	local ax, ay, az = Q.ToEuler(rf.quat)

	Navigate(rf, ax, ay, az)
	
	if rf.subState ~= 'brake' then
	
		ax, ay, az = Q.ToEuler(rf.quat)	

		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.speed .. ", " ..
				  (RF.subState or 'nil') .. ", " .. 
				  (RF.logicPos or 'nil') 
				)
end
				
function minecart_main(e)

	local RF = mineCarts[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.logicPos      = 0
		
		SetTimerSecs(RF, 5)	

		PositionOnRail(RF)
		
	elseif RF.state == 'idle' then

		if TimerExpired(RF) then	
			RF.state = 'drive'
			RF.speed = 2
		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