// ModPhysics.h

#pragma once

#include "Module.h"

class btRigidBody;
class btPoint2PointConstraint;
class btHingeConstraint;
class btCompoundShape;
class btCollisionShape;

class ModPhysics: public Module {
public:
    ModPhysics();
    ~ModPhysics();
	
	void InitModule();
	
	void SaveShapes(vector <dShape> &shapesData);
	void LoadShapes(vector <dShape> &shapesData);
	
	void SetProperty(string name, bool value);
	void SetProperty(string name, int value);
	void SetProperty(string name, float value);
	void SetProperty(string name, string value);
	void SetProperty(string name, Vector value);
	void SetProperty(string name, SGColor value);
	void SetProperty(string name, Path value);
	void SetProperty(string name, pEnumID value);
	
	void	SetSleeper(bool sleeperArg);
	bool	GetSleeper();
	void	SetGravity(float gravityArg);
	float	GetGravity();
	void	SetMass(float massArg);
	float	GetMass();
	void	SetFriction(float frictionArg);
	float	GetFriction();
	void	SetRestitution(float restitutionArg);
	float	GetRestitution();
	void	SetLinearDampening(float linearDampeningArg);
	float	GetLinearDampening();
	void	SetAngularDampening(float angularDampeningArg);
	float	GetAngularDampening();
	
	void	Impulse(Vector force, Vector pos);
	void	SetForce(Vector force);
	void	AddForce(Vector force);
	Vector	GetForce();
	void	SetVelocity(Vector velocity);
	Vector	GetVelocity();
	void	SetTorque(Vector torque);
	void	AddTorque(Vector torque);
	Vector	GetTorque();
	void	SetOmega(Vector omega);
	Vector	GetOmega();
	
	void SetTrigger(bool triggerArg);
	bool GetTrigger();
	
	int AddBox(Matrix& matrix, float width, float length, float height);
	int	AddSphere(Matrix& matrix, float radius);
	int	AddCapsule(Matrix& matrix, float radius, float height);
	int	AddCylinder(Matrix& matrix, float radius, float height);
	int	AddCone(Matrix& matrix, float radius, float height);
	int GetNumShapes();
	btCollisionShape* GetShape(int index);
	Matrix GetShapeMatrix(int index);
	void UpdateShapeMatrix(int index, Matrix& matrix);
	void RemoveShape(btCollisionShape* colShape);
	void RemoveShapeByIndex(int index);
	void RemoveAllShapes();
	
	void SetCollisionScriptEventEnabled(bool enabled);
	vector <Entity*> GetCollisions();
	
	btPoint2PointConstraint*	ConnectBallsocket(Entity* entity, Vector pivot);
	btHingeConstraint*			ConnectHinge(Entity* entity, Vector pivot, Vector axis);
	
	btRigidBody* GetDBody();
	void UpdateMatrix();
	void Wake();
	
private:
	int AddShape(Matrix& matrix, btCollisionShape* colShape);
	
	btRigidBody* body;
	
	float	gravity;
	bool	sleeper;
	float	mass;
	float	friction;
	float	restitution;
	float	linearDampening;
	float	angularDampening;
	
	bool trigger;
	
	bool dynamic;
	
	btCompoundShape* compColShape;
};

// ModPhysics.cpp

#include "ModPhysics.h"
#include "../Entity.h"

#include "../../Library/Library.h"

#include "btBulletDynamicsCommon.h"

ModPhysics::ModPhysics()
: body(NULL),
sleeper(false), gravity(-9.8f), mass(0), friction(0.5f), restitution(0), linearDampening(0), angularDampening(0), trigger(false), compColShape(NULL) {
	moduleType = MODULE_PHYSICS;
	
	NewProperty(pProperty("Sleeper",			pBool(&sleeper)));
	NewProperty(pProperty("Gravity",			pFloat(&gravity, 0, -10)));
	NewProperty(pProperty("Mass",				pFloat(&mass, 0, 100)));
	NewProperty(pProperty("Friction",			pFloat(&friction, 0, 1)));
	NewProperty(pProperty("Restitution",		pFloat(&restitution, 0, 1)));
	NewProperty(pProperty("Linear Dampening",	pFloat(&linearDampening, 0, 1)));
	NewProperty(pProperty("Angular Dampening",	pFloat(&angularDampening, 0, 1)));
	
	compColShape = new btCompoundShape(false);
}

