#pragma once

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

#include "Vx/VxFrame.h"
#include "Vx/VxUniverse.h"
#include "Vx/VxAssembly.h"
#include "Vx/VxPart.h"
#include "Vx/VxCollisionGeometry.h"
#include "Vx/VxPlane.h"
#include "Vx/VxSphere.h"
#include "Vx/VxQuaternion.h"
#include "Vx/VxBox.h"
#include "Vx/VxCapsule.h"
#include "Vx/VxAngular3.h"
#include "Vx/VxLinear3.h"
#include "Vx/VxHinge.h"
#include "Vx/VxUniversal.h"
#include "Vx/VxBallAndSocket.h"
#include "Vx/VxMaterial.h"
#include "Vx/VxMaterialTable.h"
#include "Vx/VxConstraint.h"
#include "Vx/VxMotorConstraint.h"
#include "Vx/VxConeConstraint.h"
#include "Vx/VxConstraintController.h"
#include "Vx/VxTimer.h"
#include "Vx/VxSolverParameters.h"
#include "Vx/VxRequest.h"
#include "Vx/VxRPRO.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>

#define MAX_CONTACT_FEEDBACK 200
using namespace Vx;

PHYSICS_TEMPLATE( DynamicArray<VxCollisionGeometry*> )

//this structure is used to map a rigid body to the id of its Vortex counterpart
typedef struct Vortex_RB_Map_struct{
	VxPart* id;
	RigidBody* rb;
	DynamicArray<VxCollisionGeometry*> collisionVolumes; // Only used for rigid bodies
	Vortex_RB_Map_struct() : id(NULL), rb(NULL) {}
	Vortex_RB_Map_struct(VxPart* newId, RigidBody* newRb){ this->id = newId; this->rb = newRb;}
} Vortex_RB_Map;

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

class PHYSICS_DECLSPEC VortexWorld : public World{
private:
	// Vortex's id for the simulation world
	VxFrame * frame;
	VxUniverse * universe;
	
	VxPart * spaceID;

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

	//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 setVortexStateFromRB(int i);

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

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

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

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

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

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

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

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

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

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

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

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

	/**
		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();
public:
	/**
		default constructor
	*/
	VortexWorld();

	/**
		destructor
	*/
	virtual ~VortexWorld(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 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);
	
	/**
		This method is for performance analysis
	*/
	void printAllCOMPosition();

	void drawAdditionalInformation();

	void drawBoxes();

	void collisionsPostProcessing();
};

#endif