#pragma once

#include "../compileconfig.h"
#ifdef Bullet

#include "btBulletDynamicsCommon.h"

#include <Physics/PhysicsDll.h>
#include <Physics/World.h>
#include <Physics/CollisionDetectionPrimitive.h>
#include <Physics/SphereCDP.h>
#include <Physics/CapsuleCDP.h>
#include <Physics/BoxCDP.h>
#include <Physics/PlaneCDP.h>
#include <Physics/PreCollisionQuery.h>
#include "GLDebugDrawer.h"

#define MAX_CONTACT_FEEDBACK 200

PHYSICS_TEMPLATE( DynamicArray<btCollisionShape*> )

//this structure is used to map a rigid body to the id of its Bullet counterpart
typedef struct Bullet_RB_Map_struct{
	btRigidBody* id;
	RigidBody* rb;
	DynamicArray<btCollisionShape*> collisionVolumes; // Only used for rigid bodies
	Bullet_RB_Map_struct() : id(NULL), rb(NULL) {}
	Bullet_RB_Map_struct(btRigidBody* newId, RigidBody* newRb){ this->id = newId; this->rb = newRb;}
} Bullet_RB_Map;


#define BIT(x) (1<<(x))
enum collisiontypes {
    COL_NOTHING = 0, //<Collide with nothing
    COL_BODY = BIT(1), //<Collide with body
    COL_WALL = BIT(2), //<Collide with walls
	COL_BOXES = BIT(4), //<Collide with walls
};

// Instanciate used STL classes
PHYSICS_TEMPLATE( DynamicArray<Bullet_RB_Map> )

class PHYSICS_DECLSPEC BulletWorld : public World{
private :
	GLDebugDrawer	sDebugDrawer;

	// Bullet's id for the simulation world
	btDiscreteDynamicsWorld * worldID;

	//keep track of the mapping between the rigid bodies and their Bullet counterparts with this
	DynamicArray<Bullet_RB_Map> bulletToRbs;

	//Bullet's id for collision object
	btCollisionObject * spaceID;

	//this is the max number of contacts that are going to be processed between any two objects
	int maxContactCount;

	//this is the current number of contact joints, for the current step of the simulation
	int jointFeedbackCount;

	//this is a pointer to a physical interface object that is used as an abstract way of communicating between the simulator and the application
	PreCollisionQuery* pcQuery;

	// Destroy the world, it becomes unusable, but everything is clean
	virtual void destroyWorld();

	// Setup the world as it should
	void setupWorld();

	/**
		this method is used to copy the state of the ith rigid body to its Bullet counterpart.
	*/
	void setBulletStateFromRB(int i);

	/**
		this method is used to copy the state of the ith rigid body, from the Bullet object to its rigid body counterpart 
	*/
	void setRBStateFromBullet(int i);

	/**
		this method is used to set up a Bullet plane shape. It is properly placed in body coordinates.
	*/
	btCollisionShape * getPlaneGeom(PlaneCDP* p, RigidBody* parent);

	/**
		this method is used to set up a Bullet sphere shape. It is properly placed in body coordinates.
	*/
	btCollisionShape * getSphereGeom(SphereCDP* s);

	/**
		this method is used to set up a Bullet box shape. It is properly placed in body coordinates.
	*/
	btCollisionShape * getBoxGeom(BoxCDP* b);

	/**
		this method is used to set up a Bullet capsule shape. It is properly placed in body coordinates.
	*/
	btCollisionShape * getCapsuleGeom(CapsuleCDP* c);

	/**
		This method is used to set up a Bullet fixed joint, based on the information in the fixed joint passed in as a parameter
	*/
	void setupBulletFixedJoint(StiffJoint* hj);

	/**
		This method is used to set up a Bullet hinge joint, based on the information in the hinge joint passed in as a parameter
	*/
	void setupBulletHingeJoint(HingeJoint* hj);

	/**
		This method is used to set up a Bullet universal joint, based on the information in the universal joint passed in as a parameter
	*/
	void setupBulletUniversalJoint(UniversalJoint* uj);

	/**
		This method is used to set up a Bullet ball-and-socket joint, based on the information in the ball in socket joint passed in as a parameter
	*/
	void setupBulletBallAndSocketJoint(BallInSocketJoint* basj);

	/**
		this method is used to create Bullet shapes for all the collision primitives of the rigid body that is passed in as a paramter
	*/
	void createBulletCollisionPrimitives(RigidBody* body, int index);

	/**
		This methods creates a Bullet object and links it to the RigidBody corresponding to objects[index]
	*/
	void linkRigidBodyToBullet( int index );

	/**
		This method reads a list of rigid bodies from the specified file.
	*/
	virtual void loadRBsFromFile(char* fName);

	/**
		this method is used to transfer the state of the rigid bodies, from the simulator to the rigid body wrapper
	*/
	virtual void setRBStateFromEngine();

	/**
		this method is used to transfer the state of the rigid bodies, from the rigid body wrapper to the simulator's rigid bodies
	*/
	virtual void setEngineStateFromRB();

	void collisionsPostProcessing(btScalar dt);
public :
	/**
		default constructor
	*/
	BulletWorld();

	/**
		destructor
	*/
	virtual ~BulletWorld(void);

	// Destroy all the objects, but the world is still usable
	virtual void destroyAllObjects();

	/**
		This method adds one rigid body (articulated or not).
	*/
	virtual void addRigidBody( RigidBody* rigidBody_disown );

	/**
		This method adds one articulated figure.
	*/
	virtual void addArticulatedFigure( ArticulatedFigure* articulatedFigure_disown );

	/**
		This method is used to set the state of all the rigid body in the physical world.
	*/
	void setState(DynamicArray<double>* state, int start = 0);

	/**
		This method is used to integrate the forward simulation in time.
	*/
	virtual void advanceInTime(double deltaT);

	/**
		This method is for performance analysis
	*/
	virtual void printAllCOMPosition();

	/**
		this method applies a force to a rigid body, at the specified point. The point is specified in local coordinates,
		and the force is also specified in local coordinates.
	*/
	virtual void applyRelForceTo(RigidBody* b, const Vector3d& f, const Point3d& p);

	/**
		this method applies a force to a rigid body, at the specified point. The point is specified in local coordinates,
		and the force is specified in world coordinates.
	*/
	virtual void applyForceTo(RigidBody* b, const Vector3d& f, const Point3d& p);

	/**
		this method applies a torque to a rigid body. The torque is specified in world coordinates.
	*/
	virtual void applyTorqueTo(RigidBody* b, const Vector3d& t);

	virtual void drawAdditionalInformation();

	void drawBoxes();
};

#endif