

//
// Vehicle setup. Mostly for physics
//
function	SetupVehiclePhysics(vehicle ref as vehicle, reset as integer)
	a as point
	y as float
	i as integer


	vehicle.velocity.max = 48 * SCALE_3D
	vehicle.velocity.current = 0
	vehicle.delay.amount = 0.500
	vehicle.steering = STEERING_LOCAL
	
	vehicle.velocity.linearUnit = 0.5 * SCALE_3D
	vehicle.velocity.angularUnit = 5 * SCALE_3D
	
	SetObjectCastShadow(vehicle.chassis, 1)					// Shadow stuff
//	SetObjectReceiveShadow(vehicle.chassis, 1)	
	
	for i=0 to vehicle.wheels.length
		SetObjectCastShadow(vehicle.wheels[i].id, 1)
	next

	if reset = TRUE											// Full reset at start of game
		Delete3DPhysicsBody(vehicle.hit)				
		SetObjectRotation(vehicle.hit, 0, 0, 0)		
		SetObjectPosition(vehicle.hit, 0, vehicle.size.y, 0)
	else
		y = GetObject3DPhysicsLinearVelocityY(vehicle.hit)	// In game reset (R key pressed)
		Delete3DPhysicsBody(vehicle.hit)		
		if abs(y) > 200
			SetObjectRotation(vehicle.hit, 0, 0, 0)		
			SetObjectPosition(vehicle.hit, 0, vehicle.size.y, 0)
		endif
		a.x = GetObjectAngleX(vehicle.hit)
		a.y = GetObjectAngleY(vehicle.hit)		
		a.z = GetObjectAngleZ(vehicle.hit)						
		SetObjectPosition(vehicle.hit, GetObjectX(vehicle.hit), GetObjectY(vehicle.hit) + (vehicle.size.y * 2), GetObjectZ(vehicle.hit))
		SetObjectRotation(vehicle.hit, 0, 0, 0)		
		RotateObjectLocalY(vehicle.hit, a.y)		
	endif

	Create3DPhysicsDynamicBody(vehicle.hit)						// Set out hitbox to be a convex hull
	CreateVehicleShape(vehicle)
	SetObject3DPhysicsCanSleep(vehicle.hit, 0)	
	SetObject3DPhysicsDamping( vehicle.hit , 0.2, 0.9 ) 		// Angular dampening. Works fantastic for steering
	SetObject3DPhysicsMaxLinearVelocity( vehicle.hit, vehicle.velocity.max ) 
	SetObject3DPhysicsCanSleep(vehicle.hit, 0)	
	SetObject3DPhysicsMass(vehicle.hit, GetObject3DPhysicsMass(vehicle.hit) * 2)		
endfunction

