-- 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

function WhackyRacerAddHammer( e, name )
    for k, v in pairs( whackyRacer ) do
        if v.hammer == nil then
			if sub(v.name,1,6) == sub(name,1,6) then
				v.hammer = { Ent = e, angle = -20 }
				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 ho = { x = 0, y = 55.5, z = 129.3 }

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

    local xo, yo, zo = Rotate3D( ho.x, ho.y, ho.z, xA, yA, zA )
        
    ResetPosition( v.hammer.Ent, v.x + xo, v.y + yo, v.z + zo )
    
    xA, yA, zA = Q.ToEuler( Q.Mul( v.quat, Q.FromEuler( rad( v.hammer.angle ), 0 ,0 ) ) )

    ResetRotation( v.hammer.Ent, deg( xA ), deg( yA ), deg( zA ) )
    
    if v.fireHammer then
        if v.hammer.angle > -170 then
            v.hammer.angle = v.hammer.angle - 17
        else
            v.fireHammer = false
        end
    elseif v.hammer.angle < -20 then
        v.hammer.angle = v.hammer.angle + 5
    else
        v.hammer.angle = -20
    end
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.04

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

	if v.hammer == nil then
		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.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
		
	elseif 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 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, -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

local maxSpeed      = 12
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
    if not v.fireHammer and 
       v.hammer.angle > -60 and g_MouseClick == 1 then
        v.fireHammer = true
    end
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 = {}, hammer = nil, fireHammer = false,
              x = Ent.x, y = Ent.y + 50, z = Ent.z,
              quat = Q.FromEuler( 0 , 0, 0),
			  driveTo = { x = Ent.x, z = Ent.z }, vmAng = 0,
              vmAng = 0, rotAng = 0, turnAng = 0,
              speed = 0, wpt = nil, rteDir = 1,
			  timer = math.huge, timer2 = 0
            }

        return
    end
    
    if vehicle.state == 'init' and GotEnts( e, vehicle ) then
    
        FindWaypoints( e, vehicle )
        
        vehicle.state = 'idle'
        vehicle.timer = g_Time + 3000
    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 vehicle.hammer ~= nil then
            PositionHammer( vehicle, xA, yA, zA )
			
			AttachPlayer( vehicle, xA, yA, zA )
			
			PlayerControl( vehicle )
			
			MoveVehicle( vehicle, xA, yA, zA )            
 
        else
		
			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
end