/**
Author : Stevie Giovanni (steviegiovanni@gmail.com)
**/

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

#include "VortexWorld.h"
#include <Utils/utils.h>
#include <Physics/Joint.h>
#include <Physics/StiffJoint.h>
#include <Physics/HingeJoint.h>
#include <Physics/UniversalJoint.h>
#include <Physics/BallInSocketJoint.h>
#include <Physics/PhysicsGlobals.h>
#include <string>
#include <fstream>
#include <iostream>


class MySubscriber : public VxUniverse::IntersectSubscriber
{
	public:
		virtual void notifyIntersect(VxUniverse::eIntersectEventType type,VxIntersectResult *result,VxDynamicsResponseInput* response){
			/*if (response && response->isOfClassType(VxDynamicsContactInput::getStaticClassType())){
				VxDynamicsContactInput* dynamicResponse = static_cast<VxDynamicsContactInput*>(response);

			}*/
		}
		virtual void notifyDisjoint(VxUniverse::eIntersectEventType type,VxIntersectResult *result){}
};

/**
	Default constructor
*/
VortexWorld::VortexWorld() : World(){
}

/**
	destructor
*/
VortexWorld::~VortexWorld(void){
}

void VortexWorld::destroyWorld() {
	//delete pcQuery;
	vortexToRbs.clear();
	
	//destroy the world
	//delete universe;
	frame->release();
	World::destroyWorld();
}

void VortexWorld::setupWorld() {
	countto1000 = 0;
	after1000 = 0;
	col1000 = 0;
	dynamics1000 = 0;
	//printf("VortexWorld::SetupWorld\n");
	int maxCont = 4;

	frame = VxFrame::instance();
    VxFrameRegisterAllInteractions(frame);
	//int numthread = 12;
	//frame->setMaxThreadCount(numthread);
	//frame->setAutomaticTimeStep(false);

	universe = new VxUniverse();
	Vx::VxSolverParameters * param = universe->getSolverParameters(0);
	param->setConstraintSolver(VxSolverParameters::kConstraintSolverStd);
	//param->setConstraintSolver(VxSolverParameters::kConstraintSolverIterative);
	//printf("std iteration : %d\n",param->getConstraintStdSolverMaxIteration());
	//printf("iterative iteration : %d\n",param->getConstraintIterativeSolverMaxIteration());
	//universe->setSolverThreadCount(numthread);
	Vector3d gravity = PhysicsGlobals::up * PhysicsGlobals::gravity;
	universe->setGravity(gravity.x, gravity.y, gravity.z);
	
	//MySubscriber * mysub = new MySubscriber();

	frame->addUniverse(universe);
	
	VxMaterialTable *materialTable = frame->getMaterialTable();

	
	//VxCollisionPairRequest * request = universe->getPartPartCollisionPairRequest(0,0);
	//printf("Max cont : %d\n",request->getContactMaxCount()/*universe->getPartPartCollisionRequestIDMaxCount()*/);

	VxMaterial* mDefaultMaterial = VxFrame::currentInstance()->getMaterialTable()->getDefaultMaterial();
	mDefaultMaterial->setFrictionModel(VxMaterial::kFrictionAxisLinear, VxMaterial::kFrictionModelScaledBoxFast);
    mDefaultMaterial->setFrictionCoefficient(VxMaterial::kFrictionAxisLinearPrimary, 0.8);
    mDefaultMaterial->setFrictionCoefficient(VxMaterial::kFrictionAxisLinearSecondary, 0.8);
    mDefaultMaterial->setRestitution(0.35);

    VxMaterial* mat = materialTable->registerMaterial("myDefault");
	mat->setFrictionCoefficient(VxMaterialBase::kFrictionAxisLinear, 1);
	mat->setSlide(VxMaterialBase::kFrictionAxisLinear, 0);
	mat->setSlip(VxMaterialBase::kFrictionAxisLinear, 0);
    mat->setRestitution(0);

	maxContactCount = maxCont;
	pcQuery = NULL;
	pcQuery = new PreCollisionQuery();
}

void VortexWorld::destroyAllObjects() {
	//printf("VortexWorld::DestroyAllObjectsWorld\n");
	destroyWorld();
	setupWorld();
	if(testmode){
		this->drawBoxes();
	}
}

