/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/


///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 2
#define ARRAY_SIZE_Y 2
#define ARRAY_SIZE_Z 2

#define SIZE 2

//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024)

///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 2.
#define START_POS_X -5
#define START_POS_Y -5
#define START_POS_Z -3

#define M_PI	3.14159265358979323846
#define HEIGHT	7

#include "BasicDemo.h"
#include "btBulletDynamicsCommon.h"
#include "GLDebugDrawer.h"

#include <mpi.h>
#include <stdio.h>
#include <map>
#include <string>
#include <vector>

using namespace std;

btCollisionShape* groundShape;
btTransform groundTransform;
btCollisionShape* colShape;

bool enable1, enable2;
bool simpleSimulation;
bool debugConstraints;

btScalar BasicDemo::computeVelocity(btScalar currentAngle, btScalar targetAngle) {
	
	btScalar deltaAngle = targetAngle - currentAngle;
		
	if(deltaAngle < 0)
		deltaAngle += 2 * M_PI;			
		
	if(deltaAngle > M_PI)
		deltaAngle -= 2 * M_PI;
		
	return deltaAngle;
}

void BasicDemo::updateSensors() {
	int i;
	btHingeConstraint *joint;
	Sensor *sensor;
	
	for(i = 0; i < m_sensors.size(); i++) {
		joint = (btHingeConstraint*) m_joints[m_sensors[i].first];
		sensor = m_sensors[i].second;
		sensor->setAngle(joint->getHingeAngle());
	}	
}

void BasicDemo::updateActuators() {
	int i;
	btHingeConstraint *joint;
	Sensor *sensor;
	
	for(i = 0; i < m_sensors.size(); i++) {
		joint = (btHingeConstraint*) m_joints[m_sensors[i].first];
		sensor = m_sensors[i].second;
		if (sensor->getForce() > 0.0f)
			joint->enableAngularMotor(true, computeVelocity(joint->getHingeAngle(), btScalar(sensor->getTargetAngle())) * 10, btScalar(sensor->getForce()));
		else
			joint->enableAngularMotor(false, 0, 0);
	}	
}

void BasicDemo::updateAgents() {
	int i;
	
	for(i = 0; i < m_agents.size(); i++) {
		m_agents[i]->step();
	}	
}


void BasicDemo::clientMoveAndDisplay() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	//simple dynamics world doesn't handle fixed-time-stepping
	float ms = getDeltaTimeMicroseconds();
	///step the simulation
	if (m_dynamicsWorld) {
		if(simpleSimulation) {
			btHingeConstraint *hinge1 = (btHingeConstraint*)m_joints[0];
			btHingeConstraint *hinge2 = (btHingeConstraint*)m_joints[1];
					
			if(enable1)
				hinge1->enableAngularMotor(true, computeVelocity(hinge1->getHingeAngle(), btScalar(M_PI * 0.25)) * 10, btScalar(10));
			else
				hinge1->enableAngularMotor(false, 0, 0);
			if(enable2)
				hinge2->enableAngularMotor(true, computeVelocity(hinge2->getHingeAngle(), btScalar(-M_PI * 0.25)) * 10, btScalar(50));
			else
				hinge2->enableAngularMotor(false, 0, 0);
		} else {
			updateSensors();		
			updateAgents();		
			updateActuators();		
		}
		
		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
		m_dynamicsWorld->debugDrawWorld();
	}
	
	renderme(); 
	glFlush();
	glutSwapBuffers();
}

void BasicDemo::displayCallback(void) {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 	
	renderme();
	//optional but useful: debug drawing to detect problems
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();
	glFlush();
	glutSwapBuffers();
}

void BasicDemo::debug() {	
		
}

