/*
	OgreNewt library - connecting Ogre and Newton!

	Demo02_Joints - basic demo that shows how to connect rigid bodies via joints.
*/

#include "OgreNewtonApplication.h"
#include "OgreNewtonFrameListener.h"

#include <OgreNewt.h>
#include <OgreNewt_BasicFrameListener.h>



OgreNewtonApplication::OgreNewtonApplication(void)
{
	// create OgreNewt world.
	m_World = new OgreNewt::World();

	mEntityCount = 0;

}

OgreNewtonApplication::~OgreNewtonApplication(void)
{
	// destroy world.
	delete m_World;
}





void OgreNewtonApplication::createFrameListener()
{
	mNewtonListener = new OgreNewt::BasicFrameListener( mWindow, m_World, 100 );
	mRoot->addFrameListener(mNewtonListener);

	mFrameListener = new OgreNewtonFrameListener( mWindow, mCamera, mSceneMgr, m_World);
	mRoot->addFrameListener(mFrameListener);
}


OgreNewt::Body* OgreNewtonApplication::makeSimpleBox( Ogre::Vector3& size, Ogre::Vector3& pos, Ogre::Quaternion& orient )
{
	Entity* box1;
	SceneNode* box1node;

	box1 = mSceneMgr->createEntity( "Entity"+Ogre::StringConverter::toString(mEntityCount++), "box.mesh" );
	box1node = mSceneMgr->getRootSceneNode()->createChildSceneNode();
	box1node->attachObject( box1 );
	box1node->setScale( size );
//	box1->setNormaliseNormals(true);

	OgreNewt::ConvexCollisionPtr col (new OgreNewt::CollisionPrimitives::Box( m_World, size, 0 ));
	OgreNewt::Body* bod = new OgreNewt::Body( m_World, col );


	// base mass on the size of the object.
	Ogre::Real mass = size.x * size.y * size.z * 2.5;
		
	// calculate the inertia based on box formula and mass
	Ogre::Vector3 inertia, offset;
    col->calculateInertialMatrix(inertia, offset);

#ifdef OGRENEWT_NO_COLLISION_SHAREDPTR
	delete col;
#endif
				
	bod->attachNode( box1node );
	bod->setMassMatrix( mass, mass*inertia );
    bod->setCenterOfMass(offset);
	bod->setStandardForceCallback();

	box1->setMaterialName( "Simple/BumpyMetal" );


	bod->setPositionOrientation( pos, orient );

	return bod;
}


OgreNewt::Body* OgreNewtonApplication::makeSimpleShere( Ogre::Vector3& size, Ogre::Vector3& pos, Ogre::Quaternion& orient )
{
	Entity* box1;
	SceneNode* box1node;

	box1 = mSceneMgr->createEntity( "Entity"+Ogre::StringConverter::toString(mEntityCount++), "ellipsoid.mesh" );
	box1node = mSceneMgr->getRootSceneNode()->createChildSceneNode();
	box1node->attachObject( box1 );
	box1node->setScale( size * (1.0f / 0.5f));

	OgreNewt::ConvexCollisionPtr col = OgreNewt::ConvexCollisionPtr(new OgreNewt::CollisionPrimitives::Ellipsoid( m_World, size, 0 ));
	OgreNewt::Body* bod = new OgreNewt::Body( m_World, col );

	// base mass on the size of the object.
	Ogre::Real mass = size.x * size.y * size.z * 2.5;

	// calculate the inertia based on box formula and mass
	Ogre::Vector3 inertia, offset;
	col->calculateInertialMatrix(inertia, offset);

#ifdef OGRENEWT_NO_COLLISION_SHAREDPTR
	delete col;
#endif

	bod->attachNode( box1node );
	bod->setMassMatrix( mass, mass*inertia );
	bod->setCenterOfMass(offset);
	bod->setStandardForceCallback();

	box1->setMaterialName( "Simple/BumpyMetal" );


	bod->setPositionOrientation( pos, orient );

	return bod;
}