//
// This moves the vehicle
//
function	MoveVehicle(vehicle ref as vehicle, env as environment)
	t 		as point
	i		as integer
	v		as float
	d		as integer
	turn	as float
	a		as float
	l	 	as point
	dot		as float

	if vehicle.delay.count > timer()
		SetObjectColor(vehicle.chassis, 0, 0xff, 0xff, 0xff)
		exitfunction
	endif
	
	SetObjectColor(vehicle.chassis, 0xff, 0xff, 0xff, 0xff)	
		
	turn = 0
	
																			// Get the linear velocity
	l = CreatePoint(GetObject3DPhysicsLinearVelocityX(vehicle.hit),  GetObject3DPhysicsLinearVelocityY(vehicle.hit), GetObject3DPhysicsLinearVelocityZ(vehicle.hit))
	
	if CheckWalls(vehicle, env, l) = TRUE
		print("Wall hit")
		exitfunction
	endif

	if GetRawKeyState(39) = 1										// Turn right/left
		turn = vehicle.velocity.angularUnit
	endif	
	
	if GetRawKeyState(37) = 1
		turn = -vehicle.velocity.angularUnit
	endif	

	vehicle.wheelAngle = vehicle.wheelAngle  * 0.8						// Dampen the wheel angle
	
	for i=0 to vehicle.wheels.length									// Check the wheels
		inc d, CheckWheels(vehicle, i, env)
	next
																		// If we have less than 3 wheels on the ground then we're in the air
																		// so turn everything off until we are in contact with the ground again	
	if d < 3 															
		if GetObject3DPhysicsLinearVelocityY(vehicle.hit) < -(25 * SCALE_3D)	// Falling velocity too high, throttle it
			a = DistancePoint(CreatePoint(GetObject3DPhysicsLinearVelocityX(vehicle.hit),  GetObject3DPhysicsLinearVelocityY(vehicle.hit) * 0.999, GetObject3DPhysicsLinearVelocityZ(vehicle.hit)))					
			SetObject3DPhysicsLinearVelocity( vehicle.hit, GetObject3DPhysicsLinearVelocityX(vehicle.hit), GetObject3DPhysicsLinearVelocityY(vehicle.hit), GetObject3DPhysicsLinearVelocityZ(vehicle.hit), a * .9)		
		endif
		
		if vehicle.ground = TRUE										// Use colours to see if we are in the air or not
			SetObjectColor(vehicle.chassis, 0xff, 0, 0, 0xff)
			SetObjectColor(vehicle.hit, 0xff, 0, 0, 0xff)			
		endif
		
		if vehicle.flying = FALSE										// First time in the air damp down velocity
			SetObject3DPhysicsDamping( vehicle.hit , 0.5, 0.99 ) 
		endif
		
		vehicle.flying = TRUE

		exitfunction
	endif
	
	if vehicle.flying = TRUE											// Just hit the ground, reset damping
		SetObject3DPhysicsDamping( vehicle.hit , 0.2, 0.9 ) 		
	endif
	
	vehicle.flying = FALSE
	
	if vehicle.ground = TRUE
		SetObjectColor(vehicle.hit, 0, 0xff, 0, 0xff)					
		SetObjectColor(vehicle.chassis, 0, 0xff, 0, 0xff)				// Use colours to reort if we are in the air or not
	endif


																		// Get the angular velocity
	a = DistancePoint(CreatePoint(GetObject3DPhysicsAngularVelocityX(vehicle.hit),  GetObject3DPhysicsAngularVelocityY(vehicle.hit), GetObject3DPhysicsAngularVelocityZ(vehicle.hit)))
		

	v = 0																// Acceleration

	if GetRawKeyState(40) = 1
		v = -vehicle.velocity.linearUnit
	endif
			
	if GetRawKeyState(38) = 1
		v = vehicle.velocity.linearUnit
	endif	

	if vehicle.brakes = TRUE
		v = v * 0.9
	endif
	
	if vehicle.velocity.current < 0 
		if abs( vehicle.velocity.current + v ) < ( vehicle.velocity.max * 0.5)		// Reverse maximum is
			inc vehicle.velocity.current,  v
		else
			vehicle.velocity.current = ( vehicle.velocity.max * -0.5)
		endif
	else
		if abs( vehicle.velocity.current + v ) < vehicle.velocity.max
			inc vehicle.velocity.current,  v
		else
			vehicle.velocity.current = vehicle.velocity.max * (vehicle.velocity.current / abs(vehicle.velocity.current))
		endif
	endif
	
	t = GetVehicleDirectionGround(vehicle)						// Get the vector of the vehicle
	

	
	SetVector3(vehicle.vectU, l.x, l.y, l.z)							// Get the direction
	SetVector3(vehicle.vectV, t.x, t.y, t.z)
	dot = Round(GetVector3Dot(vehicle.vectU, vehicle.VectV))

	if v <> 0 
		if vehicle.brakes = FALSE
			SetObject3DPhysicsLinearVelocity( vehicle.hit, t.x, t.y, t.z, vehicle.velocity.current)
		endif
	else
		if dot <> 0 
			vehicle.velocity.current = DistancePoint(l) * (dot / abs(dot))	
			SetObject3DPhysicsLinearVelocity( vehicle.hit, t.x, t.y, t.z, vehicle.velocity.current)		
		endif
	endif
	
	print(str(vehicle.wheelAngle)+":"+str(turn))

	if abs(vehicle.wheelAngle) < 4 and vehicle.delay.count < timer()								// turn the wheels
		inc vehicle.wheelAngle, ( turn / 10)
		SetObjectRotation(vehicle.wheels[0].pivot, 0, vehicle.wheelAngle, 0)
		SetObjectRotation(vehicle.wheels[1].pivot, 0, vehicle.wheelAngle, 0)		
	endif
	
	

																		// Apply the turning force (Dampening is set in SetupVehicle())
																		// The reason we're checking velocity is because we want a
																		// tight turning circle when it's slow and a smooth one
																		// when it's fast
	if turn <> 0 and abs(vehicle.velocity.current) > (1 * SCALE_3d)
		if dot < 0
			turn = -turn
		endif

		if dot <> 0
			if vehicle.steering = STEERING_GLOBAL
				l = GetVehicleTurn(vehicle, turn)
			else
				l.x = 0 : l.y = turn : l.z = 0
			endif
			SetObject3DPhysicsAngularVelocity( vehicle.hit, l.x, l.y, l.z, ((vehicle.velocity.max - vehicle.velocity.current) / (0.8 * scale_3d)) + (vehicle.velocity.current ) / (0.75 * scale_3d))	
		endif			
	else
			SetObject3DPhysicsAngularVelocity( vehicle.hit, 0, 1, 0, 0)
	endif

