

#include "CPhysicsCar.h"



void _cdecl PhysicsCarSetTransform(const NewtonBody *body, const float *matrix)
{
    matrix4 mat;
    memcpy(mat.pointer(), matrix, sizeof(float)*16);

	// dedicated car callback - this is a safe cast
    CPhysicsCar *car = (CPhysicsCar *)NewtonBodyGetUserData(body);

	// read the velocity into a buffer
	f32 bodyVelocityBuffer[3];
	
	NewtonBodyGetVelocity(body, bodyVelocityBuffer);

	// read into an Irrlicht vector
	vector3df bodyVelocity(bodyVelocityBuffer[0],bodyVelocityBuffer[1],bodyVelocityBuffer[2]);

	car->setVehicleSpeed((f32)bodyVelocity.getLength());

	// move the graphical node
	car->getNode()->setPosition(mat.getTranslation() * NewtonToIrr);
    car->getNode()->setRotation(mat.getRotationDegrees());


    // set transform for the tyres
    void *wheelid = 0;
    for(wheelid = NewtonVehicleGetFirstTireID(car->getVehicleJoint()); wheelid; wheelid = NewtonVehicleGetNextTireID(car->getVehicleJoint(), wheelid))
    {
        CPhysicsCarWheel *wheel = (CPhysicsCarWheel *)NewtonVehicleGetTireUserData(car->getVehicleJoint(), wheelid);
        NewtonVehicleGetTireMatrix(car->getVehicleJoint(), wheelid, mat.pointer());

        wheel->getNode()->setPosition(mat.getTranslation() * NewtonToIrr);

        wheel->getNode()->setRotation(mat.getRotationDegrees());
    }
}


CPhysicsCar::CPhysicsCar()
{
}

CPhysicsCar::~CPhysicsCar()
{
	if (m_vehicleJoint)
	{
		NewtonJointSetDestructor( m_vehicleJoint, NULL );
		NewtonDestroyJoint( m_world, m_vehicleJoint );
	}

	if (m_carBody)
	{	
		NewtonBodySetUserData( m_carBody, NULL );

		NewtonBodySetDestructorCallback( m_carBody, NULL );
		NewtonDestroyBody( m_world, m_carBody );
	}
}



void CPhysicsCar::setSteeringPercent(f32 steeringPercent)
{
	// cap at + or - 100
	cap(100.0f, &steeringPercent, true);

	steeringPercent /= 100.0f;

	setSteering(steeringPercent * m_maxSteerAngle);
}

void CPhysicsCar::setThrottlePercent(f32 throttlePercent)
{
	// cap at + or - 100
	cap(100.0f, &throttlePercent, true);

	throttlePercent /= 100.0f;

	setTorque(throttlePercent * m_maxTorque);
}

void CPhysicsCar::setBrakesPercent(f32 brakesPercent)
{
	cap(100.0f, &brakesPercent, true);

	brakesPercent /= 100.0f;

	setBrakes(brakesPercent * m_maxBrakes);
}

void CPhysicsCar::setBrakes(f32 brakes)
{
	wheel_FL.setBrakes(brakes);
	wheel_FR.setBrakes(brakes);
}

// only used by callback - shouldn't be used externally
void CPhysicsCar::setVehicleSpeed(f32 speed)
{
	m_vehicleSpeed = speed;
}

ISceneNode* CPhysicsCar::getNode()
{
	return m_carNode;
}

NewtonJoint* CPhysicsCar::getVehicleJoint()
{
	return m_vehicleJoint;
}

// interface uses this to set values for Newton - externally use setSteeringPercent()
void CPhysicsCar::setSteering(f32 steerangle)
{
	wheel_FL.setSteer(steerangle);
	wheel_FR.setSteer(steerangle);
}

// interface uses this to set values for Newton - externally use setThrottlePercent()
void CPhysicsCar::setTorque(f32 torque)
{
	wheel_FL.setTorque(torque);
	wheel_FR.setTorque(torque);
	wheel_RL.setTorque(torque);
	wheel_RR.setTorque(torque);
}