ModPhysics::~ModPhysics() {
	if(body) {
		body->setUserPointer(NULL); //Kinda strange..
		
		dWorld->removeRigidBody(body); //It is removed whether you call this or not.. strange too..
		
		/*if(body->getMotionState())
			delete body->getMotionState();*/
		
		//delete body;
	}
	
	if(compColShape)
		delete compColShape;
}

bool SolContactAddedCallback(btManifoldPoint& cp, const btCollisionObject* colObj0, int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1) {
	Entity* entity0 = NULL;
	Entity* entity1 = NULL;
	
	if(colObj0->getUserPointer())
		entity0 = ((ModPhysics*)colObj0->getUserPointer())->GetOwner();
	
	if(colObj1->getUserPointer())
		entity1 = ((ModPhysics*)colObj1->getUserPointer())->GetOwner();
	
	if(entity0 && colObj0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK //Needs to have callback
	&& !(colObj1->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE)) { //Other body needs to respond to collision
		if(entity0->GetScriptsModule())
			entity0->GetScriptsModule()->EventCollide(entity1);
	}
	
	if(entity1 && colObj1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK
	&& !(colObj0->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE)) {
		if(entity1->GetScriptsModule())
			entity1->GetScriptsModule()->EventCollide(entity0);
	}
	
	return 0;
}
ContactAddedCallback gContactAddedCallback = SolContactAddedCallback;

void ModPhysics::InitModule() {
	btTransform startTransform;
	startTransform.setFromOpenGLMatrix(&owner->GetMatrix()[0][0]);
	
	dynamic = (mass != 0);
	btVector3 localInertia(0, 0, 0);
	if(dynamic)
		compColShape->calculateLocalInertia(mass, localInertia);
	
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, compColShape, localInertia);
	body = new btRigidBody(rbInfo);
	body->setUserPointer(this);
	dWorld->addRigidBody(body);
}

void ModPhysics::SaveShapes(vector <dShape> &shapesData) {
	shapesData.clear();
	
	dShape shapeData;
	
	int numShapes = GetNumShapes();
	for(int shape = 0; shape < numShapes; ++shape) {
		btCollisionShape* colShape = GetShape(shape);
		
		shapeData.matrix = GetShapeMatrix(shape);
		
		switch(colShape->getShapeType()) {
		case BOX_SHAPE_PROXYTYPE: {
			btBoxShape* realShape = (btBoxShape*)colShape;
			
			shapeData.type = SHAPE_BOX;
			shapeData.dimensions = Vector(realShape->getHalfExtentsWithMargin()).Scale(2);
			}break;
		case SPHERE_SHAPE_PROXYTYPE: {
			btSphereShape* realShape = (btSphereShape*)colShape;
			
			shapeData.type = SHAPE_SPHERE;
			shapeData.radius = realShape->getRadius();
			}break;
		case CAPSULE_SHAPE_PROXYTYPE: {
			btCapsuleShape* realShape = (btCapsuleShape*)colShape;
			
			shapeData.type = SHAPE_CAPSULE;
			shapeData.radius = realShape->getRadius();
			shapeData.height = realShape->getHalfHeight() * 2;
			}break;
		case CYLINDER_SHAPE_PROXYTYPE: {
			btCylinderShape* realShape = (btCylinderShape*)colShape;
			
			shapeData.type = SHAPE_CYLINDER;
			shapeData.radius = realShape->getRadius();
			shapeData.height = realShape->getHalfExtentsWithMargin().y() * 2;
			}break;
		case CONE_SHAPE_PROXYTYPE: {
			btConeShape* realShape = (btConeShape*)colShape;
			
			shapeData.type = SHAPE_CONE;
			shapeData.radius = realShape->getRadius();
			shapeData.height = realShape->getHeight();
			}break;
		}
		
		shapesData.push_back(shapeData);
	}
}

