/*
 * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
 *
 * Permission to use, copy, modify, distribute and sell this software
 * and its documentation for any purpose is hereby granted without fee,
 * provided that the above copyright notice appear in all copies.
 * Erwin Coumans makes no representations about the suitability 
 * of this software for any purpose.  
 * It is provided "as is" without express or implied warranty.
*/

#include "RaycastVehicle.h"
#include "ConstraintSolver/Solve2LinearConstraint.h"
#include "ConstraintSolver/JacobianEntry.h"
#include "SimdQuaternion.h"
#include "SimdVector3.h"
#include "VehicleRaycaster.h"
#include "WheelInfo.h"


#include "Dynamics/MassProps.h"
#include "ConstraintSolver/ContactConstraint.h"



static RigidBody s_fixedObject( MassProps ( 0.0f, SimdVector3(0,0,0) ),0.f,0.f,0.f,0.f);

RaycastVehicle::RaycastVehicle(const VehicleTuning& tuning,RigidBody* chassis,	VehicleRaycaster* raycaster )
:m_vehicleRaycaster(raycaster),
m_pitchControl(0.f)
{
	m_chassisBody = chassis;
	m_indexRightAxis = 0;
	m_indexUpAxis = 2;
	m_indexForwardAxis = 1;
	DefaultInit(tuning);
}


void RaycastVehicle::DefaultInit(const VehicleTuning& tuning)
{
	m_currentVehicleSpeedKmHour = 0.f;
	m_steeringValue = 0.f;
	
}

	

RaycastVehicle::~RaycastVehicle()
{
}


//
// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
//
WheelInfo&	RaycastVehicle::AddWheel( const SimdVector3& connectionPointCS, const SimdVector3& wheelDirectionCS0,const SimdVector3& wheelAxleCS, SimdScalar suspensionRestLength, SimdScalar wheelRadius,const VehicleTuning& tuning, bool isFrontWheel)
{

	WheelInfoConstructionInfo ci;

	ci.m_chassisConnectionCS = connectionPointCS;
	ci.m_wheelDirectionCS = wheelDirectionCS0;
	ci.m_wheelAxleCS = wheelAxleCS;
	ci.m_suspensionRestLength = suspensionRestLength;
	ci.m_wheelRadius = wheelRadius;
	ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
	ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
	ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
	ci.m_frictionSlip = tuning.m_frictionSlip;
	ci.m_bIsFrontWheel = isFrontWheel;
	ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;

	m_wheelInfo.push_back( WheelInfo(ci));
	
	WheelInfo& wheel = m_wheelInfo[GetNumWheels()-1];
	
	UpdateWheelTransformsWS( wheel );
	return wheel;
}




const SimdTransform&	RaycastVehicle::GetWheelTransformWS( int wheelIndex ) const
{
	assert(wheelIndex < GetNumWheels());
	const WheelInfo& wheel = m_wheelInfo[wheelIndex];
	return wheel.m_worldTransform;

}

void	RaycastVehicle::UpdateWheelTransform( int wheelIndex )
{
	
	WheelInfo& wheel = m_wheelInfo[ wheelIndex ];
	UpdateWheelTransformsWS(wheel);
	SimdVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
	const SimdVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
	SimdVector3 fwd = up.cross(right);
	fwd = fwd.normalize();
	//rotate around steering over de wheelAxleWS
	float steering = wheel.m_steering;
	
	SimdQuaternion steeringOrn(up,steering);//wheel.m_steering);
	SimdMatrix3x3 steeringMat(steeringOrn);

	SimdQuaternion rotatingOrn(right,wheel.m_rotation);
	SimdMatrix3x3 rotatingMat(rotatingOrn);

	SimdMatrix3x3 basis2(
		right[0],fwd[0],up[0],
		right[1],fwd[1],up[1],
		right[2],fwd[2],up[2]
	);
	
	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
	wheel.m_worldTransform.setOrigin(
		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
	);
}

void RaycastVehicle::ResetSuspension()
{

	std::vector<WheelInfo>::iterator wheelIt;
	for (wheelIt = m_wheelInfo.begin();
	!(wheelIt == m_wheelInfo.end());wheelIt++)
	{
			WheelInfo& wheel = *wheelIt;
			wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
			wheel.m_suspensionRelativeVelocity = 0.0f;
			
			wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
			//wheel_info.setContactFriction(0.0f);
			wheel.m_clippedInvContactDotSuspension = 1.0f;
	}
}

