//
// Asura_Physics_Vehicle_FourWheeled.cpp
//
// Created by KJFL - 13 September 2005
//
// Copyright (C) 2005 Rebellion, All Rights Reserved.
//

#include "Core/Asura.h"

// commented out with recent physics changes - will need rewriting to work with new system
#if 0 

// This file's header.
#include "Physics/Vehicles/Asura_Physics_Vehicle_FourWheeled.h"

#include "Core/Asura_LOS.h"

// The physics configuration.
#include "ProjectSettings/Asura_PhysicsConfiguration.h"

#include "Physics/Asura_Physics.h"
#include "Physics/Asura_Physics_Object.h"
#include "Physics/Asura_Physics_RigidBody.h"
#include "Rendering/Asura_Render.h"

Asura_Physics_Vehicle_FourWheeled::Asura_Physics_Vehicle_FourWheeled()
{
	//Initialise( Asura_Physics_Object::Create( ASURA_PHYSICS_OBJECT_DYNAMIC ) );
}

void Asura_Physics_Vehicle_FourWheeled::Initialise( Asura_Physics_Object* pxPhysicsObject )
{
	Asura_Bounding_Box xBB;

	xBB.MinX = -1.0f;
	xBB.MaxX = +1.0f;

	xBB.MinY = -1.0f;
	xBB.MaxY = +0.0f;

	xBB.MinZ = -2.0f;
	xBB.MaxZ = +2.0f;

	Asura_AVector3 axBasePosition[5];

	axBasePosition[0] = Asura_AVector3( 0.0f, -1.0, 0.0f );

	// four base vertices
	axBasePosition[1] = Asura_AVector3( xBB.MaxX, 1.0f/4.0f, xBB.MaxZ );
	axBasePosition[2] = Asura_AVector3( xBB.MinX, 1.0f/4.0f, xBB.MaxZ );
	axBasePosition[3] = Asura_AVector3( xBB.MaxX, 1.0f/4.0f, xBB.MinZ );
	axBasePosition[4] = Asura_AVector3( xBB.MinX, 1.0f/4.0f, xBB.MinZ );


	
	m_pxPhysicsObject = pxPhysicsObject;
	pxPhysicsObject->SetupVehicle( xBB, axBasePosition );
	// create a rigid body with the correct vehicle pyramid, based on collision mesh

	// init positions etc.
	Asura_Physics_RigidBodyInstance* pxInstance = Asura_Physics_RigidBodyInstance::GetInstancePointer( m_pxPhysicsObject->GetHandle() );
	
	for (u_int u=0; u<uNUMBER_OF_WHEELS; ++u)
	{
		m_axWheel[u].m_fAngularPosition = 0.0f;
		m_axWheel[u].m_fAngularVelocity = 0.0f;

		m_axWheel[u].m_fRadius = 0.5f;
	
		// front wheels are steerable
		m_axWheel[u].m_bSteerable = (axBasePosition[u+1].GetZ() > 0.0f);

		pxInstance->GetBaseVertexPosition( u+1, m_axWheel[u].m_xPosition );
	}
	
	m_fBreakingStrength = 0.0f;
	m_fDriveTorque = 0.0f;
	m_fSteeringAngle = 0.0f;
}


void Asura_Physics_Vehicle_FourWheeled::SetSteering( float fSteering )
{
	const float fSteeringLimit = Asura_Maths::DegToRad( 15.0f );

    fSteering = fSteering * fSteeringLimit;
	
	fSteering = Asura_Maths::Max( fSteering, -fSteeringLimit);
	fSteering = Asura_Maths::Min( fSteering, +fSteeringLimit);

	m_fSteeringAngle = fSteering;
}

void Asura_Physics_Vehicle_FourWheeled::SetAccelerator( float fAccelerator )
{
	fAccelerator = Asura_Maths::Max( fAccelerator, -1.0f );
	fAccelerator = Asura_Maths::Min( fAccelerator, 1.0f );

	m_fAccelerator = fAccelerator;
}
void Asura_Physics_Vehicle_FourWheeled::SetBrake( float fBrake )
{
	fBrake = Asura_Maths::Max( fBrake, 0.0f );
	fBrake = Asura_Maths::Min( fBrake, 1.0f );

	m_fBreakingStrength = fBrake;
}