void ModPhysics::LoadShapes(vector <dShape> &shapesData) {
	if(owner->GetName() == "Sickle Pellet") {
		AddCylinder(GetIdentityMatrix(), 1, 1);
		return;
	}
	
	vector <dShape>::iterator shapeDataIt;
	for(shapeDataIt = shapesData.begin(); shapeDataIt != shapesData.end(); ++shapeDataIt) {
		switch(shapeDataIt->type) {
		case SHAPE_BOX: {
			AddBox(shapeDataIt->matrix, shapeDataIt->dimensions.x, shapeDataIt->dimensions.y, shapeDataIt->dimensions.z);
			}break;
		case SHAPE_SPHERE: {
			AddSphere(shapeDataIt->matrix, shapeDataIt->radius);
			}break;
		case SHAPE_CAPSULE: {
			AddCapsule(shapeDataIt->matrix, shapeDataIt->radius, shapeDataIt->height);
			}break;
		case SHAPE_CYLINDER: {
			AddCylinder(shapeDataIt->matrix, shapeDataIt->radius, shapeDataIt->height);
			}break;
		case SHAPE_CONE: {
			AddCone(shapeDataIt->matrix, shapeDataIt->radius, shapeDataIt->height);
			}break;
		}
	}
}

void ModPhysics::SetProperty(string name, bool value) {
	if(name == "Sleeper")
		SetSleeper(value);
}

void ModPhysics::SetProperty(string name, int value) {
}

void ModPhysics::SetProperty(string name, float value) {
	if(name == "Gravity")
		SetGravity(value);
	else
	if(name == "Mass")
		SetMass(value);
	else
	if(name == "Friction")
		SetFriction(value);
	else
	if(name == "Linear Dampening")
		SetLinearDampening(value);
	else
	if(name == "Angular Dampening")
		SetAngularDampening(value);
}

void ModPhysics::SetProperty(string name, string value) {
}

void ModPhysics::SetProperty(string name, Vector value) {
}

void ModPhysics::SetProperty(string name, SGColor value) {
}

void ModPhysics::SetProperty(string name, Path value) {
}

void ModPhysics::SetProperty(string name, pEnumID value) {
}

void ModPhysics::SetSleeper(bool sleeperArg) {
	sleeper = sleeperArg;

	if(sleeper)
		body->setActivationState(ACTIVE_TAG);
	else
		body->setActivationState(DISABLE_DEACTIVATION);
}

bool ModPhysics::GetSleeper() {
	return sleeper;
}

void ModPhysics::SetGravity(float gravityArg) {
	gravity = gravityArg;
	body->setGravity(btVector3(0, 0, gravity));
}

float ModPhysics::GetGravity() {
	return gravity;
}

void ModPhysics::SetMass(float massArg) {
	mass = massArg;
	
	bool dynamicOld = dynamic;
	dynamic = mass != 0;
	
	btVector3 localInertia(0,0,0);
	if(dynamic) {
		compColShape->calculateLocalInertia(mass, localInertia);
		
		if(compColShape->getNumChildShapes() == 1)
			GetShape(0)->calculateLocalInertia(mass, localInertia);
	}
	
	body->setMassProps(mass, localInertia);
	
	if(dynamic != dynamicOld) {
		dWorld->removeRigidBody(body);
		dWorld->addRigidBody(body);
	}
}

float ModPhysics::GetMass() {
	return mass;
}

void ModPhysics::SetFriction(float frictionArg) {
	friction = frictionArg;
	body->setFriction(friction);
}

float ModPhysics::GetFriction() {
	return friction;
}

void ModPhysics::SetRestitution(float restitutionArg) {
	restitution = restitutionArg;
	body->setRestitution(restitution);
}

float ModPhysics::GetRestitution() {
	return restitution;
}

void ModPhysics::SetLinearDampening(float linearDampeningArg) {
	linearDampening = linearDampeningArg;
	body->setDamping(linearDampening, angularDampening);
}

float ModPhysics::GetLinearDampening() {
	return linearDampening;
}

void ModPhysics::SetAngularDampening(float angularDampeningArg) {
	angularDampening = angularDampeningArg;
	body->setDamping(linearDampening, angularDampening);
}