/**
	this method is used to copy the state of the ith rigid body to its Vortex counterpart.
*/
void VortexWorld::setVortexStateFromRB(int i){
	//printf("VortexWorld::setVortexStateFromRB\n");
	if (i<0 || (uint)i>=vortexToRbs.size())
		return;

	//if it is a locked object, we update its CDPS
	if (vortexToRbs[i].rb->isLocked() == true) {
		//printf("%d\n",i);
		for (uint j=0;j<vortexToRbs[i].collisionVolumes.size();j++){
			VxCollisionGeometry * geom = vortexToRbs[i].collisionVolumes[j];
			//geom->set
		}
		return;
	}
	vortexToRbs[i].id->setPosition(vortexToRbs[i].rb->state.position.x, vortexToRbs[i].rb->state.position.y, vortexToRbs[i].rb->state.position.z);
	vortexToRbs[i].id->setOrientationQuaternion(vortexToRbs[i].rb->state.orientation.s,vortexToRbs[i].rb->state.orientation.v.x,vortexToRbs[i].rb->state.orientation.v.y,vortexToRbs[i].rb->state.orientation.v.z);
	vortexToRbs[i].id->setLinearVelocity(vortexToRbs[i].rb->state.velocity.x, vortexToRbs[i].rb->state.velocity.y, vortexToRbs[i].rb->state.velocity.z);
	vortexToRbs[i].id->setAngularVelocity(vortexToRbs[i].rb->state.angularVelocity.x, vortexToRbs[i].rb->state.angularVelocity.y, vortexToRbs[i].rb->state.angularVelocity.z);
}

/**
	this method is used to copy the state of the ith rigid body, from the Vortex object to its rigid body counterpart 
*/
void VortexWorld::setRBStateFromVortex(int i){
	//printf("VortexWorld::setRbStateFromVortexWorld\n");
	VxReal3 tempData;

	//if it is a locked object, we won't do anything about it
	if (vortexToRbs[i].rb->isLocked() == true){
		//printf("rb locked 1\n");
		return;
	}		

	//if the objects is supposed to be planar, make sure we don't let drift accumulate
	if (vortexToRbs[i].rb->props.isPlanar){
		//printf("rb planar\n");
		/*VxReal3 rot;
		vortexToRbs[i].id->getAngularVelocity(rot);
		VxReal4 quat_ptr;
		VxReal4 quat;
		VxReal quat_len;
		vortexToRbs[i].id->getOrientationQuaternion(quat_ptr);
		quat[0] = quat_ptr[0];
		quat[1] = quat_ptr[1];
	    quat[2] = 0; 
		quat[3] = 0; 
		quat_len = sqrt( quat[0] * quat[0] + quat[1] * quat[1] );
		quat[0] /= quat_len;
		quat[1] /= quat_len;
		vortexToRbs[i].id->setOrientationQuaternion(quat[0],quat[1],quat[2],quat[3]);
		rot[1] = 0;
		rot[2] = 0;
		vortexToRbs[i].id->setAngularVelocity(rot[0],rot[1],rot[2]);*/
	}

	vortexToRbs[i].id->getPosition(tempData);
	vortexToRbs[i].rb->state.position.x = tempData[0];
	vortexToRbs[i].rb->state.position.y = tempData[1];
    vortexToRbs[i].rb->state.position.z = tempData[2];

	VxReal4 orientation;
	vortexToRbs[i].id->getOrientationQuaternion(orientation);
	vortexToRbs[i].rb->state.orientation.s = orientation[0];
	vortexToRbs[i].rb->state.orientation.v.x = orientation[1];
	vortexToRbs[i].rb->state.orientation.v.y = orientation[2];
	vortexToRbs[i].rb->state.orientation.v.z = orientation[3];

	vortexToRbs[i].id->getLinearVelocity(tempData);
	vortexToRbs[i].rb->state.velocity.x = tempData[0];
	vortexToRbs[i].rb->state.velocity.y = tempData[1];
	vortexToRbs[i].rb->state.velocity.z = tempData[2];

	vortexToRbs[i].id->getAngularVelocity(tempData);
	vortexToRbs[i].rb->state.angularVelocity.x = tempData[0];
	vortexToRbs[i].rb->state.angularVelocity.y = tempData[1];
	vortexToRbs[i].rb->state.angularVelocity.z = tempData[2];
}

/**
	this method is used to set up a Vortex plane shape. 
*/
VxCollisionGeometry * VortexWorld::getPlaneGeom(PlaneCDP* p, RigidBody* parent){
	//printf("VortexWorld::getPlaneGeom\n");
	//and create the ground plane
	VxPlane * newgeom = new VxPlane();
	VxCollisionGeometry * g = new VxCollisionGeometry(newgeom);
	//g->setMaterial(frame->getMaterialTable()->getMaterial("myDefault"));
	Vector3d defA(0, 0, 1);
	Vector3d defB(0,1,0);
	Vector3d axis = defA.crossProductWith(defB);
	axis.toUnit();
	double rotAngle = defA.angleWith(defB);
	//g->setRelativeOrientation(axis.x*rotAngle,axis.y*rotAngle,axis.z*rotAngle);
	g->setRelativeOrientation(VX_PI/2,0,0);
	return g;
}