/**
 * Initialize the objects used for a normal simulation in Bullet. 
 * Create the virtual world for the agents.
 * Set the ground of the simulated world
*/
void BasicDemo::initPhysics() {

	m_ele = 75;
	updateCamera();
	
	setTexturing(true);
	setShadows(true);
	setCameraDistance(btScalar(SCALING*20.));
	
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_broadphase = new btDbvtBroadphase();
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;
	
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	m_dynamicsWorld->setGravity(btVector3(0,-10,0));

	groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));	
	m_collisionShapes.push_back(groundShape);

	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,-50,0));
	
	btScalar mass(0.);
	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	bool isDynamic = (mass != 0.f);
	btVector3 localInertia(0,0,0);
	if (isDynamic)
		groundShape->calculateLocalInertia(mass,localInertia);

	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);

	//add the body to the dynamics world
	m_dynamicsWorld->addRigidBody(body);

	m_dynamicsWorld->setDebugDrawer(new GLDebugDrawer());
}

btRigidBody* BasicDemo::localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape) {
	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		shape->calculateLocalInertia(mass,localInertia);

	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
	rbInfo.m_friction = 100.0;
	btRigidBody* body = new btRigidBody(rbInfo);

	m_dynamicsWorld->addRigidBody(body);

	return body;
}

btRigidBody* BasicDemo::createObject(btCollisionShape* colShape, btVector3 position, btScalar mass) {	
	m_collisionShapes.push_back(colShape);

	btTransform startTransform;
	startTransform.setIdentity();
	startTransform.setOrigin(SCALING*position);

	btRigidBody *body = localCreateRigidBody(mass, startTransform, colShape);
	body->setActivationState(ISLAND_SLEEPING);
		
	return body;
}

btRigidBody* BasicDemo::createObject(btCollisionShape* colShape, btTransform transform, btScalar mass) {	
	m_collisionShapes.push_back(colShape);

	btRigidBody *body = localCreateRigidBody(mass, transform, colShape);
	body->setActivationState(ISLAND_SLEEPING);
		
	return body;
}

void BasicDemo::createHinge(bool enable) {
		
	simpleSimulation = enable;
	
	btCollisionShape* colShape1 = new btBoxShape(SCALING*btVector3(2, 0.1, 1));
	btCollisionShape* colShape2 = new btBoxShape(SCALING*btVector3(2, 0.1, 1));
	
	btRigidBody* part1 = createObject(colShape1, btVector3(0,6,0), 1.f);
	btRigidBody* part2 = createObject(colShape2, btVector3(2,6,0), 0.f);
	
	btTransform localA, localB;
	btHingeConstraint* hingeC;
	btHingeConstraint* hingeD;
	
	localA.setIdentity(); 
	localB.setIdentity();
	
	localA.setOrigin(SCALING*btVector3(2, 0, 0));	
	localB.setOrigin(SCALING*btVector3(-2, 0, 0));
	
	hingeC = new btHingeConstraint(*part1, *part2, localA, localB);
	hingeD = new btHingeConstraint(*part1, *part2, localA, localB);
	//hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
	//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
	
	m_joints.push_back(hingeC);
	m_joints.push_back(hingeD);
	
	m_dynamicsWorld->addConstraint(hingeC, true);
	m_dynamicsWorld->addConstraint(hingeD, true);
	
	clientResetScene();
}

enum {
	LEFT=0, RIGHT=1, UP=2, DOWN=3
};

