#include <tokamak.h>
#include <GL/glut.h>

#define NUM_CUBES 200

GLfloat light_position[] = {0.0f, 0.0f, 0.0f, 1.0f};
GLfloat diffuse_light[] = {0.9f, 0.9, 0.9f, 0.0f};
static int iteration = 0;

static GLint x = 30, y = 30;

float camX = 0.0f;
float camY = 0.0f;
float camZ = 0.0f;
float camAlpha = 0.0f;
float camBeta = 0.0f;
float camZeta = 60.0f;
float camera_state[4][4];
float oldX, oldY;
int firstTime = 1;
int controlling = 0;
GLfloat deltaX = 0;
GLfloat deltaY = 0;
//GLfloat fuerzaDeAvion = 0.01;

// Physics stuff
neSimulator* gSim = NULL;
neRigidBody* gCubes[NUM_CUBES];
neRigidBody* sphere;

neAnimatedBody* floor1 = NULL;
SYSTEMTIME time, oldTime;
neT3 t;
float degree = 0.0f;
float vel = 3.0f;

float getElapsedTime() {

	float etime = (time.wMilliseconds-oldTime.wMilliseconds);
	if (etime > 0) {
		time.wMilliseconds = oldTime.wMilliseconds;
		return etime;
	}
	etime = 1+oldTime.wMilliseconds-time.wMilliseconds;
	time.wMilliseconds = oldTime.wMilliseconds;
	
	return (etime);
}

void drawSphere(double resolution) {
	double steps = resolution;
	double c = 3.1415926/180.0;
	double x, y, z;
	// back slice to front slice (starting z deep)
	for (double phi = -90.0; phi<=90.0; phi+=steps) {
		glBegin(GL_QUAD_STRIP);
		// circle around z axis
		for (float theta=0.0; theta<=360.0; theta+=steps) {
			x=sin(c*theta)*cos(c*phi);
			y=cos(c*theta)*cos(c*phi);
			z=sin(c*phi);
			glNormal3d (x, y, z);
			glTexCoord2f(1-theta/360.0, (phi+90)/180);
			glVertex3d(x, y, z);
			x=sin(c*theta)*cos(c*(phi+steps));
			y=cos(c*theta)*cos(c*(phi+steps));
			z=sin(c*(phi+steps));
			glNormal3d(x, y, z);
			glTexCoord2f(1-theta/360.0, (phi+90+steps)/180);
			glVertex3d(x, y, z);
		}
		glEnd();
	}
}

void display () {
	glMatrixMode(GL_MODELVIEW);
	degree += 0.1f;
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	GetSystemTime(&time);
	//gSim->Advance(getElapsedTime()/100000.0f);
	gSim->Advance(0.015);
	glPushMatrix();
		glTranslatef (0, -5, -10.0f);
		//Floor
		glPushMatrix();
				t = floor1->GetTransform();
				camera_state[0][0] = t.rot[0][0]; camera_state[1][0] = t.rot[1][0]; camera_state[2][0] = t.rot[2][0]; camera_state[3][0] = t.pos[0];
				camera_state[0][1] = t.rot[0][1]; camera_state[1][1] = t.rot[1][1]; camera_state[2][1] = t.rot[2][1]; camera_state[3][1] = t.pos[1];
				camera_state[0][2] = t.rot[0][2]; camera_state[1][2] = t.rot[1][2]; camera_state[2][2] = t.rot[2][2]; camera_state[3][2] = t.pos[2];
				camera_state[0][3] = 0.0f; camera_state[1][3] = 0.0f; camera_state[2][3] = 0.0f; camera_state[3][3] = 1.0f;
				glMultMatrixf ((float *)camera_state);
			glBegin(GL_POLYGON);
			glVertex3f (-50.0f, 0, -50.0f);
			glVertex3f (50.0f, 0, -50.0f);
			glVertex3f (50.0f, 0, 50.0f);
			glVertex3f (-50.0f, 0, 50.0f);
			glEnd();
		glPopMatrix();
		//Cubes
		for (int i = 0; i < NUM_CUBES; i++) {
			glPushMatrix();
				t = gCubes[i]->GetTransform();
				camera_state[0][0] = t.rot[0][0]; camera_state[1][0] = t.rot[1][0]; camera_state[2][0] = t.rot[2][0]; camera_state[3][0] = t.pos[0];
				camera_state[0][1] = t.rot[0][1]; camera_state[1][1] = t.rot[1][1]; camera_state[2][1] = t.rot[2][1]; camera_state[3][1] = t.pos[1];
				camera_state[0][2] = t.rot[0][2]; camera_state[1][2] = t.rot[1][2]; camera_state[2][2] = t.rot[2][2]; camera_state[3][2] = t.pos[2];
				camera_state[0][3] = 0.0f; camera_state[1][3] = 0.0f; camera_state[2][3] = 0.0f; camera_state[3][3] = 1.0f;

				glMultMatrixf ((float *)camera_state);
				//glTranslatef (t.pos[0], t.pos[1], t.pos[2]);
				glutSolidCube(1.0f);
			glPopMatrix();
		}
		// Sphere
		glPushMatrix();
			t = sphere->GetTransform();
				camera_state[0][0] = t.rot[0][0]; camera_state[1][0] = t.rot[1][0]; camera_state[2][0] = t.rot[2][0]; camera_state[3][0] = t.pos[0];
				camera_state[0][1] = t.rot[0][1]; camera_state[1][1] = t.rot[1][1]; camera_state[2][1] = t.rot[2][1]; camera_state[3][1] = t.pos[1];
				camera_state[0][2] = t.rot[0][2]; camera_state[1][2] = t.rot[1][2]; camera_state[2][2] = t.rot[2][2]; camera_state[3][2] = t.pos[2];
				camera_state[0][3] = 0.0f; camera_state[1][3] = 0.0f; camera_state[2][3] = 0.0f; camera_state[3][3] = 1.0f;

				glMultMatrixf ((float *)camera_state);
				//glTranslatef (t.pos[0], t.pos[1], t.pos[2]);
				//glScalef(1, 1, 1);
				drawSphere(5);
		glPopMatrix();
	glPopMatrix();
	
	glutSwapBuffers();
	glutPostRedisplay();
}