/**
	this method is used to set up a Vortex sphere shape. It is properly placed in body coordinates.
*/
VxCollisionGeometry * VortexWorld::getSphereGeom(SphereCDP* s){
	//printf("VortexWorld::getSphereGeom\n");
	VxSphere * newgeom = new VxSphere(s->getRadius());
	VxCollisionGeometry * g = new VxCollisionGeometry(newgeom);
	Point3d c = s->getCenter();
	g->setRelativePosition(c.x,c.y,c.z);
	return g;
}

/**
	this method is used to set up a Vortex box shape. It is properly placed in body coordinates.
*/
VxCollisionGeometry * VortexWorld::getBoxGeom(BoxCDP* b){
	//printf("VortexWorld::getBoxGeom\n");
	VxBox * newgeom = new VxBox(b->getXLen(), b->getYLen(), b->getZLen());
	VxCollisionGeometry * g = new VxCollisionGeometry(newgeom);
	Point3d c = b->getCenter();
	g->setRelativePosition(c.x, c.y, c.z);
	return g;
}

/**
	this method is used to set up a Vortex capsule shape. It is properly placed in body coordinates.
*/
VxCollisionGeometry * VortexWorld::getCapsuleGeom(CapsuleCDP* c){
	//printf("VortexWorld::getCapsuleGeom\n");
	//printf("code reached j\n");
	Point3d a = c->getPoint1();
	Point3d b = c->getPoint2();
	Vector3d ab(a, b);
	
	//VxBox * newbox = new VxBox(coba.x * ab.length()/2 + c->getRadius(),coba.y * ab.length()/2 + c->getRadius(),coba.z * ab.length()/2 + c->getRadius());
	VxCapsule * newgeom = new VxCapsule(c->getRadius(),ab.length());
	VxCollisionGeometry * g = new VxCollisionGeometry(newgeom);

	Vector3d defA(0, 0, 1);
	Vector3d axis = defA.crossProductWith(ab);
	axis.toUnit();
	double rotAngle = defA.angleWith(ab);
	g->setRelativeOrientation(axis.x*rotAngle,axis.y*rotAngle,axis.z*rotAngle);
	return g;
}

/**
	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 VortexWorld::setupVortexFixedJoint(StiffJoint* hj){
	//printf("VortexWorld::setupfixedjoint\n");
	//printf("code reached k\n");
	VxVector3 pos(0,0,0);
	VxRPRO * rpro = new VxRPRO(vortexToRbs[(int)(hj->parent->id)].id,vortexToRbs[(int)(hj->child->id)].id,pos);
	universe->addConstraint(rpro);
}

/**
	This method is used to set up a PhysX hinge joint, based on the information in the hinge joint passed in as a parameter
*/
void VortexWorld::setupVortexHingeJoint(HingeJoint* hj){
	//printf("VortexWorld::setuphingejoint\n");
	Point3d p = hj->child->getWorldCoordinates(hj->cJPos);
	VxVector3 pos(p.x,p.y,p.z);
	Vector3d a = hj->parent->getWorldCoordinates(hj->a);
	VxVector3 axis(a.x,a.y,a.z);
	VxHinge * hinge = new VxHinge(vortexToRbs[(int)(hj->parent->id)].id,vortexToRbs[(int)(hj->child->id)].id,pos,axis);
	if(hinge == NULL){
		printf("hinge failed!!!\n");
	}
	if(hj->useJointLimits == true && !testmode){
		hinge->setLowerLimit(0,-hj->maxAngle,0,0,VX_INFINITY,0);
		hinge->setUpperLimit(0,-hj->minAngle,0,0,VX_INFINITY,0);
		hinge->setLimitsActive(0,true);
	}

	universe->addConstraint(hinge);
}