float ModPhysics::GetAngularDampening() {
	return angularDampening;
}

void ModPhysics::Impulse(Vector force, Vector pos) {
	body->applyImpulse(force.btV(), pos.btV());
}

void ModPhysics::SetForce(Vector force) {
	body->setForce(force.btV());
}

void ModPhysics::AddForce(Vector force) {
	body->applyCentralForce(force.btV());
}

Vector ModPhysics::GetForce() {
	return body->getForce();
}

void ModPhysics::SetVelocity(Vector velocity) {
	body->setLinearVelocity(btVector3(velocity.x, velocity.y, velocity.z));
}

Vector ModPhysics::GetVelocity() {
	return body->getLinearVelocity();
}

void ModPhysics::SetTorque(Vector torque) {
	body->setTorque(torque.btV());
}

void ModPhysics::AddTorque(Vector torque) {
	body->applyTorque(torque.btV());
}

Vector ModPhysics::GetTorque() {
	return body->getTorque();
}

void ModPhysics::SetOmega(Vector omega) {
	body->setAngularVelocity(btVector3(omega.x, omega.y, omega.z));
}

Vector ModPhysics::GetOmega() {
	return body->getAngularVelocity();
}

void ModPhysics::SetTrigger(bool triggerArg) {
	trigger = triggerArg;
	
	if(trigger)
		body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
	else
		body->setCollisionFlags(body->getCollisionFlags() & ~btCollisionObject::CF_NO_CONTACT_RESPONSE);
}

bool ModPhysics::GetTrigger() {
	return trigger;
}

int ModPhysics::AddShape(Matrix& matrix, btCollisionShape* colShape) {
	btTransform shapeTransform;
	shapeTransform.setFromOpenGLMatrix(&matrix[0][0]);
	
	compColShape->addChildShape(shapeTransform, colShape);
	
	if(compColShape->getNumChildShapes() == 1) //If there's only 1 shape it will be directly linked to the body
		body->setCollisionShape(GetShape(0));
	else
	if(compColShape->getNumChildShapes() == 2) //No point in setting this when it is higher than 2, cuz it was already set
		body->setCollisionShape(compColShape);
	
	SetMass(mass); //Recalculate inertia and center of mass matrices
	
	return compColShape->getNumChildShapes() - 1;
}

int ModPhysics::AddBox(Matrix& matrix, float width, float length, float height) {
	return AddShape(matrix, new btBoxShape(btVector3(width/2, length/2, height/2)));
}

int ModPhysics::AddSphere(Matrix& matrix, float radius) {
	return AddShape(matrix, new btSphereShape(radius));
}

int ModPhysics::AddCapsule(Matrix& matrix, float radius, float height) {
	return AddShape(matrix, new btCapsuleShape(radius, height));
}

int ModPhysics::AddCylinder(Matrix& matrix, float radius, float height) {
	return AddShape(matrix, new btCylinderShape(btVector3(radius, height/2, radius)));
}

int ModPhysics::AddCone(Matrix& matrix, float radius, float height) {
	return AddShape(matrix, new btConeShape(radius, height));
}

int ModPhysics::GetNumShapes() {
	return compColShape->getNumChildShapes();
}

btCollisionShape* ModPhysics::GetShape(int index) {
	assert(index >= 0 && index < compColShape->getNumChildShapes());
	
	return compColShape->getChildShape(index);
}

Matrix ModPhysics::GetShapeMatrix(int index) {
	assert(index >= 0 && index < compColShape->getNumChildShapes());

	btTransform shapeTransform = compColShape->getChildTransform(index);
	
	Matrix shapeMatrix;
	shapeTransform.getOpenGLMatrix(&shapeMatrix[0][0]);
	
	return shapeMatrix;
}

void ModPhysics::UpdateShapeMatrix(int index, Matrix &matrix) {
	assert(index >= 0 && index < compColShape->getNumChildShapes());
	
	btTransform shapeTransform;
	shapeTransform.setFromOpenGLMatrix(&matrix[0][0]);
	
	compColShape->updateChildTransform(index, shapeTransform);
	
	SetMass(mass); //Recalculate inertia and center of mass matrices
}