void setupPhysicsEngine() {
	// This will define the size and shape of each cube
	neGeometry* geom;
	// length, width and height of the cube
	neV3 boxSize1; 
	neV3 gravity;
	neV3 pos;
	float mass;
	float fmass = 0.2f;
	// The number of total objects the simulator has to keep track of...
	neSimulatorSizeInfo sizeInfo;

	
	// Fill in the size info about the environment
	sizeInfo.rigidBodiesCount = NUM_CUBES+1;
	sizeInfo.animatedBodiesCount = 1;
	// total number of objects
	sizeInfo.geometriesCount = sizeInfo.rigidBodiesCount + sizeInfo.animatedBodiesCount;
	// total number of collisions possible  n*(n-1)/2
	sizeInfo.overlappedPairsCount = sizeInfo.geometriesCount*(sizeInfo.geometriesCount-1)/2;
	sizeInfo.rigidParticleCount = 0;
	sizeInfo.constraintsCount = 0;
	sizeInfo.terrainNodesStartCount = 0;

	gravity.Set(0.0f, -3.0f, 0.0f);
	gSim = neSimulator::CreateSimulator(sizeInfo, NULL, &gravity);
	
	// Setup a box - using loop
	for (int i = 0; i < NUM_CUBES; i++) {
		gCubes[i] = gSim->CreateRigidBody();
		// Get the geometry object from the cube
		geom = gCubes[i]->AddGeometry();
		boxSize1.Set(1.0f, 1.0f, 1.0f);
		geom->SetBoxSize(boxSize1[0], boxSize1[1], boxSize1[2]);
		gCubes[i]->UpdateBoundingInfo();
		mass = 1.0f;
		gCubes[i]->SetInertiaTensor(neBoxInertiaTensor(boxSize1[0], boxSize1[1], boxSize1[2], mass));
		gCubes[i]->SetMass(mass);
		pos.Set(i%10-5, i/10+0.5, -30);
		gCubes[i]->SetPos(pos);
		//pos.Set(0, i, 0);
		//gCubes[0]->SetRotation(pos);
		//gCubes[0]->SetVelocity(pos);
		// end loop
	}


	// Create the sphere
	sphere = gSim->CreateRigidBody();
	geom = sphere->AddGeometry();
	geom->SetSphereDiameter(2);
	sphere->UpdateBoundingInfo();
	sphere->SetInertiaTensor(neSphereInertiaTensor(2, fmass));
	sphere->SetMass(fmass);
	pos.Set(0, 1, -4);
	sphere->SetPos(pos);
	sphere->SetAngularDamping(0.01f);
	
	// Create the floor
	floor1 = gSim->CreateAnimatedBody();
	geom = floor1->AddGeometry();
	boxSize1.Set(100, 0.001, 100);
	geom->SetBoxSize(boxSize1[0], boxSize1[1], boxSize1[2]);
	floor1->UpdateBoundingInfo();
	pos.Set(0, 0, 0);
	floor1->SetPos(pos);
	
}


void teclas (unsigned char c, int x1, int y1) {
	neV3 pos;
	neV3 ballVel;
	char d = (char)c;
	switch (d) {
		case 27: exit(1);
			break;
		case 'r': setupPhysicsEngine();
			break;
		case '1': vel = 1;
			break;
		case '2': vel = 2;
			break;
		case '3': vel = 3;
			break;
		case '4': vel = 4;
			break;
		case '5': vel = 5;
			break;
		case '6': vel = 6;
			break;
		case '7': vel = 7;
			break;
		case '8': vel = 8;
			break;
		case '9': vel = 20;
			break;
		case 'm':
			pos = gCubes[0]->GetPos();
			pos.Set(-10, 0, 0);
			gCubes[10]->SetPos(pos);
			break;
		case 'j': ballVel.Set (0, 5, -vel*20);
			sphere->SetVelocity(ballVel);
			break;

	}
}

void killPhysics(void)
{
    if (gSim)
    {
        // Destroy the simulator.
        // Note that this will release all related resources that we've allocated.
        neSimulator::DestroySimulator(gSim);
        gSim = NULL;
    }
}

int main (int argv, char** argc) {
	glutInit (&argv, argc);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
	glutCreateWindow ("Tokamak Test");
	glutDisplayFunc (display);
	
	// Setup viewing
	glMatrixMode (GL_PROJECTION);
	glLoadIdentity();
	gluPerspective (60.0f, 1.5, 0.1, 1000);
	glMatrixMode (GL_MODELVIEW);
	
	glEnable (GL_DEPTH_TEST);
	glEnable (GL_LIGHTING);
	glEnable (GL_LIGHT0);

	// Setup Physics engine
	setupPhysicsEngine();

	glLightfv (GL_LIGHT0, GL_POSITION, light_position);
	glLightfv (GL_LIGHT0, GL_DIFFUSE, diffuse_light);
	
	glutKeyboardFunc(teclas);
	glutFullScreen();
	glutMainLoop();
}