#include "Core/Asura_UserInput.h"
void Asura_Physics_Vehicle_FourWheeled::Update( void )
{
#if 0
	// hack keys to remove
	if (Asura_UserInput::KeyPressed( ASURA_KEYID_NUMPAD8 ))
	{
		m_fAccelerator = 1.0f;
	}
	else
	{
		m_fAccelerator = 0.0f;
	}
	if (Asura_UserInput::KeyPressed( ASURA_KEYID_NUMPAD5 ))
	{
		m_fBreakingStrength = 1.0f;
	}
	else
	{
		m_fBreakingStrength = 0.0f;
	}

	if (Asura_UserInput::KeyPressed(ASURA_KEYID_NUMPAD4))
	{
		m_fSteeringAngle = -Asura_Maths::DegToRad( 8.0f );
	}
	else if (Asura_UserInput::KeyPressed(ASURA_KEYID_NUMPAD6))
	{
		m_fSteeringAngle = +Asura_Maths::DegToRad( 8.0f );
	}
	else
	{
		m_fSteeringAngle *= 0.8f;
	}
	// end of hack keys to remove
#endif
	
	// check control values are within sensible levels

	if ( m_fAccelerator != 0.0f )
	{
		// drive torque = engine torque * gear ratio * differential ratio * transmission efficiency

		m_fDriveTorque = m_fAccelerator * 1000.0f * 2.66f * 3.42f * 0.7f ;

//		m_fDriveTorque = Asura_Maths::Max( m_fDriveTorque, 0.5f );
	}
	else
	{
		m_fDriveTorque *= 0.8f;
	}

	
	// move the base verts according to input
	UpdateWheelPositions();
}

void Asura_Physics_Vehicle_FourWheeled::UpdateWheelPositions( void )
{
	Asura_Physics_RigidBodyInstance* pxInstance = Asura_Physics_RigidBodyInstance::GetInstancePointer( m_pxPhysicsObject->GetHandle() );
	if (!pxInstance) return;

	pxInstance->WakeUp();

	
	
	
	const float fSuspensionLength = 0.5f;
	const float fMass = 500.0f;
	const float fDrag = 0.4f / fMass;
	const float fRollingResistance = 30.0f * fDrag;


	for (u_int u=0; u<uNUMBER_OF_WHEELS; ++u)
	{
		Asura_Physics_Vehicle_Wheel& xWheel = m_axWheel[u];

		Asura_Matrix_3x3 xOrient = m_pxPhysicsObject->GetOrientation();
		
		if (xWheel.m_bSteerable)
		{
			Asura_Matrix_3x3 xOrientation;
			Asura_Euler_3 xEuler( 0.0f, m_fSteeringAngle, 0.0f );

			xEuler.ToMatrix( xOrientation );

			xOrient = xOrient * xOrientation;
		}

		// make some vectors that form the axes of the vehicle
		Asura_AVector3 xSideways	( xOrient.e11, xOrient.e21, xOrient.e31 );
		Asura_AVector3 xDownwards	( xOrient.e12, xOrient.e22, xOrient.e32 );
		Asura_AVector3 xForwards	( xOrient.e13, xOrient.e23, xOrient.e33 );

		// LOS only works on 3-vectors currently
		Asura_Vector_3 xDownwards_3( xOrient.e12, xOrient.e22, xOrient.e32 );
	


		// displacement of wheel (ignoring suspension)
		Asura_AVector3 xDisp = pxInstance->m_pxBasePosition[u+1] - pxInstance->m_pxBasePrevPosition[u+1];
	
		// find suspension position
		// fire line from topmost possible wheel position, and see when we hit something
		const Asura_LOS_OutputData* pxData;
		
		// LOS works on 3-vectors currently
		{
			Asura_Vector_3 xOrigin ( pxInstance->m_pxBasePosition[u+1] );
			xOrigin -= xDownwards_3 * fSuspensionLength;
	
			Asura_LOS_InputData xInputData( xOrigin, xDownwards_3, ASURA_GUID_UNREGISTERED, fSuspensionLength * 2.0f + xWheel.m_fRadius );
			
			pxData = Asura_LOS::TestAgainstEnvironment( &xInputData );
		}

		// if we've hit the environment, the wheel is in contact with the ground
		if (pxData->m_bHitEnvironment)
		{
			float fS = pxData->m_fDistanceToIntersection - xWheel.m_fRadius;

			if (fS<0.0f) fS = 0.0f;

			fS /= fSuspensionLength;

			if (fS>2.0f) fS = 2.0f;

			xWheel.m_fSuspensionPosition = (fS-1.0f) * fSuspensionLength;

			fS = 2.0f - fS;

			// Apply a force which is:
			//
			//	1) Zero when the wheel is 'dangling' at the end of it's suspension
			//
			//	2) Equal to gravity when the wheel is 'centred' in it's suspension
			//
			//	3) Larger (e.g. 8 times here) than the gravity force when the wheel is maximally compressed, forced up into the vehicle
			//

			float fForce = fS * fS * fS *  fASURA_PHYSICS_GRAVITY * (Asura_Physics_System::GetTimeSlice() * Asura_Physics_System::GetTimeSlice() );

			float fSuspensionSpeed = xDisp * xDownwards;

			// apply suspension damping
			xDisp -= xDownwards * fSuspensionSpeed * 0.15f;

			// apply suspension restitution force
			xDisp -= xDownwards * fForce;
	
			float fDistanceForwards = xDisp * xForwards;

			// rotate wheel based on how far forwards it's moved
			xWheel.m_fAngularPosition += -fDistanceForwards / xWheel.m_fRadius;

			// apply brakes
			float fBreaks = ( m_fBreakingStrength * 10.0f * Asura_Physics_System::GetTimeSlice() * Asura_Physics_System::GetTimeSlice());

			fBreaks = Asura_Maths::Min( Asura_Maths::Abs(fDistanceForwards), fBreaks );

			if (fDistanceForwards < 0.0f ) fBreaks = -fBreaks;
			
			xDisp -= xForwards * fBreaks;

			// rolling resistance
			xDisp -= xForwards * (fDistanceForwards) * fRollingResistance * Asura_Physics_System::GetTimeSlice();
	
			if (!xWheel.m_bSteerable) // rear wheel drive
			{
				// apply motive power

				// drive force = torque / wheel radius
				// & f = ma

				const float fAcceleration = ( m_fDriveTorque / xWheel.m_fRadius ) / fMass;

				xDisp += xForwards * fAcceleration * (Asura_Physics_System::GetTimeSlice() * Asura_Physics_System::GetTimeSlice()  );
			}

			// anti-slide friction
			float fSlidingSpeed = xDisp * xSideways;

			if (xWheel.m_bSteerable)
			{
				xDisp -=  xSideways * fSlidingSpeed * 0.10f;
			}
			else
			{
				xDisp -=  xSideways * fSlidingSpeed * 0.06f;
			
			}

			




		}
		else
		{
			// wheel at maximum suspension length, i.e. it's dangling
			xWheel.m_fSuspensionPosition = fSuspensionLength;

			// TODO - rotate wheel based on engine speed (faster than it would normally rotate 
			// if it were in contact with the ground, I guess).

			// TODO - 'air steering' could go here, if the vehicle/gameplay demanded it.
		}

		// air friction

		const Asura_AVector3 xVel = xDisp;

		xDisp -= (xVel) * xVel.Magnitude() * fDrag ;

	
		// modify prev position to update velocity:
		pxInstance->m_pxBasePrevPosition[u+1] = pxInstance->m_pxBasePosition[u+1] - xDisp;
			
		// the wheel position, taking suspension into account
		xWheel.m_xPosition = pxInstance->m_pxBasePosition[u+1] + xDownwards * xWheel.m_fSuspensionPosition;
	}
}