/**
	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 VortexWorld::setupVortexUniversalJoint(UniversalJoint* uj){
	//printf("VortexWorld::setupuniversaljoint\n");
	//printf("code reached m\n");
	Point3d p = uj->parent->getWorldCoordinates(uj->pJPos);
	VxVector3 pos(p.x,p.y,p.z);
	Vector3d a = uj->parent->getWorldCoordinates(uj->a);
	Vector3d b = uj->child->getWorldCoordinates(uj->b);
	VxVector3 axis1(a.x,a.y,a.z);
	VxVector3 axis2(b.x,b.y,b.z);
	VxUniversal * universal = new VxUniversal(vortexToRbs[(int)(uj->parent->id)].id,vortexToRbs[(int)(uj->child->id)].id,pos,axis1,axis2);

	if(uj->useJointLimits == true && !testmode){
		universal->setLowerLimit(0,-uj->maxAngleA,0,0,VX_INFINITY,0);
		universal->setUpperLimit(0,-uj->minAngleA,0,0,VX_INFINITY,0);
		universal->setLowerLimit(1,-uj->maxAngleB,0,0,VX_INFINITY,0);
		universal->setUpperLimit(1,-uj->minAngleB,0,0,VX_INFINITY,0);
		universal->setLimitsActive(0,true);
		universal->setLimitsActive(1,true);
	}

	if(universal == NULL){
		printf("universal failed!!!\n");
	}

	universe->addConstraint(universal);
}

/**
	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 VortexWorld::setupVortexBallAndSocketJoint(BallInSocketJoint* basj){
	//printf("VortexWorld::setupballandsocketjoint\n");
	Point3d p = basj->child->getWorldCoordinates(basj->cJPos);
	VxVector3 pos(p.x,p.y,p.z);
	VxBallAndSocket * ballandsocket = new VxBallAndSocket(vortexToRbs[(int)(basj->parent->id)].id, vortexToRbs[(int)(basj->child->id)].id,pos);
	
	if(basj->useJointLimits == true && !testmode){
		Vector3d a = basj->parent->getWorldCoordinates(basj->swingAxis1);
		Vector3d b =  basj->child->getWorldCoordinates(basj->twistAxis);
		VxVector3 twist(b.x,b.y,b.z);
		VxVector3 swing(a.x,a.y,a.z);
		Vector3d c = b.crossProductWith(a);
		VxVector3 swing2(c.x,c.y,c.z);
		VxMotorConstraint * motor1 = new VxMotorConstraint(vortexToRbs[(int)(basj->parent->id)].id, vortexToRbs[(int)(basj->child->id)].id,pos,twist);
		motor1->setUpperLimit(0,-basj->minTwistAngle,0,0,VX_INFINITY,0);
		motor1->setLowerLimit(0,-basj->maxTwistAngle,0,0,VX_INFINITY,0);
		VxMotorConstraint * motor2 = new VxMotorConstraint(vortexToRbs[(int)(basj->parent->id)].id, vortexToRbs[(int)(basj->child->id)].id,pos,swing);
		motor2->setUpperLimit(0,-basj->minSwingAngle1,0,0,VX_INFINITY,0);
		motor2->setLowerLimit(0,-basj->maxSwingAngle1,0,0,VX_INFINITY,0);
		VxMotorConstraint * motor3 = new VxMotorConstraint(vortexToRbs[(int)(basj->parent->id)].id, vortexToRbs[(int)(basj->child->id)].id,pos,swing2);
		motor3->setUpperLimit(0,-basj->minSwingAngle2,0,0,VX_INFINITY,0);
		motor3->setLowerLimit(0,-basj->maxSwingAngle1,0,0,VX_INFINITY,0);
		motor1->setLimitsActive(0,true);
		motor2->setLimitsActive(0,true);
		motor3->setLimitsActive(0,true);
		universe->addConstraint(motor1);
		universe->addConstraint(motor2);
		universe->addConstraint(motor3);
	}

if(ballandsocket == NULL){
		printf("ballandsocket failed!!!\n");
	}

	universe->addConstraint(ballandsocket);
}

/**
	this method is used to create Vortex shapes for all the collision primitives of the rigid body that is passed in as a paramter
*/
void VortexWorld::createVortexCollisionPrimitives(RigidBody* body, int index){
	//printf("VortexWorld::create collision primitives\n");
	//now we'll set up the body's collision detection primitivese
	for (uint j=0;j<body->cdps.size();j++){
		int cdpType = body->cdps[j]->getType();

		//depending on the type of collision primitive, we'll now create g.
		VxCollisionGeometry * g;

		switch (cdpType){
			case SPHERE_CDP:
				g = getSphereGeom((SphereCDP*)body->cdps[j]);
				break;
			case CAPSULE_CDP:
				g = getCapsuleGeom((CapsuleCDP*)body->cdps[j]);
				break;
			case BOX_CDP:
				g = getBoxGeom((BoxCDP*)body->cdps[j]);
				break;
			case PLANE_CDP:
				//NOTE: only static objects can have planes as their collision primitives - if this isn't static, force it!!
				g = getPlaneGeom((PlaneCDP*)body->cdps[j], body);
				break;
			default:
				throwError("Ooppps... No collision detection primitive was created rb: %s, cdp: %d", body->name, j);
		}

		//now associate the geom to the rigid body that it belongs to, so that we can look up the properties we need later...
		//g->setCollisionEnable(false);
		g->setUserData(body);
		//g->setMaterial(frame->getMaterialTable()->getMaterial("myDefault"));
		//if it's a plane, it means it must be static, so we can't attach a transform to it...
		if (cdpType == PLANE_CDP){
			VxPart * temp = new VxPart();
			temp->addCollisionGeometry(g);
			temp->freeze();
			universe->addEntity(temp);
			continue;
		}

		//if the object is fixed, then we want the geometry to take into account the initial position and orientation of the rigid body
		if (body->isLocked() == true){
			VxTransform transform = VxTransform::createIdentity();
			transform.setTranslation(body->state.position.x, body->state.position.y, body->state.position.z);
			//g->setRelativePosition(body->state.position.x, body->state.position.y, body->state.position.z);
			Matrix M;
			body->state.orientation.getRotationMatrix(&M);
			transform.setRotation(VxVector3(M.get(0,0),M.get(0,1),M.get(0,2)),VxVector3(M.get(1,0),M.get(1,1),M.get(1,2)),VxVector3(M.get(2,0),M.get(2,1),M.get(2,2)));
			
			g->setRelativeTransform(transform);
			// Save collision volumes for eventual updates
			vortexToRbs[body->id].id->addCollisionGeometry(g);
			vortexToRbs[body->id].id->freeze();
			vortexToRbs[index].collisionVolumes.push_back(g);
		}

		//now add t (which contains the correctly positioned geom) to the body, if we do really have an ODE body for it
		if (body->isLocked() == false){
			vortexToRbs[body->id].id->addCollisionGeometry(g);}
	}
}