// setup function
void CPhysicsCar::assembleCarPhysics(SPhysicsCar* car, NewtonWorld* world)
{
	m_maxSteerAngle = car->maxSteerAngle;
	m_maxTorque = car->maxTorque;
	m_maxBrakes = car->maxBrakes;
	m_world = world;

	car->tireRadius *= IrrToNewton;
	car->tireWidth *= IrrToNewton;
	car->axleWidth *= IrrToNewton;
	car->frontAxleOffset *= IrrToNewton;
	car->rearAxleOffset *= IrrToNewton;

	// create body offset matrix
    matrix4 offsetMatrix;

	offsetMatrix.setTranslation(car->carBodyOffset * IrrToNewton);
	
	car->carBodySize *= IrrToNewton;
	NewtonCollision* carCollision = NewtonCreateBox(world, car->carBodySize.X, car->carBodySize.Y, car->carBodySize.Z, offsetMatrix.pointer());
    m_carBody = NewtonCreateBody(world, carCollision);

    NewtonBodySetUserData(m_carBody, this);

	f32 origin[3];
	f32 inertia[3];

	// calculate the moment of inertia and the relative center of mass of the solid
	// without this your car will spin controllably and the wheels will probably break and fly off
	NewtonConvexCollisionCalculateInertialMatrix (carCollision, &inertia[0], &origin[0]);	
	
    NewtonReleaseCollision(world, carCollision);

	f32 inertia_x = car->carMass * inertia[0];
	f32 inertia_y = car->carMass * inertia[1];
	f32 inertia_z = car->carMass * inertia[2];

	// set the mass matrix
	NewtonBodySetMassMatrix (m_carBody, car->carMass, inertia_x, inertia_y, inertia_z);

	// set callbacks
    NewtonBodySetTransformCallback(m_carBody, PhysicsCarSetTransform);

    NewtonBodySetForceAndTorqueCallback(m_carBody, physicsApplyGravity);

	// y is up
	f32 upDirection[3];
	upDirection[0] = 0.0f;
	upDirection[1] = 1.0f;
	upDirection[2] = 0.0f;
	m_vehicleJoint = NewtonConstraintCreateVehicle (world, &upDirection[0], m_carBody);

    NewtonVehicleSetTireCallback(m_vehicleJoint, PhysicsCarWheelUpdate);

    // here we set up the car's wheels, determining their positions using the axleOffset and axleWidth
	// axleWidth is the width of the whole car, therefore offset from centre is half that
	f32 axleOffset = car->axleWidth / 2.0f;

	// the front of the car needs to point in the negative x axis direction,
	// otherwise positive torque makes it go backwards, which is much more confusing
	// also, as +x is forwards, -z (closer to us) should be the left wheels, +z (further from us) the right wheels
	vector3df wheelPosition_FL(car->frontAxleOffset * -1.0f, 0.0f,	-axleOffset);
	vector3df wheelPosition_FR(car->frontAxleOffset * -1.0f, 0.0f,	axleOffset);
	vector3df wheelPosition_RL(car->rearAxleOffset,			0.0f,	-axleOffset);
	vector3df wheelPosition_RR(car->rearAxleOffset,			0.0f,	axleOffset);

	// call each wheel's setup routine
	wheel_FL.rigupPhysics(m_vehicleJoint, wheelPosition_FL, car, &wheel_FL, 1);
    wheel_FR.rigupPhysics(m_vehicleJoint, wheelPosition_FR, car, &wheel_FR, 2);
    wheel_RL.rigupPhysics(m_vehicleJoint, wheelPosition_RL, car, &wheel_RL, 3);
    wheel_RR.rigupPhysics(m_vehicleJoint, wheelPosition_RR, car, &wheel_RR, 4);

	// wire up pointers
	m_carNode = car->carBodyNode;
	wheel_FL.setNode(car->tireNode_FL);
	wheel_FR.setNode(car->tireNode_FR);
	wheel_RL.setNode(car->tireNode_RL);
	wheel_RR.setNode(car->tireNode_RR);

	// disable auto freeze - we always want this to be updated
    NewtonBodySetAutoFreeze(m_carBody, 0);

	// set minimum linear damping
	NewtonBodySetLinearDamping(m_carBody, 0);
}


// interface functions
vector3df CPhysicsCar::getPosition()
{
	return m_carNode->getPosition();
}

void CPhysicsCar::setPosition(vector3df position)
{
	matrix4 matrix;
	NewtonBodyGetMatrix(m_carBody, matrix.pointer());
	matrix.setTranslation(position * IrrToNewton);
	NewtonBodySetMatrix(m_carBody, matrix.pointer());

	m_carNode->setPosition(position);
}


vector3df CPhysicsCar::getRotation()
{
	return m_carNode->getRotation();
}

void CPhysicsCar::setRotation(vector3df rotation)
{
	matrix4 matrix;
	NewtonBodyGetMatrix(m_carBody, matrix.pointer());
	matrix.setRotationDegrees(rotation);
	NewtonBodySetMatrix(m_carBody, matrix.pointer());

	m_carNode->setRotation(rotation);
}

void CPhysicsCar::setScale(vector3df scale)
{
	matrix4 matrix;
	NewtonBodyGetMatrix(m_carBody, matrix.pointer());
	matrix.setScale(scale);
	NewtonBodySetMatrix(m_carBody, matrix.pointer());

	m_carNode->setRotation(scale);
}

vector3df CPhysicsCar::getScale()
{
	return m_carNode->getScale();
}


NewtonBody* CPhysicsCar::getBody()
{
	return m_carBody;
}

// cap the target variable to the maxMagnitude
// if positiveAndNegative is true, target will be capped at -maxMagnitude <= target <= maxMagnitude
void CPhysicsCar::cap(f32 maxMagnitude, f32* target, bool positiveAndNegative)
{
	if(*target > maxMagnitude)
	{
		*target = maxMagnitude;
	}
	else if(*target < -maxMagnitude)
	{
		*target = -maxMagnitude;
	}
}