//static void AddSimpleRobot (dVector position, SceneManager* scene, NewtonWorld* nWorld)
void OgreNewtonApplication::SimpleRobot (Ogre::Vector3 pos, Ogre::Vector3 size)
{
	Ogre::Real radius;
	OgreNewt::Body* robotBody;
	OgreNewt::Body* leftWheel;
	OgreNewt::Body* rightWheel;
	OgreNewt::Body* frontWheel;
	OgreNewt::Joint* joint;

	radius = 0.25f;
	Ogre::Quaternion orient (Ogre::Quaternion::IDENTITY);

	// find location above the ground
	Ogre::Vector3 start(pos.x, 1000.0f, pos.z);
	Ogre::Vector3 end(pos.x, -1000.0f, pos.z);
	OgreNewt::BasicRaycast castRay (m_World, start, end, true);
	OgreNewt::BasicRaycast::BasicRaycastInfo info = castRay.getFirstHit();

	// place contraction at some location in above the floor
	pos.y = 2 * size.y + start.y + (end.y - start.y) * info.mDistance;

	// create the robot body
	robotBody = makeSimpleBox(size, pos, orient);

	// make the front wheel
	// connect the front wheel with a ball and socket joint
	Ogre::Vector3 frontWheelPos (pos.x, pos.y - size.y * 0.5f, pos.z + size.z * 0.65f);
	frontWheel = makeSimpleShere (Ogre::Vector3 (radius, radius, radius), frontWheelPos, orient);
	joint = new OgreNewt::BallAndSocket (frontWheel, robotBody, frontWheelPos );

	Ogre::Vector3 leftWheelPos (pos.x - size.x * 0.65f , pos.y - size.y * 0.5f, pos.z - size.z * 0.65f);
	leftWheel = makeSimpleShere (Ogre::Vector3 (radius, radius, radius), leftWheelPos, orient);
	joint = new OgreNewt::Hinge (leftWheel, robotBody, leftWheelPos, Ogre::Vector3(1.0f, 0.0f, 0.0f));

	Ogre::Vector3 rightWheelPos (pos.x + size.x * 0.65f, pos.y - size.y * 0.5f, pos.z - size.z * 0.65f);
	rightWheel = makeSimpleShere (Ogre::Vector3 (radius, radius, radius), rightWheelPos, orient);
	joint = new OgreNewt::Hinge (rightWheel, robotBody, rightWheelPos, Ogre::Vector3(1.0f, 0.0f, 0.0f));
}




void OgreNewtonApplication::BallAndSocketRope(Ogre::Vector3 pos, Ogre::Vector3 size)
{
	int segmentsCount = 5;
	Ogre::Quaternion orient (Ogre::Quaternion::IDENTITY);

	// find location above the ground
	Ogre::Vector3 start(pos.x, 1000.0f, pos.z);
	Ogre::Vector3 end(pos.x, -1000.0f, pos.z);
	OgreNewt::BasicRaycast castRay (m_World, start, end, true);
	OgreNewt::BasicRaycast::BasicRaycastInfo info = castRay.getFirstHit();
	
	pos.y = 0.5f * size.x + segmentsCount * size.x + start.y + (end.y - start.y) * info.mDistance;

	// loop through, making bodies and connecting them.
	OgreNewt::Body* parent = NULL;
	OgreNewt::Body* child = NULL;

	for (int x = 0; x < segmentsCount; x++)
	{
		// make the next box.
		child = makeSimpleBox(size, pos, orient);

		// now make a new joint connecting this to the last box.
		OgreNewt::Joint* joint;

		joint = new OgreNewt::BallAndSocket( child, parent, pos-Ogre::Vector3(size.x/2,0,0) );


		// offset pos a little more.
		pos += Ogre::Vector3(size.x,0,0);

		// save the last body for the next loop!
		parent = child;
	}
}

void OgreNewtonApplication::SwinDoors (Ogre::Vector3 pos, Ogre::Vector3 size)
{
	int segmentsCount = 3;
	Ogre::Quaternion orient (Ogre::Quaternion::IDENTITY);

	// find location above the ground
	Ogre::Vector3 start(pos.x, 1000.0f, pos.z);
	Ogre::Vector3 end(pos.x, -1000.0f, pos.z);
	OgreNewt::BasicRaycast castRay (m_World, start, end, true);
	OgreNewt::BasicRaycast::BasicRaycastInfo info = castRay.getFirstHit();

	pos.y = 0.25f + size.y * 0.5f + start.y + (end.y - start.y) * info.mDistance;

	// loop through, making bodies and connecting them.
	OgreNewt::Body* parent = NULL;
	OgreNewt::Body* child = NULL;

	for (int x = 0; x < segmentsCount; x++)
	{
		// make the next box.
		child = makeSimpleBox(size, pos, orient);

		// now make a new joint connecting this to the last box.
		OgreNewt::Hinge* joint;

		Ogre::Radian minAngle (-45.0f * 3.1416f / 180);
		Ogre::Radian maxAngle ( 45.0f * 3.1416f / 180);

		// create a hinge with limits
		joint = new OgreNewt::Hinge( child, parent, pos+Ogre::Vector3(size.x/2, 0, 0), Ogre::Vector3(0, 1, 0));
		joint->enableLimits(true);
		joint->setLimits(minAngle, maxAngle);

		// offset pos a little more.
		pos -= Ogre::Vector3(size.x,0,0);

		// save the last body for the next loop!
		parent = child;
	}
}


