
#include "giofx.h"

#ifdef USE_BULLET

#include "btBulletDynamicsCommon.h"
#include "btBulletCollisionCommon.h"

static int counter = 0;
static int lastCounter = 0;

#define USE_Z_CONSTRAINT

//-------------------------------------------------------------------------
struct YourOwnFilterCallback : public btOverlapFilterCallback
{
	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
	{
		bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
		collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
		++counter;
		if (collides) 
		{	
			if (lastCounter < counter + 10000)
			{	
				PhysicCollideFromCallback(collides);
			}
			lastCounter = counter;
		}
		return collides;
	}
};
//-------------------------------------------------------------------------
void cScene::PhysicAddCallback()
{
	btOverlapFilterCallback * filterCallback = new YourOwnFilterCallback();
	
	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(p_worldHandle);
	dynamicsWorld->getPairCache()->setOverlapFilterCallback(filterCallback);
}

//-------------------------------------------------------------------------
void cScene::PhysicStart(float gravityX,float gravityY,float gravityZ,float stepSimulation)
{
	p_stepSimulation = stepSimulation;

	if (!p_sdkHandle)
		p_sdkHandle = plNewBulletSdk();
	
	if (!p_worldHandle)
	{	
		p_worldHandle = plCreateDynamicsWorld(p_sdkHandle);
		btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(p_worldHandle);
		btVector3 gravity(gravityX,gravityY,gravityZ);
		dynamicsWorld->setGravity(gravity);
	}

}
//-------------------------------------------------------------------------
void cScene::PhysicSetGravity(float gravityX,float gravityY,float gravityZ)
{
	if (p_worldHandle)
	{	
		btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(p_worldHandle);
		btVector3 gravity(gravityX,gravityY,gravityZ);
		dynamicsWorld->setGravity(gravity);
	}
}
//-------------------------------------------------------------------------
void cScene::PhysicStop()
{	
	if (p_worldHandle)
		plDeleteDynamicsWorld((plDynamicsWorldHandle)p_worldHandle);
	
	if (p_sdkHandle)
		plDeletePhysicsSdk(p_sdkHandle);
}
//-------------------------------------------------------------------------
plCollisionShapeHandle cScene::PhysicCreateShape(cObj* tObj)
{
	plCollisionShapeHandle p_Shape = plNewConvexHullShape();
	
	for (int i=0;i<tObj->totObjects;i++)
	{
		cSub* tSub = &tObj->subObj[i];
		for (int v=0;v<tSub->totVertex;v++)
			plAddVertex(p_Shape,tSub->vertex[v].x,tSub->vertex[v].y,tSub->vertex[v].z);
	}
	
	return p_Shape;
}
//-------------------------------------------------------------------------
//ATTENZIONE: updatare tutte le posizioni degli oggetti statici *se mossi*
void cObj::PhysicSetPos()
{
	//setta posizione e orientamento
	plVector3 p_position={position.x,position.y,-position.z};
	plSetPosition(p_Body, p_position);
	
	plQuaternion plQuat;
	plSetEuler(D2R(-rotation.y),D2R(-rotation.x),D2R(rotation.z),plQuat);

	plSetOrientation(p_Body, plQuat);
}

//-------------------------------------------------------------------------
//chiamare questa se l'oggetto e' dinamico ad ogni ciclo
void cObj::PhysicGetPos()
{
	if (p_static || !parent->p_worldHandle)
		return;
		
	plVector3 p_position;
	plGetPosition(p_Body,p_position);
	position.Set(p_position[0],p_position[1],-p_position[2]);
	
	btQuaternion orientation;
	plGetOrientation(p_Body,orientation);	
	double x,y,z;

	util_QuatToEuler(orientation.getX(),orientation.getY(),orientation.getZ(),orientation.getW(),x,y,z);
	rotation.Set(-x,-y,z);
	
	UpdateBoundBoxes();
	updatePos=TRUE;
}
//-------------------------------------------------------------------------
//DA BULLET - DemoApplication.cpp
//-------------------------------------------------------------------------