void BasicDemo::createMagicCarpet(int n, int m) {
	int i, j, base = m_dynamicsWorld->getNumCollisionObjects();
	btCollisionShape *shape;
	btRigidBody *body, *body1, *body2;
	btVector3 pivotA, pivotB, axisA, axisB;
	btHingeConstraint* hinge;
	btTransform transform;
	btTransform frameA, frameB;
		
	for(i = 0; i < n; i++) {
		for(j = 0; j < m; j++) {
			transform = btTransform();
			transform.setIdentity();
			transform.setOrigin(SCALING*btVector3(i*1.0,HEIGHT,j*1.0));
			transform.setRotation(btQuaternion(0, 0, 0));

			shape = new btSphereShape(SCALING*btScalar(0.1));
			body = createObject(shape, transform, (!i && !j) ? 1.f : 1.f);

			btAlignedObjectArray<int> tmp;
			tmp.resize(4);
			m_matrix.push_back(tmp);
		}
	}	
		
	for(i = 0; i < n; i++) {
		for(j = 0; j < m; j++) {
			if (i > 0) {
				transform = btTransform();
				transform.setIdentity();
				transform.setOrigin(SCALING*btVector3(i*1.0-0.5,HEIGHT,j*1.0));
				transform.setRotation(btQuaternion(0, 0, -M_PI/2));

				shape = new btCapsuleShape(SCALING*btScalar(0.05),SCALING*btScalar(0.6));
				body = createObject(shape, transform, 1.f);

				body1 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + (i - 1) * m + j]);
				body2 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + i * m + j]);

				frameA = btTransform();
				frameA.setIdentity();
				frameA.setOrigin(SCALING*btVector3(0, -0.5, 0));
				frameA.setRotation(btQuaternion(0, 0, M_PI/2));
				frameB = btTransform();
				frameB.setIdentity();
				frameB.setOrigin(SCALING*btVector3(0, 0, 0));
				frameB.setRotation(btQuaternion(0, 0, 0));
				hinge = new btHingeConstraint(*body, *body1, frameA, frameB);
				hinge->setLimit(btScalar(- M_PI/2), btScalar(M_PI/2));
				hinge->setDbgDrawSize(btScalar(1.f));
				m_matrix[(i - 1) * m + j][RIGHT] = m_joints.size();
				m_joints.push_back(hinge);
				m_dynamicsWorld->addConstraint(hinge, true);

				frameA = btTransform();
				frameA.setIdentity();
				frameA.setOrigin(SCALING*btVector3(0, 0.5, 0));
				frameA.setRotation(btQuaternion(0, 0, M_PI/2));
				frameB = btTransform();
				frameB.setIdentity();
				frameB.setOrigin(SCALING*btVector3(0, 0, 0));
				frameB.setRotation(btQuaternion(0, 0, 0));
				hinge = new btHingeConstraint(*body, *body2, frameA, frameB);
				hinge->setLimit(btScalar(- M_PI/2), btScalar(M_PI/2));
				hinge->setDbgDrawSize(btScalar(1.f));
				m_matrix[i * m + j][LEFT] = m_joints.size();
				m_joints.push_back(hinge);
				m_dynamicsWorld->addConstraint(hinge, true);
			}
			if (j > 0) {
				transform = btTransform();
				transform.setIdentity();
				transform.setOrigin(SCALING*btVector3(i*1.0,HEIGHT,j*1.0-0.5));
				transform.setRotation(btQuaternion(-M_PI/2, 0, -M_PI/2));
				
				shape = new btCapsuleShape(SCALING*btScalar(0.05),SCALING*btScalar(0.6));
				body = createObject(shape, transform, 1.f);

				body1 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + i * m + (j - 1)]);
				body2 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + i * m + j]);

				frameA = btTransform();
				frameA.setIdentity();
				frameA.setOrigin(SCALING*btVector3(0, -0.5, 0));
				frameA.setRotation(btQuaternion(0, 0, 0));
				frameB = btTransform();
				frameB.setIdentity();
				frameB.setOrigin(SCALING*btVector3(0, 0, 0));
				frameB.setRotation(btQuaternion(-M_PI/2, 0, -M_PI/2));
				hinge = new btHingeConstraint(*body, *body1, frameA, frameB);
				hinge->setLimit(btScalar(- M_PI/2), btScalar(M_PI/2));
				hinge->setDbgDrawSize(btScalar(1.f));
				m_matrix[i * m + (j - 1)][DOWN] = m_joints.size();
				m_joints.push_back(hinge);
				m_dynamicsWorld->addConstraint(hinge, true);

				frameA = btTransform();
				frameA.setIdentity();
				frameA.setOrigin(SCALING*btVector3(0, 0.5, 0));
				frameA.setRotation(btQuaternion(0, 0, 0));
				frameB = btTransform();
				frameB.setIdentity();
				frameB.setOrigin(SCALING*btVector3(0, 0, 0));
				frameB.setRotation(btQuaternion(-M_PI/2, 0, -M_PI/2));
				hinge = new btHingeConstraint(*body, *body2, frameA, frameB);
				hinge->setLimit(btScalar(- M_PI/2), btScalar(M_PI/2));
				hinge->setDbgDrawSize(btScalar(1.f));
				m_matrix[i * m + j][UP] = m_joints.size();
				m_joints.push_back(hinge);
				m_dynamicsWorld->addConstraint(hinge, true);
			}
		}
	}	
}