/**
	This method reads a list of rigid bodies from the specified file.
*/
void VortexWorld::loadRBsFromFile(char* fName){
	//printf("VortexWorld::loadrbsfromfile\n");
	//make sure we don't go over the old articulated figures in case this method gets called multiple times.
	int index = objects.size();
	int index_afs = AFs.size();

	World::loadRBsFromFile(fName);

	// Add all non-articulated rigid bodies in ODE
	for (uint i=index;i<objects.size();i++){
		RigidBody* rigidBody = objects[i];
		// Add a placeholder in the odeToRbs mapping
		vortexToRbs.push_back(Vortex_RB_Map(0, rigidBody));
		if( !rigidBody->isArticulated() )
			linkRigidBodyToVortex(objects.size()-1);
	}

	DynamicArray<Joint*> joints;

	// Check every newly added articulated figures
	for (uint i=index_afs;i<AFs.size();i++){

		// For each, add the articulated bodies they contain to ODE		
		for (uint j=0;j<objects.size();j++){
			if( !objects[j]->isArticulated() )
				continue;
			ArticulatedRigidBody* arb = (ArticulatedRigidBody*)objects[j];
			if( arb->getAFParent() == AFs[i] )
				linkRigidBodyToVortex(j);
		}

		//now we will go through all the new joints, and create and link their ode equivalents
		joints.clear();
		AFs[i]->addJointsToList(&joints);
		for (uint j=0;j<joints.size();j++){
			//connect the joint to the two bodies
			int jointType = joints[j]->getJointType();
			switch (jointType){
				case STIFF_JOINT:
					setupVortexFixedJoint((StiffJoint*)joints[j]);
					break;
				case BALL_IN_SOCKET_JOINT:
					setupVortexBallAndSocketJoint((BallInSocketJoint*)joints[j]);
					break;
				case HINGE_JOINT:
					setupVortexHingeJoint((HingeJoint*)joints[j]);
					//setupVortexHingeJoint((HingeJoint*)joints[j]);
					break;
				case UNIVERSAL_JOINT:
					setupVortexUniversalJoint((UniversalJoint*)joints[j]);
					//setupVortexUniversalJoint((UniversalJoint*)joints[j]);
					break;
				default:
					throwError("Ooops.... Only BallAndSocket, Hinge, Universal and Stiff joints are currently supported.\n");
			}
		}
	}
}

/**
	This methods creates an Vortex object and links it to the passed RigidBody
*/
void VortexWorld::linkRigidBodyToVortex( int index ) {
	//printf("VortexWorld::link rigid to vortex\n");
	RigidBody* rigidBody = vortexToRbs[index].rb; 

	//CREATE AND LINK THE ODE BODY WITH OUR RIGID BODY
	//if the body is fixed, we'll only create the colission detection primitives
	if (!rigidBody->isLocked()){
		vortexToRbs[index].id = new VxPart(vortexToRbs[index].rb->getMass());
		vortexToRbs[index].id->setUserData((void*)index);
		//the ID of this rigid body will be its index in the 
		rigidBody->setBodyID( index );
		//we will use the user data of the object to store the index in this mapping as well, for easy retrieval
	}

	//if this is a planar object, make sure we constrain it to always stay planar
	if (rigidBody->props.isPlanar){
		//dJointID j = dJointCreatePlane2D(worldID, 0);
		//dJointAttach(j, odeToRbs[index].id, 0);
	}

	//PROCESS THE COLLISION PRIMITIVES OF THE BODY
	createVortexCollisionPrimitives(rigidBody, index);

	//SET THE INERTIAL PARAMETERS

	if (rigidBody->isLocked() == false){
		Vector3d principalMoments = vortexToRbs[index].rb->getPMI();
		//printf("pmi : %f,%f,%f\n",principalMoments.x,principalMoments.y,principalMoments.z);
		VxReal33 pmi;
		pmi[0][0] = principalMoments.x;
		pmi[0][1] = 0;
		pmi[0][2] = 0;
		pmi[1][0] = 0;
		pmi[1][1] = principalMoments.y;
		pmi[1][2] = 0;
		pmi[2][0] = 0;
		pmi[2][1] = 0;
		pmi[2][2] = principalMoments.z;
		//vortexToRbs[index].id->seti
		vortexToRbs[index].id->setMassAndInertia(vortexToRbs[index].rb->getMass(),pmi);
		universe->addEntity(vortexToRbs[index].id);
		setVortexStateFromRB(index);
	}
}