btVector3 cObj::getRayTo(int x,int y)
{
	// GIO
	int m_glutScreenWidth  = parent->resolutionX;
	int m_glutScreenHeight = parent->resolutionY;
	btVector3 cameraPos(parent->position.x,parent->position.y,parent->position.z);
	btVector3 cameraTargetPos(parent->position.x,parent->position.y,-10000);
	btVector3 m_cameraUp(0,1,0);
	//
	
	float top = 1.f;
	float bottom = -1.f;
	float nearPlane = 1.f;
	float tanFov = (top-bottom)*0.5f / nearPlane;
	float fov = btScalar(2.0) * btAtan(tanFov);

	btVector3 rayFrom(position.x,position.y,position.z);
	btVector3 rayForward = (cameraTargetPos - cameraPos);
	rayForward.normalize();
	float farPlane = -10000.f;
	rayForward*= farPlane;

	btVector3 rightOffset;
	btVector3 vertical = m_cameraUp;

	btVector3 hor;
	hor = rayForward.cross(vertical);
	hor.normalize();
	vertical = hor.cross(rayForward);
	vertical.normalize();

	float tanfov = tanf(0.5f*fov);

	hor *= 2.f * farPlane * tanfov;
	vertical *= 2.f * farPlane * tanfov;

	btScalar aspect;
	
	if (m_glutScreenWidth > m_glutScreenHeight) 
	{
		aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
		hor*=aspect;
	}
	else 
	{
		aspect = m_glutScreenHeight / (btScalar)m_glutScreenWidth;
		vertical*=aspect;
	}


	btVector3 rayToCenter = rayFrom + rayForward;
	btVector3 dHor = hor * 1.f/float(m_glutScreenWidth);
	btVector3 dVert = vertical * 1.f/float(m_glutScreenHeight);

	btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
	rayTo += btScalar(x) * dHor;
	rayTo -= btScalar(y) * dVert;
	return rayTo;
}
//-------------------------------------------------------------------------
//DA BULLET - DemoApplication.cpp
//-------------------------------------------------------------------------

static int m_mouseOldX = 0;
static int m_mouseOldY = 0;
static btRigidBody* pickedBody = NULL;
static btPoint2PointConstraint* m_pickConstraint = NULL;
static btVector3 gOldPickingPos;
static btVector3 gHitPos;
static float gOldPickingDist = 0;