void BasicDemo::createSwarm3D(int n, int m, int p) {
/*
	int i, j, k, base = m_dynamicsWorld->getNumCollisionObjects();
	btCollisionShape *shape;
	btRigidBody *body, *body1, *body2;
	btVector3 pivotA, pivotB, axisA, axisB;
	btHingeConstraint* hinge;
	btPoint2PointConstraint* hingeP2P;
	btTransform transform;
	for(i = 0; i < n; i++) {
		for(j = 0; j < m; j++) {
			for(k = 0; k < p; k++) {
				shape = new btSphereShape(SCALING*btScalar(0.18));
				body = createObject(shape, btVector3(i*1.0,k*1.0 + HEIGHT,j*0.0), 0.f);
			}
		}
	}	

	for(i = 0; i < n; i++) {
		for(j = 0; j < m; j++) {
			for(k = 0; k < p; k++) {
				if (i > 0) {
					transform.setIdentity();
					transform.setOrigin(SCALING*btVector3(i*1.0-0.5,k*1.0+HEIGHT,j*1.0));
					transform.setRotation(btQuaternion(btVector3(0,0,1), -M_PI/2));

					shape = new btCapsuleShape(SCALING*btScalar(0.05),SCALING*btScalar(0.6));
					body = createObject(shape, transform, 1.f);
					
					body1 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + (i - 1) * m * p + j * p + k]);
					body2 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + i * m * p + j * p + k]);
					
					pivotA = SCALING*btVector3(0, -0.6, 0);
					pivotB = SCALING*btVector3(0, 0, 0);
					axisA = btVector3(0, 0, 1);
					axisB = btVector3(0, 0, 1);
					hinge = new btHingeConstraint(*body, *body1, pivotA, pivotB, axisA, axisB);
					hinge->setLimit(btScalar(- 0.75 * M_PI), btScalar(0.75 * M_PI));
					m_joints.push_back(hinge);
					m_dynamicsWorld->addConstraint(hinge, true);

					pivotA = SCALING*btVector3(0, 0.6, 0);
					pivotB = SCALING*btVector3(0, 0, 0);
					axisA = btVector3(0, 0, 1);
					axisB = btVector3(0, 0, 1);
					hinge = new btHingeConstraint(*body, *body2, pivotA, pivotB, axisA, axisB);
					hinge->setLimit(btScalar(- 0.75 * M_PI), btScalar(0.75 * M_PI));
					m_joints.push_back(hinge);
					m_dynamicsWorld->addConstraint(hinge, true);
				}
				if (j > 0) {
					btTransform transform;
					transform.setIdentity();
					transform.setOrigin(SCALING*btVector3(i*1.0,k*1.0+HEIGHT,j*1.0-0.5));
					transform.setRotation(btQuaternion(btVector3(1,0,0), M_PI/2));
					
					shape = new btCapsuleShape(SCALING*btScalar(0.05),SCALING*btScalar(0.6));
					body = createObject(shape, transform, 1.f);

					body1 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + i * m * p + (j - 1) * p+ k]);
					body2 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + i * m * p + j * p + k]);

					pivotA = SCALING*btVector3(0, -0.6, 0);
					pivotB = SCALING*btVector3(0, 0, 0);
					axisA = btVector3(1, 0, 0);
					axisB = btVector3(1, 0, 0);
					hinge = new btHingeConstraint(*body, *body1, pivotA, pivotB, axisA, axisB);
					hinge->setLimit(btScalar(- 0.75 * M_PI), btScalar(0.75 * M_PI));
					m_joints.push_back(hinge);
					m_dynamicsWorld->addConstraint(hinge, true);

					pivotA = SCALING*btVector3(0, 0.6, 0);
					pivotB = SCALING*btVector3(0, 0, 0);
					axisA = btVector3(1, 0, 0);
					axisB = btVector3(1, 0, 0);
					hinge = new btHingeConstraint(*body, *body2, pivotA, pivotB, axisA, axisB);
					hinge->setLimit(btScalar(- 0.75 * M_PI), btScalar(0.75 * M_PI));
					m_joints.push_back(hinge);
					m_dynamicsWorld->addConstraint(hinge, true);
				}
				if(k > 0) {
					btTransform transform;
					transform.setIdentity();
					transform.setOrigin(SCALING*btVector3(i*1.0,k*1.0+HEIGHT-0.5,j*1.0));
					
					shape = new btCapsuleShape(SCALING*btScalar(0.05),SCALING*btScalar(0.6));
					body = createObject(shape, transform, 1.f);

					body1 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + i * m * p + j * p + (k - 1)]);
					body2 = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[base + i * m * p + j * p + k]);

					pivotA = SCALING*btVector3(0, -0.6, 0);
					pivotB = SCALING*btVector3(0, 0, 0);
//					axisA = btVector3(1, 0, 0);
//					axisB = btVector3(1, 0, 0);
//					hinge = new btHingeConstraint(*body, *body1, pivotA, pivotB, axisA, axisB);
					hingeP2P = new btPoint2PointConstraint(*body, *body1, pivotA, pivotB);
//					hinge->setLimit(btScalar(- 0.75 * M_PI), btScalar(0.75 * M_PI));
					m_joints.push_back(hingeP2P);
					m_dynamicsWorld->addConstraint(hingeP2P, true);

					pivotA = SCALING*btVector3(0, 0.6, 0);
					pivotB = SCALING*btVector3(0, 0, 0);
//					axisA = btVector3(1, 0, 0);
//					axisB = btVector3(1, 0, 0);
//					hinge = new btHingeConstraint(*body, *body2, pivotA, pivotB, axisA, axisB);
					hingeP2P = new btPoint2PointConstraint(*body, *body2, pivotA, pivotB);
//					hinge->setLimit(btScalar(- 0.75 * M_PI), btScalar(0.75 * M_PI));
					m_joints.push_back(hingeP2P);
					m_dynamicsWorld->addConstraint(hingeP2P, true);					
				}
			}
		}
	}
*/
}