void OgreNewtonApplication::SlidingDoors (Ogre::Vector3 pos, Ogre::Vector3 size)
{
	int segmentsCount = 3;
	Ogre::Quaternion orient (Ogre::Quaternion::IDENTITY);

	// find location above the ground
	Ogre::Vector3 start(pos.x, 1000.0f, pos.z);
	Ogre::Vector3 end(pos.x, -1000.0f, pos.z);
	OgreNewt::BasicRaycast castRay (m_World, start, end, true);
	OgreNewt::BasicRaycast::BasicRaycastInfo info = castRay.getFirstHit();

	pos.y = 0.25f + size.y * 0.5f + start.y + (end.y - start.y) * info.mDistance;

	// loop through, making bodies and connecting them.
	OgreNewt::Body* parent = NULL;
	OgreNewt::Body* child = NULL;

	for (int x = 0; x < segmentsCount; x++)
	{
		// make the next box.
		child = makeSimpleBox(size, pos, orient);

		// now make a new joint connecting this to the last box.
		OgreNewt::Slider* joint;

		// create a hinge with limits
		joint = new OgreNewt::Slider( child, parent, pos+Ogre::Vector3(size.x/2, 0, 0), Ogre::Vector3(1, 0, 0));
		joint->enableLimits(true);
		joint->setLimis(0, size.x * 0.8f);

		// offset pos a little more.
		pos -= Ogre::Vector3(size.x,0,0);

		// save the last body for the next loop!
		parent = child;
	}
}


void OgreNewtonApplication::createScene()
{
	// sky box.
	mSceneMgr->setSkyBox(true, "Examples/CloudyNoonSkyBox", 500.0f);

	// shadows on!
	mSceneMgr->setShadowTechnique( Ogre::SHADOWTYPE_STENCIL_ADDITIVE );

	// floor object!
	Entity* floor;
	SceneNode* floornode;
//	floor = mSceneMgr->createEntity("Floor", "simple_terrain.mesh" );
	floor = mSceneMgr->createEntity("Floor", "playground.mesh" );
//	floor = mSceneMgr->createEntity("Floor", "castle.mesh" );
//	floor = mSceneMgr->createEntity("Floor", "xxx.mesh" );

	floornode = mSceneMgr->getRootSceneNode()->createChildSceneNode( "FloorNode" );
	floornode->attachObject( floor );
	floor->setCastShadows( false );

	// using the new "SceneParser" TreeCollision primitive.  this will automatically parse an entire tree of
	// SceneNodes (parsing all children), and add collision for all meshes in the tree.
	OgreNewt::CollisionPrimitives::TreeCollisionSceneParser* stat_col = new OgreNewt::CollisionPrimitives::TreeCollisionSceneParser( m_World );
	stat_col->parseScene( floornode, true, 1 );
	OgreNewt::Body* bod = new OgreNewt::Body( m_World, OgreNewt::CollisionPtr(stat_col) );
#ifdef OGRENEWT_NO_COLLISION_SHAREDPTR
	delete stat_col;
#endif

	bod->attachNode( floornode );
	bod->setPositionOrientation( Ogre::Vector3(0.0,-20.0,0.0), Ogre::Quaternion::IDENTITY );

	// add few demos of bodies connected by simple joints

	// demo of a simple hinges, a simple robot with hinge for wheels
	SimpleRobot (Ogre::Vector3 (0.0f, 0.0f, 0.0f), Ogre::Vector3 (1.0f, 1.0f, 1.5f));
	SimpleRobot (Ogre::Vector3 (-4.0f, 0.0f, 0.0f), Ogre::Vector3 (1.0f, 1.0f, 1.5f));
	SimpleRobot (Ogre::Vector3 ( 4.0f, 0.0f, 0.0f), Ogre::Vector3 (1.0f, 1.0f, 1.5f));

	// demos of hinge with limits, swing door 
	SwinDoors (Ogre::Vector3 (2.0f, 0.0f, -2.0f), Ogre::Vector3 (1.0f, 2.5f, 0.2f));

	// demos of hinge with limits, swing door 
	SlidingDoors (Ogre::Vector3 (-2.0f, 0.0f, -2.0f), Ogre::Vector3 (1.0f, 2.5f, 0.2f));


	// demo of simple ball and sockets, a simple rope.
	BallAndSocketRope(Ogre::Vector3 (1.5f, 0.0f, 0.0f), Ogre::Vector3 (1.0f, 0.5f, 0.5f));


	// set Ogre smoothing the FPS 
//	mRoot->setFrameSmoothingPeriod (1.0f/60.0f) ;

	// position camera
	Ogre::Vector3 start(0.0f, 1000.0f, 10.0f);
	Ogre::Vector3 end(0.0f, -1000.0f, 10.0f);
	OgreNewt::BasicRaycast castRay (m_World, start, end, true);
	OgreNewt::BasicRaycast::BasicRaycastInfo info = castRay.getFirstHit();
	mCamera->setPosition(start.x, 2.0f + start.y + (end.y - start.y) * info.mDistance, start.z);

	// set the near and far clip plane
	mCamera->setNearClipDistance(0.2f);
	mCamera->setFarClipDistance(1000.0f);

	//make a light
	Ogre::Light* light;

	light = mSceneMgr->createLight( "Light1" );
	light->setType( Ogre::Light::LT_POINT );
	light->setPosition( Ogre::Vector3(0.0, 100.0, 100.0) );
}