void ModPhysics::RemoveShape(btCollisionShape* colShape) {
	compColShape->removeChildShape(colShape);
	
	if(compColShape->getNumChildShapes() == 0) //Only way to have 0 shapes is by having compColShape as colshape
		body->setCollisionShape(compColShape);
	else
	if(compColShape->getNumChildShapes() == 1) //If there's only 1 shape it will be directly linked to the body
		body->setCollisionShape(GetShape(0));
	
	SetMass(mass); //Recalculate inertia and center of mass matrices
}

void ModPhysics::RemoveShapeByIndex(int index) {
	assert(index >= 0 && index < compColShape->getNumChildShapes());
	
	compColShape->removeChildShapeByIndex(index);
	
	if(compColShape->getNumChildShapes() == 0) //Only way to have 0 shapes is by having compColShape as colshape
		body->setCollisionShape(compColShape);
	else
	if(compColShape->getNumChildShapes() == 1) //If there's only 1 shape it will be directly linked to the body
		body->setCollisionShape(GetShape(0));
	
	SetMass(mass); //Recalculate inertia and center of mass matrices
}

void ModPhysics::RemoveAllShapes() {
	while(compColShape->getNumChildShapes() > 0) {
		compColShape->removeChildShapeByIndex(compColShape->getNumChildShapes()-1);
	}
	
	body->setCollisionShape(compColShape);
	
	SetMass(mass); //Recalculate inertia and center of mass matrices
}

void ModPhysics::SetCollisionScriptEventEnabled(bool enabled) {
	if(enabled)
		body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
	else
		body->setCollisionFlags(body->getCollisionFlags() & ~btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
}

vector <Entity*> ModPhysics::GetCollisions() {
	vector <Entity*> colliders;

	return colliders;
}

btPoint2PointConstraint* ModPhysics::ConnectBallsocket(Entity* entity, Vector pivot) {
	Vector myLocalPivot = owner->GetMatrix().UntransformVector(pivot);
	
	btPoint2PointConstraint* p2p;
	if(entity) {
		if(!entity->GetPhysicsModule())
			return NULL;
		
		Vector hisLocalPivot = entity->GetMatrix().UntransformVector(pivot);

		p2p = new btPoint2PointConstraint(*body, *entity->GetPhysicsModule()->GetDBody(), myLocalPivot.btV(), hisLocalPivot.btV());
	}else
		p2p = new btPoint2PointConstraint(*body, myLocalPivot.btV());
	
	dWorld->addConstraint(p2p);
	
	return p2p;
}

btHingeConstraint* ModPhysics::ConnectHinge(Entity* entity, Vector pivot, Vector axis) {
	Vector myLocalPivot = owner->GetMatrix().UntransformVector(pivot);
	Vector myLocalAxis = owner->GetMatrix().UnrotateVector(axis);
	
	btHingeConstraint* hinge;
	
	if(entity) {
		if(!entity->GetPhysicsModule())
			return NULL;
		
		Vector hisLocalPivot = entity->GetMatrix().UntransformVector(pivot);
		Vector hisLocalAxis = entity->GetMatrix().UnrotateVector(axis);
		
		hinge = new btHingeConstraint(*body, *entity->GetPhysicsModule()->GetDBody(), myLocalPivot.btV(), hisLocalPivot.btV(), myLocalAxis.btV(), hisLocalAxis.btV());
	}else
		hinge = new btHingeConstraint(*body, myLocalPivot.btV(), myLocalAxis.btV());
	
	dWorld->addConstraint(hinge);
	
	return hinge;
}

btRigidBody* ModPhysics::GetDBody() {
	return body;
}

void ModPhysics::UpdateMatrix() {
	btTransform trans;
	trans.setFromOpenGLMatrix(&owner->GetMatrix()[0][0]);
	
	body->getMotionState()->setWorldTransform(trans);
	body->setWorldTransform(trans);
}

void ModPhysics::Wake() {
	btTransform trans;
	body->getMotionState()->getWorldTransform(trans);
	trans.getOpenGLMatrix(&owner->GetMatrixRef()[0][0]);
	owner->UpdateChildren();
}