void	RaycastVehicle::UpdateWheelTransformsWS(WheelInfo& wheel )
{
	wheel.m_raycastInfo.m_isInContact = false;

	const SimdTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform();

	wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() *  wheel.m_wheelDirectionCS ;
	wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
}

SimdScalar RaycastVehicle::Raycast(WheelInfo& wheel)
{
	UpdateWheelTransformsWS( wheel );

	
	SimdScalar depth = -1;
	
	SimdScalar raylen = wheel.GetSuspensionRestLength()+wheel.m_wheelsRadius;

	SimdVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
	const SimdVector3& source = wheel.m_raycastInfo.m_hardPointWS;
	wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
	const SimdVector3& target = wheel.m_raycastInfo.m_contactPointWS;

	SimdScalar param = 0.f;
	
	VehicleRaycaster::VehicleRaycasterResult	rayResults;

	void* object = m_vehicleRaycaster->CastRay(source,target,rayResults);

	wheel.m_raycastInfo.m_groundObject = 0;

	if (object)
	{
		param = rayResults.m_distFraction;
		depth = raylen * rayResults.m_distFraction;
		wheel.m_raycastInfo.m_contactNormalWS  = rayResults.m_hitNormalInWorld;
		wheel.m_raycastInfo.m_isInContact = true;
		
		wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!;
		//wheel.m_raycastInfo.m_groundObject = object;


		SimdScalar hitDistance = param*raylen;
		wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
		//clamp on max suspension travel

		float  minSuspensionLength = wheel.GetSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*0.01f;
		float maxSuspensionLength = wheel.GetSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*0.01f;
		if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
		{
			wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
		}
		if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
		{
			wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
		}

		wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;

		SimdScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );

		SimdVector3 chassis_velocity_at_contactPoint;
		SimdVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-GetRigidBody()->getCenterOfMassPosition();

		chassis_velocity_at_contactPoint = GetRigidBody()->getVelocityInLocalPoint(relpos);

		SimdScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );

		if ( denominator >= -0.1f)
		{
			wheel.m_suspensionRelativeVelocity = 0.0f;
			wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f;
		}
		else
		{
			SimdScalar inv = -1.f / denominator;
			wheel.m_suspensionRelativeVelocity = projVel * inv;
			wheel.m_clippedInvContactDotSuspension = inv;
		}
			
	} else
	{
		//put wheel info as in rest position
		wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
		wheel.m_suspensionRelativeVelocity = 0.0f;
		wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
		wheel.m_clippedInvContactDotSuspension = 1.0f;
	}

	return depth;
}


void RaycastVehicle::UpdateVehicle( SimdScalar step )
{

	m_currentVehicleSpeedKmHour = 3.6f * GetRigidBody()->getLinearVelocity().length();
	
	const SimdTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform();
	SimdVector3 forwardW (
		chassisTrans.getBasis()[0][m_indexForwardAxis],
		chassisTrans.getBasis()[1][m_indexForwardAxis],
		chassisTrans.getBasis()[2][m_indexForwardAxis]);

	if (forwardW.dot(GetRigidBody()->getLinearVelocity()) < 0.f)
	{
		m_currentVehicleSpeedKmHour *= -1.f;
	}

	//
	// simulate suspension
	//
	std::vector<WheelInfo>::iterator wheelIt;
	int i=0;
	for (wheelIt = m_wheelInfo.begin();
	!(wheelIt == m_wheelInfo.end());wheelIt++,i++)
	{
		WheelInfo& wheelInfo = *wheelIt;
		
		SimdScalar depth; 
		depth = Raycast( *wheelIt );
	}

	UpdateSuspension(step);

	
	for (wheelIt = m_wheelInfo.begin();
	!(wheelIt == m_wheelInfo.end());wheelIt++)
	{
		//apply suspension force
		WheelInfo& wheel = *wheelIt;
		
		float suspensionForce = wheel.m_wheelsSuspensionForce;
		
		float gMaxSuspensionForce = 6000.f;
		if (suspensionForce > gMaxSuspensionForce)
		{
			suspensionForce = gMaxSuspensionForce;
		}
		SimdVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
		SimdVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody()->getCenterOfMassPosition();
		
		GetRigidBody()->applyImpulse(impulse, relpos);
	
	}
	

	
	UpdateFriction( step);

	
	for (wheelIt = m_wheelInfo.begin();
	!(wheelIt == m_wheelInfo.end());wheelIt++)
	{
		WheelInfo& wheel = *wheelIt;
		SimdVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - GetRigidBody()->getCenterOfMassPosition();
		SimdVector3 vel = GetRigidBody()->getVelocityInLocalPoint( relpos );

		if (wheel.m_raycastInfo.m_isInContact)
		{
			SimdVector3 fwd (
				GetRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis],
				GetRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis],
				GetRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]);

			SimdScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
			fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;

			SimdScalar proj2 = fwd.dot(vel);
			
			wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
			wheel.m_rotation += wheel.m_deltaRotation;

		} else
		{
			wheel.m_rotation += wheel.m_deltaRotation;
		}
		
		wheel.m_deltaRotation *= 0.99f;//damping of rotation when not in contact

	}



}