endfunction

//
// This checks for collision with walls
// If the collision is dramatic it turns everything off for vehicle.delay.amount seconds
// If the collision is just grazing the wall then ignore it
//
function	CheckWalls(vehicle ref as vehicle, env as environment, vel as point)
	o	as integer
	b	as float
	a	as float
	c	as point
	v	as point
	dot	as float
	r as integer = FALSE

	if vehicle.delay.count < timer() 
		if GetObject3DPhysicsFirstContact( vehicle.hit ) 
			repeat 
				o = GetObject3DPhysicsContactObjectB()
				if o = env.walls 
					a = DistancePoint(vel)
					b = DistancePoint(vehicle.velocity.last)
					if a / b < 0.7													// Change in velocity is dramatic so we are not just grazing a wall.					
						r = TRUE						
						vehicle.velocity.current=0
						vehicle.delay.count = timer() + vehicle.delay.amount		//  Turn everything off
					endif
				endif
			until GetObject3DPhysicsNextContact() = FALSE
		endif
	endif
endfunction r

//
// This manages the wheels
// 1. It determines if the wheel is in contact with the ground
// 2. It manages the suspension (Moving the wheel up and down)
//
function	CheckWheels(vehicle ref as vehicle, i as integer, env as environment)
	l 		as line
	h		as point
	d		as float
	m		as point
	c		as integer
	w   	as wheel
	v		as float
	y		as float
	ease	as float = 0.3
	p		as point	
	id		as integer
	
	w = vehicle.wheels[i]
		
	//
	// Manage the rolling of the wheel
	//
	if vehicle.velocity.current > 0
		d = 1
	else
		d = -1
	endif
	
	v = Distance3D(CreatePoint(GetObjectX(vehicle.hit), GetObjectY(vehicle.hit), GetObjectZ(vehicle.hit)), vehicle.lastPos)  * d	
	RotateObjectLocalX(w.id, (v / w.circumfrence) * 360)
				
	//
	// Determine if the wheel is touching the ground
	// and manage the suspension
	//
	SetObjectColor(w.toSensor, 0xff, 0, 0, 0xff)	
	
	p = w.pos	
	l.f = GetObjectPos(w.fromSensor)
	l.t = GetObjectPos(w.toSensor)		

	c = ObjectRayCast( env.ground, l.f.x, l.f.y, l.f.z, l.t.x, l.t.y, l.t.z )

	w.hit.contact = FALSE
	
	if c <> 0			// Sensor has hit the ground
		id = GetObjectRayCastHitID( 0 )
		if id <> env.ground
			Message("Collision not with ground")
		endif
		w.hit.pos.x = GetObjectRayCastX(0) : w.hit.pos.y = GetObjectRayCastY(0) :w.hit.pos.z = GetObjectRayCastZ(0)
		d = GetObjectRayCastDistance( 0 ) 
		SetObjectPosition(w.id, 0, -d, 0)		
		SetObjectPosition(w.hit.id, 0, GetObjectY(w.fromSensor) - d, 0)		
		if d > w.radius  // Not touching the ground
			d = 0
		else
			w.hit.contact = TRUE
			SetObjectColor(w.toSensor, 0, 0xff, 0, 0xff)
			d = 1
		endif
	else				// Not touching the ground
		d = 0
		l.f = GetObjectLocalPos(w.id)
		l.t.y = -(w.radius * vehicle.suspension)
		SetObjectPosition(w.id, 0, l.f.y + (l.t.y - l.f.y) * ease, 0)
	endif
	
	vehicle.wheels[i] = w

