#include "GyroscopicSetup.h"



#include "../CommonInterfaces/CommonRigidBodyBase.h"


struct GyroscopicSetup : public CommonRigidBodyBase
{

	GyroscopicSetup(struct GUIHelperInterface* helper);

	virtual ~GyroscopicSetup()
	{
	}
	virtual void initPhysics();


	virtual void physicsDebugDraw(int debugFlags);

	void resetCamera()
	{
		float dist = 20;
		float pitch = -16;
		float yaw = 180;
		float targetPos[3]={-2.4,0.4,-0.24};
		m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
	}

};


static int gyroflags[4] = {
    0,//none, no gyroscopic term
    BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT,
    BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD,
    BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
};

static const char* gyroNames[4] = {
    "No Gyroscopic",
    "Explicit",
    "Implicit (World)",
	"Implicit (Body)"
};


GyroscopicSetup::GyroscopicSetup(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{

}

void GyroscopicSetup::initPhysics()
{
	m_guiHelper->setUpAxis(1);
	createEmptyDynamicsWorld();
	m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);

	
	btVector3 positions[4] = {
		btVector3( -10, 8,4),
		btVector3( -5, 8,4),
		btVector3( 0, 8,4),
		btVector3( 5, 8,4),
	};


	for (int i = 0; i<4; i++)
	{
		btCylinderShapeZ* pin = new btCylinderShapeZ(btVector3(0.1,0.1, 0.2));
		btBoxShape* box = new btBoxShape(btVector3(1,0.1,0.1));
		box->setMargin(0.01);
		pin->setMargin(0.01);
		btCompoundShape* compound = new btCompoundShape();
		compound->addChildShape(btTransform::getIdentity(), pin);
		btTransform offsetBox(btMatrix3x3::getIdentity(),btVector3(0,0,0.2));
		compound->addChildShape(offsetBox, box);
		btScalar masses[2] = {0.3,0.1};
		btVector3 localInertia;
		btTransform principal;
		compound->calculatePrincipalAxisTransform(masses,principal,localInertia);
		
		btRigidBody* body = new btRigidBody(1, 0, compound, localInertia);
		btTransform tr;
		tr.setIdentity();
		tr.setOrigin(positions[i]);
		body->setCenterOfMassTransform(tr);
		body->setAngularVelocity(btVector3(0, 0.1, 10));//51));
		//body->setLinearVelocity(btVector3(3, 0, 0));
		body->setFriction(btSqrt(1));
		m_dynamicsWorld->addRigidBody(body);
		body->setFlags(gyroflags[i]);
		m_dynamicsWorld->getSolverInfo().m_maxGyroscopicForce = 10.f;
		body->setDamping(0.0000f, 0.000f);


	}

    {
        //btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(0.5)));
        btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 1, 0), 0);
        
        m_collisionShapes.push_back(groundShape);
        btTransform groundTransform;
        groundTransform.setIdentity();
        groundTransform.setOrigin(btVector3(0, 0, 0));
        btRigidBody* groundBody;
        groundBody = createRigidBody(0, groundTransform, groundShape);
        groundBody->setFriction(btSqrt(2));
    }
	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}

void GyroscopicSetup::physicsDebugDraw(int debugFlags)
{
	CommonRigidBodyBase::physicsDebugDraw(debugFlags);
    
    //render method names above objects
    for (int i=0;i<m_dynamicsWorld->getNumCollisionObjects();i++)
    {
        btRigidBody* body = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[i]);
        if (body && body->getInvMass()>0)
        {
            btTransform tr = body->getWorldTransform();
            btVector3 pos = tr.getOrigin()+btVector3(0,0,2);
            btScalar size=1;
            m_guiHelper->drawText3D(gyroNames[i],pos.x(),pos.y(),pos.z(),size);
        }
    }
}


class CommonExampleInterface*    GyroscopicCreateFunc(CommonExampleOptions& options)
{
	return new GyroscopicSetup(options.m_guiHelper);
}