void	RaycastVehicle::SetSteeringValue(SimdScalar steering,int wheel)
{
	assert(wheel>=0 && wheel < GetNumWheels());

	WheelInfo& wheelInfo = GetWheelInfo(wheel);
	wheelInfo.m_steering = steering;
}



SimdScalar	RaycastVehicle::GetSteeringValue(int wheel) const
{
	return GetWheelInfo(wheel).m_steering;
}


void	RaycastVehicle::ApplyEngineForce(SimdScalar force, int wheel)
{
	for (int i=0;i<GetNumWheels();i++)
	{
		WheelInfo& wheelInfo = GetWheelInfo(i);

		bool applyOnFrontWheel = !wheel;

		if (applyOnFrontWheel == wheelInfo.m_bIsFrontWheel)
		{
			wheelInfo.m_engineForce = force;
		}
	}
}


const WheelInfo&	RaycastVehicle::GetWheelInfo(int index) const
{
	ASSERT((index >= 0) && (index < 	GetNumWheels()));
	
	return m_wheelInfo[index];
}

WheelInfo&	RaycastVehicle::GetWheelInfo(int index) 
{
	ASSERT((index >= 0) && (index < 	GetNumWheels()));
	
	return m_wheelInfo[index];
}

void RaycastVehicle::SetBrake(float brake,int wheelIndex)
{
	ASSERT((wheelIndex >= 0) && (wheelIndex < 	GetNumWheels()));
	GetWheelInfo(wheelIndex).m_brake;
}


void	RaycastVehicle::UpdateSuspension(SimdScalar deltaTime)
{

	SimdScalar chassisMass = 1.f / m_chassisBody->getInvMass();
	
	for (int w_it=0; w_it<GetNumWheels(); w_it++)
	{
		WheelInfo &wheel_info = m_wheelInfo[w_it];
		
		if ( wheel_info.m_raycastInfo.m_isInContact )
		{
			SimdScalar force;
			//	Spring
			{
				SimdScalar	susp_length			= wheel_info.GetSuspensionRestLength();
				SimdScalar	current_length = wheel_info.m_raycastInfo.m_suspensionLength;

				SimdScalar length_diff = (susp_length - current_length);

				force = wheel_info.m_suspensionStiffness
					* length_diff * wheel_info.m_clippedInvContactDotSuspension;
			}
		
			// Damper
			{
				SimdScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
				{
					SimdScalar	susp_damping;
					if ( projected_rel_vel < 0.0f )
					{
						susp_damping = wheel_info.m_wheelsDampingCompression;
					}
					else
					{
						susp_damping = wheel_info.m_wheelsDampingRelaxation;
					}
					force -= susp_damping * projected_rel_vel;
				}
			}

			// RESULT
			wheel_info.m_wheelsSuspensionForce = force * chassisMass;
			if (wheel_info.m_wheelsSuspensionForce < 0.f)
			{
				wheel_info.m_wheelsSuspensionForce = 0.f;
			}
		}
		else
		{
			wheel_info.m_wheelsSuspensionForce = 0.0f;
		}
	}

}

