#include "viewer.h"
#include "object.h"
#include "cube.h"
#include <GL/glut.h>

using namespace std;
using namespace qglviewer;

namespace
{
	void getAABB(const QVector<Object *>& objects,dReal aabb[6])
	{
		dGeomGetAABB(objects[0]->geom,aabb);
		foreach (Object *o,objects)
		{
			dReal aabb[6];
			dGeomGetAABB(o->geom,aabb);
			for (int i=0;i<3;++i)
			{
				aabb[  i] = min(aabb[  i],aabb[  i]);
				aabb[3+i] = max(aabb[3+i],aabb[3+i]);
			}
		}
	}
	void renderPlane(dVector4 equation,float size)
	{
		Vec z = Vec(equation).unit();
		Vec o = equation[3]*z;
		Vec x = z.orthogonalVec().unit();
		Vec y = (z^x).unit();
		glBegin(GL_QUADS);
		glNormal3fv(z);
		glVertex3fv(o-size*x-size*y);
		glVertex3fv(o+size*x-size*y);
		glVertex3fv(o+size*x+size*y);
		glVertex3fv(o-size*x+size*y);
		glEnd();
	}
	void nearCallback(void *data, dGeomID o0, dGeomID o1)
	{
		reinterpret_cast<Viewer*>(data)->handleCollisionBetween(o0,o1);
	}	
}

Viewer::Viewer(QWidget *parent)
	: QGLViewer(QGLFormat(QGL::SampleBuffers |
												QGL::DoubleBuffer |
												QGL::DepthBuffer |
												QGL::Rgba |
												QGL::AlphaChannel |
												QGL::StencilBuffer),parent)
{
	////////////////////////////////////////////////////////////
	// Create world, collision space and contact group
	////////////////////////////////////////////////////////////
	_world = dWorldCreate();
	_space = dHashSpaceCreate(0);
	// Set up gravity force
	dWorldSetGravity (_world,0,0,-9.81);
	// Create contact group
	_contactgroup = dJointGroupCreate(0); 	
	////////////////////////////////////////////////////////////
	// Creating objects
	////////////////////////////////////////////////////////////s
	static const int nb = 5;
	for (int k=0;k<3;++k)
	{
		float z = 2.0+0.15*k;
		for (int i=-nb;i<=nb;++i)
		{
			float x = 0.15*i+(k%2)*0.01;
			for (int j=-nb;j<=nb;++j)
			{
				float y = 0.15*j+(k%2)*0.01;
				Cube *cube = new Cube(_world,_space,0.1,0.1,0.1,1);
				cube->setPosition(x,y,z);
				cube->color[0] = qrand()%255;
				cube->color[1] = qrand()%255;
				cube->color[2] = qrand()%255;
				_objects.push_back(cube);
			}
		}
	}
	// Create static planes
	_planes.push_back(dCreatePlane(_space,0,0,1,0));
	_planes.push_back(dCreatePlane(_space,1,0,1,0.5));
	// Compute the bounding box
	getAABB(_objects,_aabb);
}
Viewer::~Viewer()
{
	//////////////////////////////////////////////////////////////
	// Deleting objects
	//////////////////////////////////////////////////////////////
	qDeleteAll(_objects);
	//////////////////////////////////////////////////////////////
	// Terminate with ODE
	//////////////////////////////////////////////////////////////
	dSpaceDestroy(_space);
	dWorldDestroy(_world);
	dCloseODE();	
}
void Viewer::init()
{
	Vec min(_aabb); 
	Vec max(_aabb+3);
	Vec center = (min+max)/2.0f;
	center.z = 0.0f;
	setSceneRadius((max-min).norm());
	setSceneCenter(center);
	// Try to restore previous state or focus the whole scene
	if (!restoreStateFromFile())
	{
		showEntireScene();
	}
	// Setup OpenGL
	GLfloat ambient[] = { 0.3f, 0.3f, 0.3f };
	GLfloat diffuse[] = { 0.1f, 0.1f, 0.1f , 1.0f};
	GLfloat specular[] = { 0.0f, 0.0f, 0.0f , 1.0f};
	glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);	
	glLightfv(GL_LIGHT0, GL_SPECULAR, specular);	
}
void Viewer::draw()
{
	// Draw the objects
	foreach (Object *o,_objects)
	{
		o->render();
	}
	// Draw the planes
	qglColor(Qt::white);
	foreach (dGeomID g,_planes)
	{
		if (dGeomGetClass(g))
		{
			dVector4 equation;
			dGeomPlaneGetParams(g,equation);
			renderPlane(equation,sceneRadius());
		}
	}
}
void Viewer::startAnimation()
{
	_time.start();
	QGLViewer::startAnimation();
}
void Viewer::animate()
{
	static float nbSecondsByStep = 0.001f; 
	
	// Find the time elapsed between last time
	float nbSecsElapsed = _time.elapsed()/1000.0f;
	// Find the corresponding number of steps that must be taken
	int nbStepsToPerform = static_cast<int>(nbSecsElapsed/nbSecondsByStep);
	// Make these steps to advance world time
	for (int i=0;i<nbStepsToPerform;++i)
	{
		// Detect collision
		dSpaceCollide(_space,this,&nearCallback);
		// Step world
		dWorldQuickStep(_world, nbSecondsByStep);
		// Remove all temporary collision joints now that the world has been stepped
		dJointGroupEmpty(_contactgroup);	
	}
	// Restart the elapsed time counter
	_time.restart();
}
void Viewer::handleCollisionBetween(dGeomID o0, dGeomID o1)
{
		// Create an array of dContact objects to hold the contact joints
		static const int MAX_CONTACTS = 10;
		dContact contact[MAX_CONTACTS];

		for (int i = 0; i < MAX_CONTACTS; i++)
		{
			contact[i].surface.mode = dContactBounce | dContactSoftCFM;
			contact[i].surface.mu = dInfinity;
			contact[i].surface.mu2 = 0;
			contact[i].surface.bounce = 0.8;
			contact[i].surface.bounce_vel = 0.1;
			contact[i].surface.soft_cfm = 0.01;
		}
		if (int numc = dCollide(o0, o1, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)))
		{
			// Get the dynamics body for each geom
			dBodyID b1 = dGeomGetBody(o0);
			dBodyID b2 = dGeomGetBody(o1);
			// To add each contact point found to our joint group we call dJointCreateContact which is just one of the many
			// different joint types available.  
			for (int i = 0; i < numc; i++)
			{
				// dJointCreateContact needs to know which world and joint group to work with as well as the dContact
				// object itself. It returns a new dJointID which we then use with dJointAttach to finally create the
				// temporary contact joint between the two geom bodies.
				dJointID c = dJointCreateContact(_world, _contactgroup, contact + i);
				dJointAttach(c, b1, b2);
			}
		}	
}