-- 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 sub  = string.sub

local random = math.random

local names = {}

function whacky_racer_init_name( e, name )
    Include( "quatlib.lua" )
	names[e] = name
end

function WhackyRacerAddWheel( e, name )
    for k, v in pairs( whackyRacer ) do
        if #v.wheels < 4 then
			if sub(v.name,1,6) == sub(name,1,6) then
                v.wheels[#v.wheels + 1] = { Ent = e, quat = nil }
                return true
			end
        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 },  -- left  front
         { x = -62.7, y = -3.7, z =   0.6 }, -- left  rear
         { x =  62.7, y = -3.7, z = 136.4 },  -- right front
         { x =  62.7, y = -3.7, z =   0.6 }  -- right rear
        }

local wheelHeights = {}

local function calcAngle( h1, h2, l )
    return asin( ( h2 - h1 ) / l )
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 = calcAngle( wheelHeights[1], wheelHeights[3], 
	                       wheelOffsets[3].x - wheelOffsets[1].x );
						   
    local ax2 = calcAngle( wheelHeights[2], wheelHeights[4], 
	                       wheelOffsets[4].x - wheelOffsets[2].x );
    
    avgRoll = (ax1 + ax2) / 2
    
    local p1 = calcAngle( wheelHeights[1], wheelHeights[2], 
	                      wheelOffsets[1].z + wheelOffsets[2].z );
						  
    local p2 = calcAngle( wheelHeights[3], wheelHeights[4], 
	                      wheelOffsets[3].z + wheelOffsets[4].z );
    
    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 speedTarget      = 10
local acceleration     = 0.04

local function MoveVehicle( v, xA, yA, zA )
    
    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.2
        end
    elseif turnAng < -30 then 
        turnAng = -30
        if v.speed > 1 then
            v.speed = v.speed - acceleration * 1.2
        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

local function AttachPlayer( v, xA, yA, zA )
        
    local xo, yo, zo = Rotate3D( 0, 0, -500, 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

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

	-- accelerate
	if v.speed < speedTarget then
		v.speed = v.speed + acceleration
	end
	
	local fX, fY, fZ = Rotate3D( 0, 0, v.speed * 10, xA, yA, zA )
	local mX, mZ = v.x + fX, v.z + fZ

	if PointIsInZone(mX, mZ, v.zone) then
		Prompt("F")
		v.driveTo.x = mX
		v.driveTo.z = mZ
		return
	end
	-- try to left	
	fX, fY, fZ = Rotate3D( -v.speed * 3, 0, v.speed * 5, xA, yA, zA )
	mX, mZ = v.x + fX, v.z + fZ
	if PointIsInZone(mX, mZ, v.zone) then
		Prompt("L1")
		v.driveTo.x = mX
		v.driveTo.z = mZ
		return
	end
	-- try to right
	fX, fY, fZ = Rotate3D( v.speed * 3, 0, v.speed * 5, xA, yA, zA )
	mX, mZ = v.x + fX, v.z + fZ
	if PointIsInZone(mX, mZ, v.zone) then
		Prompt("R1")
		v.driveTo.x = mX
		v.driveTo.z = mZ
		return
	end
	-- try further to left	
	fX, fY, fZ = Rotate3D( -v.speed * 5, 0, v.speed * 3, xA, yA, zA )
	mX, mZ = v.x + fX, v.z + fZ
	if PointIsInZone(mX, mZ, v.zone) then
		Prompt("L2")
		v.driveTo.x = mX
		v.driveTo.z = mZ
		return
	end
	-- try further to right
	fX, fY, fZ = Rotate3D( v.speed * 5, 0, v.speed * 3, xA, yA, zA )
	mX, mZ = v.x + fX, v.z + fZ
	if PointIsInZone(mX, mZ, v.zone) then
		Prompt("R2")
		v.driveTo.x = mX
		v.driveTo.z = mZ
		return
	end
	v.speed = v.speed - acceleration
end

function whacky_racer_main(e)

    local vehicle = whackyRacer[e]
    
    if vehicle == nil then
        local Ent = g_Entity[e]
        
        whackyRacer[e] = 
            { state = 'init', name = names[e],
              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,
              zone = nil,
              rotAng = 0, turnAng = 0,
              timer = math.huge, timer2 = 0,
              speed = 0
            }
        return
    end
    
    if vehicle.state == 'init' and GotEnts( e, vehicle ) then
    
        if PointIsInZone ~= nil then
			vehicle.zone = ZonePointIn(vehicle.x, vehicle.z)
        else
			return
		end
        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, 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         
            
            if vehicle.zone ~= 'None' then
				FollowZone(vehicle, xA, yA, zA )
			end
			
			MoveVehicle( vehicle, xA, yA, zA )  
			 
			--AttachPlayer( vehicle, xA, yA, zA )
        end
    end
end