float sideFrictionStiffness2 = 1.0f;
void	RaycastVehicle::UpdateFriction(SimdScalar	timeStep)
{

		//calculate the impulse, so that the wheels don't move sidewards
		int numWheel = GetNumWheels();
		if (!numWheel)
			return;


		SimdVector3*	forwardWS = new	SimdVector3[numWheel];
		SimdVector3*	axle = new SimdVector3[numWheel];
		SimdScalar* forwardImpulse = new SimdScalar[numWheel];
		SimdScalar* sideImpulse = new SimdScalar[numWheel];
		
		int numWheelsOnGround = 0;
	

		//collapse all those loops into one!
		for (int i=0;i<GetNumWheels();i++)
		{
			WheelInfo& wheelInfo = m_wheelInfo[i];
			class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
			if (groundObject)
				numWheelsOnGround++;
			sideImpulse[i] = 0.f;
			forwardImpulse[i] = 0.f;

		}
	
		{
	
			for (int i=0;i<GetNumWheels();i++)
			{

				WheelInfo& wheelInfo = m_wheelInfo[i];
					
				class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;

				if (groundObject)
				{

					const SimdTransform& wheelTrans = GetWheelTransformWS( i );

					SimdMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
					axle[i] = SimdVector3(	
						wheelBasis0[0][m_indexRightAxis],
						wheelBasis0[1][m_indexRightAxis],
						wheelBasis0[2][m_indexRightAxis]);
					
					const SimdVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
					SimdScalar proj = axle[i].dot(surfNormalWS);
					axle[i] -= surfNormalWS * proj;
					axle[i] = axle[i].normalize();
					
					forwardWS[i] = surfNormalWS.cross(axle[i]);
					forwardWS[i].normalize();

				
					resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
							  *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
							  0.f, axle[i],sideImpulse[i],timeStep);

					sideImpulse[i] *= sideFrictionStiffness2;
						
				}
				

			}
		}

	SimdScalar sideFactor = 1.f;
	SimdScalar fwdFactor = 0.5;

	bool sliding = false;
	{
		for (int wheel =0;wheel <GetNumWheels();wheel++)
		{
			WheelInfo& wheelInfo = m_wheelInfo[wheel];
			class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;


			forwardImpulse[wheel] = 0.f;
			m_wheelInfo[wheel].m_skidInfo= 1.f;

			if (groundObject)
			{
				m_wheelInfo[wheel].m_skidInfo= 1.f;
				
				SimdScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
				SimdScalar maximpSide = maximp;

				SimdScalar maximpSquared = maximp * maximpSide;

				forwardImpulse[wheel] = wheelInfo.m_engineForce* timeStep;

				float x = (forwardImpulse[wheel] ) * fwdFactor;
				float y = (sideImpulse[wheel] ) * sideFactor;
				
				float impulseSquared = (x*x + y*y);

				if (impulseSquared > maximpSquared)
				{
					sliding = true;
					
					SimdScalar factor = maximp / SimdSqrt(impulseSquared);
					
					m_wheelInfo[wheel].m_skidInfo *= factor;
				}
			} 

		}
	}

	


		if (sliding)
		{
			for (int wheel = 0;wheel < GetNumWheels(); wheel++)
			{
				if (sideImpulse[wheel] != 0.f)
				{
					if (m_wheelInfo[wheel].m_skidInfo< 1.f)
					{
						forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
						sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
					}
				}
			}
		}

		// apply the impulses
		{
			for (int wheel = 0;wheel<GetNumWheels() ; wheel++)
			{
				WheelInfo& wheelInfo = m_wheelInfo[wheel];

				SimdVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
						m_chassisBody->getCenterOfMassPosition();

				if (forwardImpulse[wheel] != 0.f)
				{
					m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos);
				}
				if (sideImpulse[wheel] != 0.f)
				{
					class RigidBody* groundObject = (class RigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;

					SimdVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - 
						groundObject->getCenterOfMassPosition();

					
					SimdVector3 sideImp = axle[wheel] * sideImpulse[wheel];

					rel_pos[2] *= wheelInfo.m_rollInfluence;
					m_chassisBody->applyImpulse(sideImp,rel_pos);

					//apply friction impulse on the ground
					groundObject->applyImpulse(-sideImp,rel_pos2);
				}
			}
		}

		delete []forwardWS;
		delete [] axle;
		delete[]forwardImpulse;
		delete[] sideImpulse;
}