void BasicDemo::createSimulation2D(int n, int m) {
	//btCollisionShape* colShape = new btSphereShape(SCALING*btScalar(1.5));
	//btRigidBody *ball = createObject(colShape, btVector3(4,0,3), 8);	
	//ball->activate();
	
	createMagicCarpet(n, m);
}

void BasicDemo::createSimulation3D(int n, int m, int p) {
	createSwarm3D(n,m,p);
}

void BasicDemo::keyboardCallback(unsigned char key, int x, int y) {	
	if(key == '1') {
		enable1 = !enable1;
		return;
	} 
	
	if(key == '2') {
		enable2 = !enable2;
		return;
	} 
	
	if(key == 'b') {
		btCollisionShape* colShape0 = new btSphereShape(SCALING*btScalar(0.5));
		btRigidBody* box = createObject(colShape0, btVector3(-2., 10, 0.5), 4.f);
					
		box->activate();
		
		return;
	}

	if (key == 'c') {
		debugConstraints = !debugConstraints;
		this->setDebugMode(debugConstraints ? btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits : 0);
	}

	DemoApplication::keyboardCallback(key, x, y);
}


void BasicDemo::exitPhysics() {
	//cleanup in the reverse order of creation/initialization
	//remove the rigidbodies from the dynamics world and delete them
	int i;
	
	for (i = 0; i < m_joints.size(); i++) {
		btTypedConstraint *joint = m_joints[i];
		delete joint;
	}
	
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--) {
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (i = 0; i < m_collisionShapes.size(); i++) {
		btCollisionShape* shape = m_collisionShapes[i];
		delete shape;
	}

	delete m_dynamicsWorld;
	
	delete m_solver;
	
	delete m_broadphase;
	
	delete m_dispatcher;

	delete m_collisionConfiguration;	
}