endfunction d	


function	GetVehicleTurn(vehicle as vehicle, turn as float)
	t 	as point
	id	as integer
	
	if turn > 0
		id = vehicle.right
	else
		id = vehicle.left
	endif
	
	t.x = NormalizeAngle(GetObjectWorldAngleX(id) - GetObjectWorldAngleX(vehicle.chassis)) 
	t.y = NormalizeAngle(GetObjectWorldAngleY(id) - GetObjectWorldAngleY(vehicle.chassis))
	t.z = NormalizeAngle(GetObjectWorldAngleZ(id) - GetObjectWorldAngleZ(vehicle.chassis))
	
endfunction t


function	NormalizeAngle(a as float)
	//a = mod((a + 180), 360) - 180
	if a > 180
		dec a, 360
	endif
	if a < -180
		inc a, 360
	endif
endfunction a

function	GetVehicleDirectionAverage(vehicle as vehicle, dir as integer)
	v1	as point
	v2	as point
	v3	as point
	
	v1 = GetVehicleDirectionGround(vehicle)
	v2 = GetVehicleDirectionVehicle(vehicle, dir)
	
	v3.x = (v1.x + v2.x) / 2
	v3.y = (v1.y + v2.y) / 2
	v3.z = (v1.z + v2.z) / 2		
endfunction v3

function	GetVehicleDirectionGround(vehicle as vehicle)
	v	as point
	f	as point
	t	as point
	
	//
	// Select right or left side wheels contact to calculate direction
	//
	if vehicle.wheels[0].hit.contact = TRUE and vehicle.wheels[2].hit.contact = TRUE 
		f = vehicle.wheels[2].hit.pos
		t = vehicle.wheels[0].hit.pos		
	elseif vehicle.wheels[1].hit.contact = TRUE and vehicle.wheels[3].hit.contact = TRUE 
		f = vehicle.wheels[3].hit.pos
		t = vehicle.wheels[1].hit.pos		
	else
		Message("Catastrophic error : Wheels aren't touching")
	endif
	
	v.x = t.x - f.x : v.y = t.y - f.y : v.z = t.z - f.z 
	
endfunction v


function	GetVehicleDirectionVehicle(vehicle as vehicle, dir as integer)
	v	as point
	c	as point
	d	as point
	
	c = GetObjectPos(vehicle.chassis)
	
	select dir
		case MOVE_FORWARD
			d = GetObjectPos(vehicle.forward)
		endcase			
		case MOVE_REVERSE
			d = GetObjectPos(vehicle.reverse)		
		endcase			
		case MOVE_RIGHT
			d = GetObjectPos(vehicle.right)					
		endcase			
		case MOVE_LEFT
			d = GetObjectPos(vehicle.left)								
		endcase
	endselect
	
	v = CreatePoint(d.x - c.x, d.y - c.y, d.z - c.z )
	
endfunction v
