#include "CPhysicsCamera.h"

/// Static objects to use in static functions.
NewtonWorld* CPhysicsCamera::m_world;
vector3df CPhysicsCamera::m_bodySize;
NewtonBody* CPhysicsCamera::m_ground;
vector3df CPhysicsCamera::m_camForce;
vector3df CPhysicsCamera::m_rayStart;
vector3df CPhysicsCamera::m_rayEnd;
NewtonBody* CPhysicsCamera::m_body;


CPhysicsCamera::CPhysicsCamera()
{

}

CPhysicsCamera::~CPhysicsCamera()
{
	if (m_body)
	{
		NewtonBodySetDestructorCallback( m_body, NULL );
		NewtonDestroyBody( m_world, m_body );
	}
}

/// Destructor callback
void _cdecl CPhysicsCamera::destructor( const NewtonBody* body )
{
	//newton wants to destroy the body.. so first find it.
	CPhysicsEntity* me;

	me = (CPhysicsEntity*)NewtonBodyGetUserData( body );

	// remove destructor callback
	NewtonBodySetDestructorCallback( body, NULL );
	// remove the user data
	NewtonBodySetUserData( body, NULL );

	//now delete the object.
	delete me;
}

/// Initialize physics for camera
void CPhysicsCamera::init(IrrlichtDevice* dev, NewtonWorld* world, SPhysicsCamera* camera)
{
	/// Set up local world and device variables.
	m_device = dev;
	m_world = world;
	
	/// Get properties from SPhysicsCamera struct.
	f32 mass = camera->mass;
	m_bodySize = camera->bodySize * IrrToNewton;
	m_sensitivity = camera->sensitivity;
	m_moveSpeed = camera->moveSpeed;
	m_node = camera->node;
	m_maxStepSize = 3;

	NewtonCollision* sphere = NewtonCreateSphere(m_world, m_bodySize.X, m_bodySize.Y, m_bodySize.Z, NULL);

	//NewtonCollision* collision = NewtonCreateConvexHullModifier(m_world, sphere);

	m_body = NewtonCreateBody(m_world, sphere);

	NewtonBodySetDestructorCallback(m_body, destructor);

	vector3df inertia;
	vector3df origin(0,0,0);

	NewtonBodySetMassMatrix (m_body, camera->mass, inertia.X, inertia.Y, inertia.Z);

	NewtonConvexCollisionCalculateInertialMatrix(sphere, &inertia.X, &origin.X);
	
	NewtonReleaseCollision (m_world, sphere);

	
	NewtonBodySetContinuousCollisionMode(m_body, 1);

	NewtonBodySetAutoFreeze (m_body, 0);

	NewtonWorldUnfreezeBody (m_world, m_body);

	vector3df damp(0,0,0);

	NewtonBodySetLinearDamping (m_body, 0.0f);
	NewtonBodySetAngularDamping (m_body, &damp.X);

	NewtonBodySetTransformCallback (m_body, this->camSetTransform);

	NewtonBodySetForceAndTorqueCallback (m_body, this->camApplyForce);
	
	NewtonBodySetUserData(m_body, m_node);

	vector3df upDirection (0.0f, 1.0f, 0.0f);
	upDirection *= IrrToNewton;

	NewtonJoint* m_upVector = NewtonConstraintCreateUpVector (m_world, &upDirection.X, m_body); 

	m_node->setPosition(vector3df(0,m_bodySize.Y*0.8,0));

	SCameraKeyMap* km = new SCameraKeyMap;
	km[0].key = KEY_UP;
	km[0].action = EA_MOVE_FORWARD;
	km[1].key = KEY_DOWN;
	km[1].action = EA_MOVE_BACKWARD;
	km[2].key = KEY_LEFT;
	km[2].action = EA_STRAFE_LEFT;
	km[3].key = KEY_RIGHT;
	km[3].action = EA_STRAFE_RIGHT;
	km[4].key = KEY_KEY_P;
	km[4].action = EA_JUMP;

	setKeyMap(km,4);

	m_firstUpdate = true;
}

/// On event callback for the camera.
void CPhysicsCamera::onEvent(SEvent event)
{

	s32 cnt = (s32)m_keyMap.size();

	for (s32 i=0; i<cnt;i++)
	{
		if (m_keyMap[i].key == event.KeyInput.Key)
		{
			m_cursorKeys[m_keyMap[i].action] = event.KeyInput.PressedDown;
		}
	}
}