void Asura_Physics_Vehicle_FourWheeled::GetWheelPosition( u_int uWheel, Asura_Vector_3& xPosition ) const
{
	Asura_Assert( uWheel < uNUMBER_OF_WHEELS, "Invalid wheel number" );

	xPosition = m_axWheel[ uWheel ].m_xPosition;
}

void Asura_Physics_Vehicle_FourWheeled::GetWheelOrientation( u_int uWheel, Asura_Matrix_3x3& xOrientation ) const
{
	Asura_Assert( uWheel < uNUMBER_OF_WHEELS, "Invalid wheel number" );

	Asura_Euler_3 xEuler( m_axWheel[ uWheel ].m_fAngularPosition, 0.0f, 0.0f );

	if (m_axWheel[ uWheel ].m_bSteerable)
	{
		xEuler.y = m_fSteeringAngle;
	}

	xEuler.ToMatrix( xOrientation );

	xOrientation = m_pxPhysicsObject->GetOrientation() * xOrientation;
}


void Asura_Physics_Vehicle_FourWheeled::DebuggingRender( void )
{
	for (u_int u=0; u<uNUMBER_OF_WHEELS; ++u)
	{
		Asura_Vector_3 xCentre;
		Asura_Matrix_3x3 xOrientation;
			
		GetWheelPosition( u, xCentre );
		GetWheelOrientation( u, xOrientation );

		DebuggingRender_Wheel(  xCentre, xOrientation, m_axWheel[u].m_fRadius );
	}
}

void Asura_Physics_Vehicle_FourWheeled::DebuggingRender_Wheel( Asura_Vector_3& xCentre, Asura_Matrix_3x3& xOrient, float fRadius )
{
	const u_int uNumVertsInWheel = 16;

	Asura_Vector_3 axWheelVerts[uNumVertsInWheel];
	
	float fAngle = 0;

	for (u_int u=0; u<uNumVertsInWheel; ++u)
	{
		axWheelVerts[u].x = 0;
		axWheelVerts[u].y = fRadius * Asura_Maths::Cosine( fAngle );
		axWheelVerts[u].z = fRadius * Asura_Maths::Sine( fAngle );

		fAngle += Asura_Maths::Pi * 2.0f / (float)uNumVertsInWheel;

		axWheelVerts[u] = (axWheelVerts[u] * xOrient); 
		axWheelVerts[u] += xCentre;
	};

	for (u_int u=0; u<uNumVertsInWheel; ++u)
	{
		u_int v = (u+1)%uNumVertsInWheel;
		Asura_Render::RenderLine( axWheelVerts[u], axWheelVerts[v], 0xffffffff );
	}

	Asura_Render::RenderLine( axWheelVerts[0], axWheelVerts[uNumVertsInWheel/2], 0xffffffff );
}

#endif
