-- 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 asin = math.asin
local atan = math.atan2


function whacky_racer_walkthru_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 }
            return
        end
    end    
end

function WhackyRacerAddHammer( e )
    for k, v in pairs( whackyRacer ) do
        if v.hammer == nil then
            v.hammer = { Ent = e, angle = -20 }
            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 },  -- 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 ) + 24
        
        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 + 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 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 maxSpeed      = 20
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

local function MoveVehicle( v, xA, yA, zA )
    
    if 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 GotEnts( e, v )
    if #v.wheels ~= 4 then
        PromptLocal( e, "Not enough wheel entities!" )
        return false
    end
	if v.hammer == nil then
        PromptLocal( e, "No weapon entity!" )
        return false
    end
	
    return true
end

local function AttachPlayer( v, xA, yA, zA )
        
    local xo, yo, zo = Rotate3D( 0, 0, -300, xA, yA, zA )

    local y = GetTerrainHeight( v.x + xo, v.z + zo )
    
    SetFreezePosition( v.x + xo, y + 200, v.z + zo )
    SetFreezeAngle( 20, v.vmAng, 0 )
    TransportToFreezePosition()
end

function whacky_racer_walkthru_main( e )

    local vehicle = whackyRacer[e]
    
    if vehicle == nil then
        local Ent = g_Entity[e]
        
        whackyRacer[e] = 
            { state = 'init',
              wheels = {}, hammer = nil, fireHammer = false,
              x = Ent.x, y = Ent.y + 50, z = Ent.z,
              quat = Q.FromEuler( rad( GetEntityAngleX( e ) ), 
                                  rad( GetEntityAngleY( e ) ), 
                                  rad( GetEntityAngleZ( e ) ) ),
              vmAng = 0, rotAng = 0, turnAng = 0,
              speed = 0
            }
        return
    end
    
    if vehicle.state == 'init' and GotEnts( e, vehicle ) then

        vehicle.state = 'idle'
    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 )

        AttachPlayer( vehicle, xA, yA, zA )
        
        MoveVehicle( vehicle, xA, yA, zA )
        
        PlayerControl( vehicle )
        
        if vehicle.hammer ~= nil then
            PositionHammer( vehicle, xA, yA, zA )
        end            
    end
end