/// Function to update the camera.
void CPhysicsCamera::update()
{
	if (m_firstUpdate)
	{
		if (m_device->getCursorControl())
			m_device->getCursorControl()->setPosition(0.5f, 0.5f);

		m_centerCursor = m_device->getCursorControl()->getRelativePosition();

		m_firstUpdate = false;
		return;
	} 

	vector3df rotation;
	vector3df movedir, target, strafevect, movevect;
	
	target.set(0,0,1);

	rotation = getRotation();

	rotation.X *= -1.0f;
	rotation.Y *= -1.0f;

	position2d<f32> cursorpos = m_device->getCursorControl()->getRelativePosition();

	if (!equals(cursorpos.X, m_centerCursor.X) ||
		!equals(cursorpos.Y, m_centerCursor.Y))
	{
		rotation.Y += (0.5f - cursorpos.X) * m_sensitivity;
		rotation.X += (0.5f - cursorpos.Y) * m_sensitivity;
		m_device->getCursorControl()->setPosition(0.5f, 0.5f);
		m_centerCursor = m_device->getCursorControl()->getRelativePosition();

		if (rotation.X > 89.0f)
			rotation.X = 89.0f;
		if (rotation.X < -89.0f)
			rotation.X = -89.0f;
	}

	matrix4 mat;
 	mat.setRotationDegrees(vector3df(-rotation.X, -rotation.Y, 0));
	mat.transformVect(target);

	movevect = m_node->getTarget() - m_node->getPosition();
	movevect.Y = 0;
	movevect *= InvPi;

	strafevect = target;
	strafevect.Y =0;
	strafevect = strafevect.crossProduct(vector3df(0,1,0));
	strafevect *= InvPi;

	vector3df goalVel(0,0,0);
	
	if(m_cursorKeys[0])
		goalVel += movevect;

	if(m_cursorKeys[1])
		goalVel -= movevect;

	if(m_cursorKeys[2])
		goalVel += strafevect;

	if(m_cursorKeys[3])
		goalVel -= strafevect;

/*	if(m_cursorKeys[4])
	{
		vector3df jumpVeloc(0,10,0);
		vector3df pos = getPosition();
		NewtonAddBodyImpulse(m_body, &jumpVeloc.X, &pos.X);
	}
*/	
	goalVel.setLength(m_moveSpeed);

	vector3df currentVel ;
	NewtonBodyGetVelocity(m_body, &currentVel.X);
	
	vector3df accel = ((goalVel-currentVel) / NewtonGetTimeStep(m_world) ) * 0.5f;

	accel.Y = 0;

	m_camForce = accel;

	vector3df forward = (goalVel).normalize() + vector3df(m_bodySize.X, 0, m_bodySize.Z);
	forward.Y = 0;

	m_rayStart = getPosition() - vector3df(0, m_bodySize.Y * NewtonToIrr,0) + forward;
	m_rayEnd = m_rayStart +vector3df(0,0.3f,0);
	m_rayStart.Y += m_maxStepSize;

	m_device->getVideoDriver()->draw3DLine(m_rayStart, m_rayEnd, SColor(255, 255, 0, 0)); 

	m_rayStart *= IrrToNewton;
	m_rayEnd *= IrrToNewton;

	f32 param = 1;
	NewtonWorldRayCast (m_world, &m_rayStart.X, &m_rayEnd.X, OnRayCastPlacement, &param, NULL);

	rotation.X *= -1.0f;
	rotation.Y *= -1.0f;

	m_node->setRotation(rotation);
	m_node->setTarget(m_node->getPosition() + target);
}

/// A static callback which is called when a ray cast hits something.
f32 CPhysicsCamera::OnRayCastPlacement (const NewtonBody* body, const dFloat* normal, int collisionID, void* userData, dFloat intersetParam)
{
	vector3df i =  m_rayStart - ((m_rayEnd-m_rayStart)*intersetParam);

	matrix4 matrix;
	NewtonBodyGetMatrix(m_body, matrix.pointer());
	matrix.setTranslation(i + vector3df(0, m_bodySize.Y, 0));
	NewtonBodySetMatrix(m_body, matrix.pointer());

	return 1;
}

/// To check in a raycast if it is the floor.
f32 CPhysicsCamera::OnFloorCheckRayCast (const NewtonBody* body, const dFloat* normal, int collisionID, void* userData, dFloat intersetParam)
{
	// any body can be a floor
	if (body == m_ground) 
	{
		return 1;
	}

	return 0;
}