/**
	This method adds one rigid body (not articulated).
*/
void VortexWorld::addRigidBody( RigidBody* rigidBody ) {
	//printf("VortexWorld::addrigidbody\n");
	//printf("BulletWorld::addRigidBody\n");
	World::addRigidBody(rigidBody);

	// Add a placeholder in the odeToRbs mapping
	vortexToRbs.push_back(Vortex_RB_Map(0, rigidBody));

	// Non-articulated rigid body are already well-positioned, link them to Vortex
	if( !rigidBody->isArticulated() )
		linkRigidBodyToVortex(objects.size()-1);
	// For articulated rigid bodies, we will only add them when (and if) they appear in an ArticulatedFigure
}

/**
	This method adds one rigid body (not articulated).
*/
void VortexWorld::addArticulatedFigure(ArticulatedFigure* articulatedFigure){
	//printf("VortexWorld::addarticulatedfigure\n");
	World::addArticulatedFigure( articulatedFigure );
	//printf("shalalalala\n");
	// Add the articulated bodies contained into that figure to Vortex	
	for (uint j=0;j<objects.size();j++){
		if( !objects[j]->isArticulated() )
			continue;
		ArticulatedRigidBody* arb = (ArticulatedRigidBody*)objects[j];
		if( arb->getAFParent() == articulatedFigure )
			linkRigidBodyToVortex(j);
	}

	DynamicArray<Joint*> joints;

	//now we will go through all the new joints, and create and link their ode equivalents
	articulatedFigure->addJointsToList(&joints);
	for (uint j=0;j<joints.size();j++){
		//connect the joint to the two bodies
		int jointType = joints[j]->getJointType();
		//printf("child id : %d\n",joints[j]->child->id);
		//printf("parent id : %d\n",joints[j]->parent->id);
		switch (jointType){
			case STIFF_JOINT:
				setupVortexFixedJoint((StiffJoint*)joints[j]);
				//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				break;
			case BALL_IN_SOCKET_JOINT:
				setupVortexBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				break;
			case HINGE_JOINT:
				//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				//setupVortexBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				setupVortexHingeJoint((HingeJoint*)joints[j]);
				//setupPhysXHingeJoint((HingeJoint*)joints[j]);
				break;
			case UNIVERSAL_JOINT:
				//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				//setupVortexBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				setupVortexUniversalJoint((UniversalJoint*)joints[j]);
				//setupPhysXUniversalJoint((UniversalJoint*)joints[j]);
				break;
			default:
				throwError("Ooops.... Only BallAndSocket, Hinge, Universal and Stiff joints are currently supported.\n");
		}
	}

	updateCharacterHeight();
	for (uint j=objects.size()-16;j<objects.size();j++){
		if( !objects[j]->isArticulated() )
			continue;
		for(uint k = objects.size()-16; k<j;k++){
			if( !objects[k]->isArticulated() )
				continue;
			universe->disablePairIntersect(vortexToRbs[objects[j]->id].id,vortexToRbs[objects[k]->id].id);
		}
	}
}

/**
	This method is used to set the state of all the rigid body in this collection.
*/
void VortexWorld::setState(DynamicArray<double>* state, int start){
	//printf("VortexWorld::setstate\n");
	World::setState(state, start);
}

/**
	this method is used to transfer the state of the rigid bodies, from PhysX to the rigid body wrapper
*/
void VortexWorld::setRBStateFromEngine(){
	//printf("VortexWorld::setrbstatefromengine\n");
	//now update all the rigid bodies...
	for (uint i=0;i<objects.size();i++){
		setRBStateFromVortex(i);
//		objects[i]->updateToWorldTransformation();
	}
}

/**
	this method is used to transfer the state of the rigid bodies, from the rigid body wrapper to ODE's rigid bodies
*/
void VortexWorld::setEngineStateFromRB(){
	//printf("VortexWorld::setenginestatefromrb\n");
	//now update all the rigid bodies...
	for (uint i=0;i<objects.size();i++){
		setVortexStateFromRB(i);
	}
}