void cObj::PhysicAddConstraint( int x, int y)
{

	m_mouseOldX = x;
    m_mouseOldY = y;

	btVector3 rayTo = getRayTo(x,y);

	//btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	btDynamicsWorld* m_dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(parent->p_worldHandle);
	
	//add a point to point constraint for picking
	if (m_dynamicsWorld)
	{
		btVector3 m_cameraPosition(parent->position.x,parent->position.y,parent->position.z); //GIO
		btVector3 rayFrom;
		rayFrom = m_cameraPosition;
					
		btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
		m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
		if (rayCallback.hasHit())
		{
			btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
			if (body)
			{
				//other exclusions?
				//if (!(body->isStaticObject() || body->isKinematicObject()))
				{
					pickedBody = body;
					pickedBody->setActivationState(DISABLE_DEACTIVATION);
					btVector3 pickPos = rayCallback.m_hitPointWorld;
					printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());

					btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
					
					btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
					m_dynamicsWorld->addConstraint(p2p);
					m_pickConstraint = p2p;
					p2p->m_setting.m_impulseClamp = 1;//0.30f;
					//very weak constraint for picking
					p2p->m_setting.m_tau = 0.001f;
					//p2p->setParam(BT_CONSTRAINT_CFM,0.8,0);
					//p2p->setParam(BT_CONSTRAINT_CFM,0.8,1);
					//p2p->setParam(BT_CONSTRAINT_CFM,0.8,2);
					//p2p->setParam(BT_CONSTRAINT_ERP,0.1,0);
					//p2p->setParam(BT_CONSTRAINT_ERP,0.1,1);
					//p2p->setParam(BT_CONSTRAINT_ERP,0.1,2);
						
					//save mouse position for dragging
					gOldPickingPos		= rayTo;
					gHitPos				= pickPos;
					
					gOldPickingDist		= (pickPos-rayFrom).length();
				}
			}
		}
	}
}
//-------------------------------------------------------------------------
//DA BULLET - DemoApplication.cpp
//-------------------------------------------------------------------------
void cObj::PhysicUpdateConstraint(int x,int y)
{

	// GIO
	int m_glutScreenWidth  = parent->resolutionX;
	int m_glutScreenHeight = parent->resolutionY;
	btVector3 m_cameraPosition(parent->position.x,parent->position.y,parent->position.z);
	btVector3 cameraTargetPos(parent->position.x,parent->position.y,-10000);
	btVector3 m_cameraUp(0,1,0);
	//
	
	if (m_pickConstraint)
	{
		//move the constraint pivot
		btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickConstraint);
		if (pickCon)
		{
			//keep it at the same picking distance
			
			btVector3 newRayTo = getRayTo(x,y);
			btVector3 rayFrom;
			btVector3 oldPivotInB = pickCon->getPivotInB();
			btVector3 newPivotB;
			
			rayFrom = m_cameraPosition;
			btVector3 dir = newRayTo-rayFrom;
			dir.normalize();
			dir *= gOldPickingDist;
			
			newPivotB = rayFrom + dir;
			
			pickCon->setPivotB(newPivotB);
		}
	}

	float dx, dy;
    dx = btScalar(x) - m_mouseOldX;
    dy = btScalar(y) - m_mouseOldY;

	m_mouseOldX = x;
    m_mouseOldY = y;
	//updateCamera();
}
//-------------------------------------------------------------------------
//
//-------------------------------------------------------------------------
void cObj::PhysicAddConstraint( float x, float y , float z)
{

	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(parent->p_worldHandle);
	
	if (body)
	{

		static btScalar mousePickClamping   = 30;//30;
		static btScalar mousePickTau		= 1;//0.001f;

		body->setActivationState(DISABLE_DEACTIVATION);
		
		btVector3 pickPos(x,y,z);
	#ifdef USE_ORTHO
		btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
	#else
		btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
	#endif
		p_constraint = new btPoint2PointConstraint(*body,localPivot);
		/*
		p_constraint->setParam(BT_CONSTRAINT_CFM,0.8,0);
		p_constraint->setParam(BT_CONSTRAINT_CFM,0.8,1);
		p_constraint->setParam(BT_CONSTRAINT_CFM,0.8,2);
		p_constraint->setParam(BT_CONSTRAINT_ERP,0.1,0);
		p_constraint->setParam(BT_CONSTRAINT_ERP,0.1,1);
		p_constraint->setParam(BT_CONSTRAINT_ERP,0.1,2);
		*/
		p_constraint->m_setting.m_impulseClamp = mousePickClamping;
		p_constraint->m_setting.m_tau = mousePickTau;
		
		dynamicsWorld->addConstraint(p_constraint);
	}
}
//-------------------------------------------------------------------------
//
//-------------------------------------------------------------------------
void cObj::PhysicUpdateConstraint( float x, float y, float z)
{
	return;
	
	if (p_constraint)
	{
		btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
		
		btVector3 pickPos(x,y,z);
	#ifdef USE_ORTHO
		btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
	#else
		btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
	#endif
	
		//p_constraint->setPivotA(localPivot);
		p_constraint->setPivotB(localPivot);
	}
}
//-------------------------------------------------------------------------
//
//-------------------------------------------------------------------------
void cObj::PhysicDeleteConstraint()
{
	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(parent->p_worldHandle);
	
	if (p_constraint && dynamicsWorld)
	{
		dynamicsWorld->removeConstraint(p_constraint);
		delete p_constraint;
		p_constraint = 0;
		body->forceActivationState(ACTIVE_TAG);
		body->setDeactivationTime( 0.f );
	}
}
//-------------------------------------------------------------------------
//nota: con massa a 0 l'oggetto diventa statico
//-------------------------------------------------------------------------
void cObj::PhysicAdd(plCollisionShapeHandle tShape,float tMass,float tFriction)
{
	p_static = (tMass == 0);
	
	//crea una shape di collisione e la riempie coi vertici dell'oggetto
	if (tShape == NULL)
		p_Shape = parent->PhysicCreateShape(this);
	else
		p_Shape = tShape;
		
	//crea un rigid body che usa la stessa shape di collisione
	p_Body = plCreateRigidBody(NULL,tMass,p_Shape);
#ifdef USE_Z_CONSTRAINT
	PhysicSet2DConstraint();
#endif
	plAddRigidBody(parent->p_worldHandle,p_Body);
	PhysicSetFriction(tFriction);	
	PhysicSetActivationState(true);
	PhysicSetPos();
}
//-------------------------------------------------------------------------
void cObj::PhysicApplyVelocity(float mx,float my,float mz,float rx,float ry,float rz)
{

	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	body->setLinearVelocity(btVector3(mx,my,mz));
	body->setAngularVelocity(btVector3(rx,ry,rz));
	
}
//-------------------------------------------------------------------------
void cObj::PhysicApplyImpulse(float mx,float my,float mz,float rposx,float rposy,float rposz)
{

	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	body->applyImpulse(btVector3(mx,my,mz),btVector3(rposx,rposy,rposz));
	
}
/*
void setAngularFactor(btScalar angFac)
body->setCcdMotionThreshold(1.);
*/

//-------------------------------------------------------------------------
void cObj::PhysicSetFriction(float friction)
{
	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	body->setFriction(friction);
}
//-------------------------------------------------------------------------
void cObj::PhysicSetDamping(float dump1,float dump2)
{
	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	body->setDamping(btScalar(dump1), btScalar(dump2));
	body->setDeactivationTime(0.8);
}
//-------------------------------------------------------------------------
void cObj::PhysicSetGravity(float gravx,float gravy,float gravz)
{
	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	const btVector3 temp(gravx,gravy,gravz);
	body->setGravity(temp);

}
//-------------------------------------------------------------------------
void cObj::PhysicSetActivationState(bool active)
{
	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	body->setActivationState((active==false)?WANTS_DEACTIVATION:DISABLE_DEACTIVATION);
}

//-------------------------------------------------------------------------
void cObj::PhysicSetBodyAngularVelocity(float ax,float ay,float az)
{
	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	const btVector3 temp(ax,ay,az);
	body->setAngularVelocity(temp);
}
//-------------------------------------------------------------------------
void cObj::PhysicSet2DConstraint()
{
	btRigidBody* body = reinterpret_cast< btRigidBody* >(p_Body);
	body->setLinearFactor(btVector3(1,1,0));
	body->setAngularFactor(btVector3(0,0,1));
}
//-------------------------------------------------------------------------

#endif