void CPhysicsCamera::setMaterial(int material)
{
	NewtonBodySetMaterialGroupID (m_body, material);
}

void CPhysicsCamera::setGround(NewtonBody* grnd)
{
	m_ground = grnd;
}
// set position for the physics body and graphic node
void CPhysicsCamera::setPosition(vector3df position)
{
	matrix4 matrix;
	NewtonBodyGetMatrix(m_body, matrix.pointer());
	matrix.setTranslation(position * IrrToNewton);
	NewtonBodySetMatrix(m_body, matrix.pointer());

	m_node->setPosition(position);
}

// set rotation for the physics body and graphic node
void CPhysicsCamera::setRotation(vector3df rotation)
{
	m_node->setRotation(rotation);
}

vector3df CPhysicsCamera::getPosition()
{
	return m_node->getPosition();
}

vector3df CPhysicsCamera::getRotation()
{
	return m_node->getRotation();
}

NewtonBody* CPhysicsCamera::getBody()
{
	return m_body;
}

ICameraSceneNode *CPhysicsCamera::getCameraNode()
{
	return m_node;
}

void CPhysicsCamera::setKeyMap(SCameraKeyMap* kMap, s32 kMapSize)
{
	m_keyMap.clear();

	if (kMap && kMapSize)
	{
		for (s32 i=0; i<kMapSize; ++i)
		{
			switch(kMap[i].action)
			{
			case EA_MOVE_FORWARD: m_keyMap.push_back(SCameraKeyMap(EA_MOVE_FORWARD, kMap[i].key));
				break;
			case EA_MOVE_BACKWARD: m_keyMap.push_back(SCameraKeyMap(EA_MOVE_BACKWARD, kMap[i].key));
				break;
			case EA_STRAFE_LEFT: m_keyMap.push_back(SCameraKeyMap(EA_STRAFE_LEFT, kMap[i].key));
				break;
			case EA_STRAFE_RIGHT: m_keyMap.push_back(SCameraKeyMap(EA_STRAFE_RIGHT, kMap[i].key));
				break;
			default:
				break;
			} // end switch
		} // end for
	}
}


void CPhysicsCamera::camSetTransform(const NewtonBody* body, const float* matrix)
{
    matrix4 mat;
    memcpy(mat.pointer(), matrix, sizeof(float)*16);

	// get the graphic object form the rigid body
	ICameraSceneNode *cam = (ICameraSceneNode *) NewtonBodyGetUserData(body);
    
	cam->setPosition(mat.getTranslation() * NewtonToIrr);
}

void CPhysicsCamera::camApplyForce (const NewtonBody* body) 
{
	f32 mass, Ixx, Iyy, Izz; 
	f32 floor, accelY, deltaHeight; 
	vector3df velocity;

	// get the character mass
	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);

	// Get the current world timestep
	f32 timestep = NewtonGetTimeStep(m_world);
	f32 timestepInv = 1.0f / timestep;

	// apply the gravity force, cheat a little with the character gravity
	vector3df force (0.0f, -9.8f * mass, 0.0f);  
	force += m_camForce;

	// Get the velocity vector
	NewtonBodyGetVelocity(body, &velocity.X);

	matrix4 matrix;
	NewtonBodyGetMatrix(body, matrix.pointer());

	m_rayStart = matrix.getTranslation() - vector3df(0, m_bodySize.Y,0);
	m_rayEnd = m_rayStart + vector3df(0,0.3f,0);
	m_rayStart.Y += 3;

	f32 param = 1;
	NewtonWorldRayCast (m_world, &m_rayStart.X, &m_rayEnd.X, OnFloorCheckRayCast, &param, NULL);

	floor = m_rayStart.Y - 3 * param;
	
	deltaHeight = (matrix.getTranslation().Y - m_bodySize.Y) - floor;
	if ((deltaHeight < (0.25f - 0.001f)) && (deltaHeight > 0.01f))
	{
		// snap to floor only if the floor is lower than the character feet		
		accelY = - (deltaHeight * timestepInv + velocity.Y) * timestepInv;
		force.Y = mass * accelY;
	}

	//cout << camForce.X << " " << camForce.Y << " " << camForce.Z << endl;

	NewtonBodySetForce(body, &force.X); 
}