void VortexWorld::advanceInTime(double deltaT){
	//printf("BulletWorld::advanceInTime\n");
	if( deltaT <= 0 )
		return;

	//make sure that the state of the RB's is synchronized with the engine...
	setEngineStateFromRB();

	//restart the counter for the joint feedback terms
	jointFeedbackCount = 0;

	//go through all the rigid bodies in the world, and apply their external force
	//printf("num objects : %d\n",objects.size());
	for (uint j=0;j<objects.size();j++){
		if( objects[j]->isLocked() ) 
			continue;
		const Vector3d& f = /*Vector3d(0,0,5);//*/objects[j]->externalForce;
		VxReal3 force;
		force[0] = f.x;
		force[1] = f.y;
		force[2] = f.z;
		if( !f.isZeroVector() /*&& objects[j]->id == 6*/)
			vortexToRbs[objects[j]->id].id->addForce(force);
		const Vector3d& t = /*Vector3d(5,0,0);//*/objects[j]->externalTorque;
		VxReal3 torque;
		torque[0] = t.x;
		torque[1] = t.y;
		torque[2] = t.z;
		if( !t.isZeroVector() ){
				vortexToRbs[objects[j]->id].id->addTorque(torque);}

		/*if(perturb && j == 3){
			//printf("code reached\n");
			nperturb++;
			force[0] = -200;
			force[1] = 0;
			force[2] = -150;
			vortexToRbs[objects[j]->id].id->addForce(force);
			if(nperturb == 400){
				nperturb = 0;
				perturb = false;
			}
		}*/
	}

	//go through all the joints in the world, and apply their torques to the parent and child rb's
	for (uint j=0;j<jts.size();j++){
		Vector3d t = jts[j]->torque;
		VxReal3 parenttorque;
		parenttorque[0] = t.x;
		parenttorque[1] = t.y;
		parenttorque[2] = t.z;
		VxReal3 childtorque;
		childtorque[0] = -t.x;
		childtorque[1] = -t.y;
		childtorque[2] = -t.z;
		vortexToRbs[jts[j]->parent->id].id->addTorque(parenttorque);
		vortexToRbs[jts[j]->child->id].id->addTorque(childtorque);
	}

	//clear the previous list of contact forces
	contactPoints.clear();
	
	frame->setTimeStep(deltaT);

	//VxTimer timer;
	//timer.start();
	frame->step();
	//timer.stop();
	/*VxReal secondSpent = timer.getLastTime();
	VxReal collisionTime = frame->getCollisionTime();
	VxReal dynamicsTime = frame->getDynamicsTime();
	//printf("time of simulation : %f s\n",secondSpent);

	countto1000++;
	if(countto1000 <= 2000 && countto1000 >= 1001){
		after1000 += secondSpent;
		col1000 += collisionTime;
		dynamics1000 += dynamicsTime;
		std::ofstream myfile;
		if(countto1000 == 1001){
			myfile.open ("performancetest.txt",std::ios::out);
		}else{
			myfile.open ("performancetest.txt",std::ios::app);
		}
		//printf("Number of frames in this walk cycle : %d\n",nbFrames);
		myfile<<"Step iteration - "<<countto1000<<std::endl;
		myfile<<"==============================================================="<<std::endl;
		myfile<<"Step time      : "<<secondSpent<<std::endl;
		myfile<<"Collision time : "<<collisionTime<<std::endl;
		myfile<<"Dynamics time  : "<<dynamicsTime<<std::endl;
		myfile<<"==============================================================="<<std::endl;
		if(countto1000 == 2000){
			myfile<<"Average step time      : "<<after1000 / 1000<<std::endl;
			myfile<<"Average collision time : "<<col1000 / 1000<<std::endl;
			myfile<<"Average dynamics time  : "<<dynamics1000 / 1000<<std::endl;
		}
		myfile.close();
	}
*/
	if(!testmode)
		collisionsPostProcessing();
	//copy over the state of the ODE bodies to the rigid bodies...
	setRBStateFromEngine();
}

/**
	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.
*/
void VortexWorld::applyRelForceTo(RigidBody* b, const Vector3d& f, const Point3d& p){
	//printf("BulletWorld::applyRelForceTo\n");
	if (!b)
		return;
	VxReal3 force;
	force[0] = f.x;
	force[1] = f.y;
	force[2] = f.z;
	VxReal3 pos;
	pos[0] = p.x;
	pos[1] = p.y;
	pos[2] = p.z;
	vortexToRbs[b->id].id->addForceAtLocalPosition(force,pos);
}

