#include <stdio.h>
#include "Environment.h"
#include "BulletObject.h"

Environment::Environment()
: m_dynamicsWorld(0)
{
}

/** Initialize the environment of the virtual world.
	Create de ground and add it as an object to the virtual world
*/
void Environment::setupEnvironment()
{
	btScalar mass(0.);
	bool isDynamic = (mass != 0.f);
	btVector3 localInertia(0,0,0);
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;

	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_broadphase = new btDbvtBroadphase();
	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));

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	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());
}


void Environment::addAgent(Agent *agent)
{
}

btDynamicsWorld* Environment::getDynamicsWorld()
{
	return m_dynamicsWorld;
}

btAlignedObjectArray<btCollisionShape*>	Environment::getCollisionShapes()
{
	return m_collisionShapes;
}

/// Simulation 2D
void Environment::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;

	BulletObject *object = new BulletObject(this);

	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 = object->createBulletObject(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 = object->createBulletObject(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 = object->createBulletObject(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);
			}
		}
	}
}


/** Remove all objects from the virtual world */
void Environment::exitEnvironment()
{
	int i;
	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 (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}

	delete m_dynamicsWorld;
	delete m_solver;
	delete m_broadphase;
	delete m_dispatcher;
	delete m_collisionConfiguration;
}