/**
	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.
*/
void VortexWorld::applyForceTo(RigidBody* b, const Vector3d& f, const Point3d& p){
	if (!b)
		return;
	VxReal3 force;
	force[0] = f.x;
	force[1] = f.y;
	force[2] = f.z;
	VxReal3 pos;
	pos[0] = p.x;
	pos[1] = p.y;
	pos[2] = p.z;
	vortexToRbs[b->id].id->addForceAtLocalPosition(force,pos);
}

/**
	this method applies a torque to a rigid body. The torque is specified in world coordinates.
*/
void VortexWorld::applyTorqueTo(RigidBody* b, const Vector3d& t){
	//printf("BulletWorld::applyTorqueTo\n");
	if (!b)
		return;
	VxReal3 torque;
	torque[0] = t.x;
	torque[1] = t.y;
	torque[2] = t.z;
	vortexToRbs[b->id].id->addTorque(torque);
	//dBodyAddTorque(odeToRbs[b->id].id, t.x, t.y, t.z);
}

/**
		This method is for performance analysis
	*/
void VortexWorld::printAllCOMPosition(){
	//printf("printing com position Bullet\n");
}

/**
	draw additional information
*/
void VortexWorld::drawAdditionalInformation(){
	//dJoint curjoint = worldID->firstjoint;
	//dBody curbody = worldID->firstbody;
	//printf("number of joints : %d\n", djoint);
	GLboolean lighting = glIsEnabled(GL_LIGHTING);
	if (lighting)
		glDisable(GL_LIGHTING);
//	universe->gete

	for(uint j = 0; j < objects.size();j++){
		//printf("code reached\n");
		/*if(!objects[j]->isArticulated())
			continue;
		for(uint i = 0; i < j; i++){
			dJointID connectingjoint = dConnectingJoint(odeToRbs[objects[j]->id].id,odeToRbs[objects[i]->id].id);
			int type = dJointGetType(connectingjoint);
			dVector3 anchor;
			if(type == dJointTypeBall){
				//dJointGetBallAnchor(connectingjoint,anchor);
				//glColor3f(0,0,255);
			} else if(type == dJointTypeHinge){
				dJointGetHingeAnchor(connectingjoint,anchor);
				dVector3 axis;
				dJointGetHingeAxis(connectingjoint,axis);
				glBegin(GL_LINE);
				glColor3f(0,0,255);
				glVertex3f(anchor[0],anchor[1],anchor[2]);   
				glVertex3f(axis[0],axis[1],axis[2]);     
				glEnd();
			} else if(type == dJointTypeUniversal){
				//dJointGetUniversalAnchor(connectingjoint,anchor);
				//glColor3f(0,0,255);
			}
		}*/
	}
	if (lighting)
		glEnable(GL_LIGHTING);
}

void VortexWorld::drawBoxes(){
	for(int i = 0; i < 50;i++){
		for(int j = 0;j < 50;j++){
			Point3d p1(-0.125,-0.125,-0.125);
			Point3d p2(0.125,0.125,0.125);
			BoxCDP * bcdp =  new BoxCDP(p1,p2);
			RigidBody * rb = new RigidBody();
			rb->addCollisionDetectionPrimitive(bcdp);
			rb->state.position.x =2;
			rb->state.position.y =0.125 + 0.3 * i;
			rb->state.position.z = - (0.3 * 50 / 2) + 0.3 * j ;
			this->addRigidBody(rb);
		}
	}
}

void VortexWorld::collisionsPostProcessing(){
	//printf("Code reach\n");
	VxUniverse::DynamicsContactIterator it = universe->dynamicsContactBegin();
	int index = 0;
	while (it != universe->dynamicsContactEnd()) {
		VxReal3 p;
		(*it)->getPosition(p);
		VxPart * p1;
		VxPart * p2;
		(*it)->getPartPair(&p1,&p2);
		RigidBody * rb1 = vortexToRbs[(int)p1->getUserData()].rb;
		RigidBody * rb2 = vortexToRbs[(int)p2->getUserData()].rb;
		contactPoints.push_back(ContactPoint());
		//now we'll set up the feedback for this contact joint
		contactPoints[index].rb1 = rb1;
		contactPoints[index].rb2 = rb2;
		contactPoints[index].cp = Point3d(p[0], p[1], p[2]);
		VxReal3 f;
		(*it)->getForce(0,f);
		contactPoints[index].f = Vector3d(f[0], f[1], f[2]);
		//make sure that the force always points away from the static objects
		//printf("Contact forces : %f,%f,%f\n",f[0],f[1],f[2]);
		if (contactPoints[index].rb1->isLocked() && !contactPoints[index].rb2->isLocked()){
			contactPoints[index].f = contactPoints[index].f * (-1);
			RigidBody* tmpBdy = contactPoints[index].rb1;
			contactPoints[index].rb1 = contactPoints[index].rb2;
			contactPoints[index].rb2 = tmpBdy;
		}
		index++;
		++it;
	}
}



#endif
