// Castle.cpp : Defines the entry point for the console application.
//

/*********************Includes*******************************/
#include <iostream>
#include <list>
#include <assert.h>
#include <GL/glut.h>
#include "ODESphere.h"
#include "ODEBox.h"
#include "TimerPC.h"
#include "ODEWorld.h"
#include "SkyBox.h"
#include "ODEHuman.h"
#include "ODETrebuchet.h"
#include "ODECameraPC.h"
#include "ODETrimesh.h"
#include "ODECappedCylinder.h"
#include "LightObject.h"
#include "ODESchoolOfFish.h"
#include "WaterParticleSystem.h"
#include "FountainParticleSystem.h"
#include "FireParticleSystem.h"
#include "SmokeParticleSystem.h"
/************************************************************/

/*********************Prototypes*****************************/
//Note: Using VRJuggler-like functions to make porting easier.
void init();
void apiInit();
//void bufferPreDraw(); //not used here
void preFrame();
void intraFrame();
void postFrame();
void contextInit();
void draw();
void Idle();
void MouseButtons(int button, int button_state, int x, int y);
void MouseMovement(int x, int y);
void KeyboardAction(unsigned char key, int x, int y);
void KeyboardSpecialFunc(int key, int x, int y);
void Reshape(int w, int h);
void ShootObject(int obj);
void Create3DObjects();
void AdjustUnderwaterForces();
void TryToGrabObject();
dReal Distance3D(point3d point1, point3d point2);
void UpdateObjectInHand();
void DropObject();
void BreakBridgeJoint();
/************************************************************/

/*********************Defines********************************/
#define GRAVITY -9.81*5
#define UNDERWATER_FORCE 9.81*10
#define UNDERWATER_DAMPING_FACTOR 1.03 //ranges from 1 to ~1.05
#define WATER_LEVEL 0
#define BODY_SIZE 1.3
#define GRABBING_DISTANCE 10
#define HAND_DISTANCE_FROM_CAMERA 10
#define PARTICLE_TEXTURE_FILE "data/water_particle.jpg"

typedef struct point2i
{
	int x, y;
} point2i;

#ifdef VJ_API_OPENGL
#define USING_VRJUGGLER
#include <vrj/Draw/OGL/GlContextData.h>
#endif
/************************************************************/

/*********************Global Variables***********************/
int main_window;
dReal aspect_ratio;
int windowWidth = 800;
int windowHeight = 600;
ODEObject* pickedObject = NULL;
bool LeftMouseButton = false;
bool RightMouseButton = false;
//point2i oldMousePos = {-1, -1};
int oldDeltaMouse[2] = {0, 0}; //used for smooth mouse motion
std::list<Base3DObject*> objectList;
std::list<Base3DObject*>::iterator iter;
std::list<BaseParticleSystem*> particleSystemList;
std::list<BaseParticleSystem*>::iterator particleSystemIter;
std::list<Base3DObject*> transparencyList;
std::list<Base3DObject*>::iterator transparencyIter;
Timer timer;
dReal deltaTime;
SkyBox* skybox=NULL;
ODECamera* camera=NULL;
point3d initialCameraPosition = {-118, 140, -130};
//point3d initialCameraPosition = {-293, 23, 177};
LightObject* light0;

//#ifdef USING_VRJUGGLER //using Juggler's special context-specific data types
//		vrj::GlContextData<GLuint> waterParticlesDisplayListID;
//#else //using normal data types
//		GLuint waterParticlesDisplayListID;
//#endif

//ODE objects
Base3DObject* objectPtr=NULL;
ODESphere* ball=NULL;
ODEWorld* world=NULL;
NeuralNet* brain=NULL;
int brainSize[3] = {2, 5, 5};
ODEHuman* humanPtr=NULL;
ODETrebuchet* trebuchet=NULL;
ODEObject* objectInHand=NULL;
dJointID bridgeSupport1;
dJointID bridgeSupport2;
dJointID centerBridgeLink;
ODEBox* burningPlank=NULL;
ODEBox* bridgeSection1=NULL;
ODEBox* bridgeSection2=NULL;
//dGeomID bridgeFireGeom1, bridgeFireGeom2, plankFireGeom;
BaseParticleSystem* plankFire=NULL;
BaseParticleSystem* plankSmoke=NULL;
//BaseParticleSystem* bridgeFire1=NULL;
//BaseParticleSystem* bridgeSmoke1=NULL;
//BaseParticleSystem* bridgeFire2=NULL;
//BaseParticleSystem* bridgeSmoke2=NULL;
/************************************************************/


int main(int argc, char** argv)
{
	//init stuff
	glutInit(&argc, argv);
	init();

	//initialize things that may need OpenGL
	apiInit();

	//setting up OpenGL
	contextInit();

	//starting the main loop
	glutMainLoop();
}

void init()
{
	srand(time(NULL));
	deltaTime = 0;

	//setting up GLUT; using GLUT game mode
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	main_window = glutEnterGameMode();
	glutSetWindow(main_window);
	glutDisplayFunc(draw);
	glutReshapeFunc(Reshape);
	glutMouseFunc(MouseButtons);
	glutPassiveMotionFunc(MouseMovement); 
	glutKeyboardFunc(KeyboardAction);
	glutSpecialFunc(KeyboardSpecialFunc);
	//glutIgnoreKeyRepeat(false); //set key repeat mode
	glutSetCursor(GLUT_CURSOR_NONE); //make cursor invisible
	glutIdleFunc(Idle);

	//OLD WAY...
	////setting up GLUT
	//glutInit(&argc, argv);
	//glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	//glutInitWindowSize(windowWidth, windowHeight);
	//glutInitWindowPosition(150, 100);
	//main_window = glutCreateWindow("Castle");
	//glutDisplayFunc(Redraw);
	//glutReshapeFunc(Reshape);
	//glutMouseFunc(MouseButtons);
	//glutMotionFunc(MouseMovement);
	//glutKeyboardFunc(KeyboardAction);
	//glutSpecialFunc(KeyboardSpecialFunc);
	//glutIdleFunc(Idle);
	//glutFullScreen();

	//setting up the ODE world
	world = new ODEWorld(GRAVITY);
}

void apiInit()
{
	//setting up the 3D objects in the world
	std::cout << "Loading models..." << std::endl;
	Create3DObjects();

	//resetting the timer
	timer.Reset();
}

void preFrame()
{
	deltaTime = timer.GetElapsedSeconds();

	if (deltaTime > 0.1 || deltaTime <= 0) 
	{
		return; //we don't want to deal with these situations
	}

	//animate human
	if (humanPtr!=NULL)
		humanPtr->UseBrain();

	//check for splashes -> this must come before objects' Update() function
	for (iter = objectList.begin(); iter != objectList.end(); iter++)
	{
		if ((*iter)->IsODEObject() && ((ODEObject*)(*iter))->HasBody())
		{
			point3d currentPosition = ((ODEObject*)(*iter))->GetPosition();
			point3d lastPosition = ((ODEObject*)(*iter))->GetLastPosition();
			if (lastPosition.y > 0 && currentPosition.y < 0)
			{
				dReal yVelocity = ((ODEObject*)(*iter))->GetVelocity().y;
				yVelocity /= (int)-10.0;
				if (yVelocity <= 1)
				{
					yVelocity = 1;
				}
				else if (yVelocity > 10)
				{
					yVelocity = 10;
				}

				BaseParticleSystem* waterPtr = 
					new WaterParticleSystem(currentPosition.x, currentPosition.y, 
					currentPosition.z, yVelocity, false, PARTICLE_TEXTURE_FILE);
				particleSystemList.push_back(waterPtr);

				//in Juggler, make the display list dynamically in ContextPreDraw()
				waterPtr->MakeDisplayList();
			}
		}
	}

	bool particleSystemActive;

	for (particleSystemIter = particleSystemList.begin();
		particleSystemIter != particleSystemList.end(); particleSystemIter++)
	{
		particleSystemActive = (*particleSystemIter)->Update(deltaTime);

		if (particleSystemActive == false)
		{
			std::list<BaseParticleSystem*>::iterator tempIter = particleSystemIter;
			particleSystemIter--;
			delete (*tempIter);
			particleSystemList.erase(tempIter);
		}
	}

	//adjust position of object in hand
	UpdateObjectInHand();

	//adjust objects that are underwater
	AdjustUnderwaterForces();

	//update particles systems' positions
	//const dReal* firePos = dGeomGetPosition(plankFireGeom);
	point3d endOfPlank = {0, 0, 0};// = burningPlank->GetPosition();
	endOfPlank.z += burningPlank->GetZDimension()/2;
	dVector3 firePos;
	dBodyGetRelPointPos (burningPlank->GetBodyID(), endOfPlank.x, endOfPlank.y, endOfPlank.z,
                          firePos);

	point3d firePosition = {firePos[0], firePos[1], firePos[2]};
	//firePosition.x += burningPlank->GetXDimension()/2;
	if (plankFire != NULL)
	{
		plankFire->SetPosition(firePosition);
	}
	if (plankSmoke != NULL)
	{
		plankSmoke->SetPosition(firePosition);
	}

	////update particles systems' positions
	////const dReal* firePos = dGeomGetPosition(plankFireGeom);
	//point3d endOfBridge = {0, 0, 0};// = burningPlank->GetPosition();
	//endOfBridge.x += bridgeSection1->GetXDimension()/2;
	//dBodyGetRelPointPos (bridgeSection1->GetBodyID(), endOfBridge.x, endOfBridge.y, endOfBridge.z,
 //                         firePos);

	//firePosition.x = firePos[0];
	//firePosition.y = firePos[1];
	//firePosition.z = firePos[2];
	////firePosition.x += burningPlank->GetXDimension()/2;
	//if (plankFire != NULL)
	//{
	//	bridgeFire1->SetPosition(firePosition);
	//}
	//if (plankSmoke != NULL)
	//{
	//	bridgeSmoke1->SetPosition(firePosition);
	//}

	////update particles systems' positions
	////const dReal* firePos = dGeomGetPosition(plankFireGeom);
	//endOfBridge.x=0;
	//endOfBridge.x += bridgeSection2->GetXDimension()/2;
	//dBodyGetRelPointPos (bridgeSection2->GetBodyID(), endOfBridge.x, endOfBridge.y, endOfBridge.z,
 //                         firePos);

	//firePosition.x = firePos[0];
	//firePosition.y = firePos[1];
	//firePosition.z = firePos[2];
	////firePosition.x += burningPlank->GetXDimension()/2;
	//if (plankFire != NULL)
	//{
	//	bridgeFire2->SetPosition(firePosition);
	//}
	//if (plankSmoke != NULL)
	//{
	//	bridgeSmoke2->SetPosition(firePosition);
	//}

	//give all objects a chance to update things
	for (iter = objectList.begin(); iter != objectList.end(); iter++)
	{
		(*iter)->Update(deltaTime);
	}

	//give all objects a chance to update things
	for (transparencyIter = transparencyList.begin(); transparencyIter != transparencyList.end(); 
		transparencyIter++)
	{
		(*transparencyIter)->Update(deltaTime);
	}

	//call ODE loop
	world->ODELoop(deltaTime); // handle collisions and dynamics

	if (glutGetWindow() != main_window) 
		glutSetWindow(main_window);  

	//reset pointer to center of window
	glutWarpPointer(windowWidth/2, windowHeight/2);
}

void intraFrame()
{
	glutPostRedisplay();
}

void postFrame()
{
	//nothing
}

void contextInit()
{
	glClearColor(0.9, 0.9, 0.9, 1.0);
	glColor3f(0.0, 0.0, 0.0);
	glPointSize(4.0);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(40.0, aspect_ratio, 3, 1500.0);
	glMatrixMode(GL_MODELVIEW);

	glEnable(GL_TEXTURE_2D);	// Turn on texture mapping
	glEnable(GL_DEPTH_TEST);

	glCullFace(GL_BACK); // don't draw the back of faces
	glEnable(GL_CULL_FACE);

	float fogcolor[4] = {.7, .7, .6, 1};
	glFogfv(GL_FOG_COLOR, fogcolor);
	//glFogi(GL_FOG_DENSITY, 1);
	glFogi(GL_FOG_START, 0);
	glFogi(GL_FOG_END, 600);
	glFogi(GL_FOG_MODE, GL_LINEAR);
	glEnable(GL_FOG);

	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	
	//The blending function now gets changed by specific objects.
	//glBlendFunc(GL_SRC_ALPHA,GL_SRC_COLOR);
	//glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);

	glEnable(GL_NORMALIZE);

	glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 0); //don't light the back of faces
	glEnable(GL_LIGHTING);

	//make display lists here
	for (iter = objectList.begin(); iter != objectList.end(); iter++)
	{
		(*iter)->MakeDisplayList();
	}

	for (transparencyIter = transparencyList.begin(); transparencyIter != transparencyList.end(); 
		transparencyIter++)
	{
		(*transparencyIter)->MakeDisplayList();
	}

	for (particleSystemIter = particleSystemList.begin(); 
		particleSystemIter != particleSystemList.end(); particleSystemIter++)
	{
		(*particleSystemIter)->MakeDisplayList();
	}

//	WaterParticleSystem tempWaterSystem(0, 0, 0, 1, false, PARTICLE_TEXTURE_FILE);
//	tempWaterSystem.MakeDisplayList();
//#ifdef USING_VRJUGGLER //using Juggler's special context-specific data types
//	*waterParticlesDisplayListID = tempWaterSystem.GetDisplayList();
//#else //using normal data types
//	waterParticlesDisplayListID = tempWaterSystem.GetDisplayList();
//#endif
}

void draw()
{
	glClearColor(0.9, 0.9, 0.9, 1.0);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glLoadIdentity();

	//place camera
	camera->PlaceCamera();

	for (iter = objectList.begin(); iter != objectList.end(); iter++)
	{
		(*iter)->Draw();
	}

	for (transparencyIter = transparencyList.begin(); transparencyIter != transparencyList.end(); 
		transparencyIter++)
	{
		(*transparencyIter)->Draw();
	}

	for (particleSystemIter = particleSystemList.begin(); 
		particleSystemIter != particleSystemList.end(); particleSystemIter++)
	{
		(*particleSystemIter)->Draw();
	}

	glutSwapBuffers(); 
}

void Idle()
{
	preFrame();
	intraFrame();
	postFrame();
}

void Create3DObjects()
{
	//creating skybox
	objectPtr = new SkyBox(0, 200, 0, 1200, 600, 1200);
	skybox = ((SkyBox*)(objectPtr));
	objectList.push_back(objectPtr);

	//creating light source
	objectPtr = new LightObject(0, 0, 100, -500);
	light0 = ((LightObject*)(objectPtr));
	objectList.push_back(objectPtr);

	//creating camera
	objectPtr = new ODECamera(world, initialCameraPosition.x, initialCameraPosition.y, 
		initialCameraPosition.z, 10);
	camera = ((ODECamera*)(objectPtr));
	camera->SetFloats(true);
	objectList.push_back(objectPtr);

	//creating terrain trimesh
	objectPtr = new ODETrimesh(world, 0, 0, 0, "data", "terrain.obj");
	objectList.push_back(objectPtr);

	//creating school of fish1
	objectPtr = new ODESchoolOfFish(world, 25, -22, -264, 3, &objectList);
	objectList.push_back(objectPtr);

	////creating school of fish2
	//objectPtr = new ODESchoolOfFish(world, 19, -18, 29, 4, &objectList);
	//objectList.push_back(objectPtr);

	//creating school of fish3
	objectPtr = new ODESchoolOfFish(world, -259, -6, 263, 3, &objectList);
	objectList.push_back(objectPtr);

	//creating bridge
	objectPtr = new ODEBox(world, false, true, "data", "bridge.obj", 10, 117, -100, 0, 0, 0);
	objectList.push_back(objectPtr);

	//creating trebuchet
	objectPtr = new ODETrebuchet(world, -118, 134, -155);
	trebuchet = ((ODETrebuchet*)(objectPtr));
	objectList.push_back(objectPtr);

	//creating platform by trebuchet
	objectPtr = new ODEBox(world, false, true, "data", "platform.obj", -139, 125, -155, 0, 0, 0);
	//((ODEObject*)objectPtr)->SetOrientation(2, PI/6);
	objectList.push_back(objectPtr);

	//creating brick1
	objectPtr = new ODEBox(world, true, true, "data", "small_brick_piece.obj", -142, 130, -155, 0, 0, 0);
	objectList.push_back(objectPtr);

	//creating brick2
	objectPtr = new ODEBox(world, true, true, "data", "small_brick_piece.obj", -139, 130, -155, 0, 0, 0);
	objectList.push_back(objectPtr);

	//creating brick3
	objectPtr = new ODEBox(world, true, true, "data", "small_brick_piece.obj", -140, 130, -153, 0, 0, 0);
	objectList.push_back(objectPtr);

	//creating dead fish1
	objectPtr = new ODEBox(world, true, true, "data", "codfish.obj", -138, 135, -152, 0, 0, 0);
	((ODEObject*)(objectPtr))->SetFloats(true);
	objectList.push_back(objectPtr);

	//creating dead fish2
	objectPtr = new ODEBox(world, true, true, "data", "codfish.obj", -140, 135, -158, 0, 0, 0);
	((ODEObject*)(objectPtr))->SetFloats(true);
	objectList.push_back(objectPtr);

	//creating plank1
	objectPtr = new ODEBox(world, true, true, "data", "small_plank.obj", -48, 128, -100, 0, 0, 0);
	((ODEObject*)objectPtr)->SetOrientation(1, PI/6);
	((ODEObject*)(objectPtr))->SetFloats(true);
	objectList.push_back(objectPtr);

	//creating plank2
	burningPlank = new ODEBox(world, true, true, "data", "small_plank.obj", 10, 128, -93, 0, 0, 0);
	//plankFireGeom = dCreateBox(world->GetSpaceID(), 0.1, 0.1, 0.1);
	//dGeomSetBody(plankFireGeom, burningPlank->GetBodyID());
	//dGeomSetPosition(plankFireGeom, 10-burningPlank->GetXDimension(), 128, -93);
	burningPlank->SetOrientation(1, PI/3);
	burningPlank->SetFloats(true);
	objectList.push_back(burningPlank);

	//creating plank3
	objectPtr = new ODEBox(world, true, true, "data", "small_plank.obj", 50, 128, -107, 0, 0, 0);
	((ODEObject*)objectPtr)->SetOrientation(1, -PI/5);
	((ODEObject*)(objectPtr))->SetFloats(true);
	objectList.push_back(objectPtr);

	// Diagram of Towers
	//
	//			o		#3
	//
	//	o			o	#2, #4
	//
	//		o			#1

	//create tower 1
	//objectPtr = new ODECappedCylinder(world, false, true, "data", "tower.obj", 250, 150, -130, 15, 60);
	objectPtr = new ODEBox(world, false, true, "data", "tower.obj", 185, 150, -160, 0, 0, 0);
	objectList.push_back(objectPtr);

	//create square wall section 1
	objectPtr = new ODEBox(world, false, true, "data", "square_brick_piece.obj", 195, 135, -170, 0, 0, 0);
	((ODEObject*)objectPtr)->SetOrientation(1, PI/4);
	objectList.push_back(objectPtr);

	//create tower 2
	//objectPtr = new ODECappedCylinder(world, false, true, "data", "tower.obj", 250, 150, -130, 15, 60);
	objectPtr = new ODEBox(world, false, true, "data", "tower.obj", 240, 135, -180, 0, 0, 0);
	((ODEObject*)objectPtr)->SetOrientation(0, -PI/3);
	objectList.push_back(objectPtr);

	//create tower 3
	//objectPtr = new ODECappedCylinder(world, false, true, "data", "tower.obj", 250, 150, -130, 15, 60);
	objectPtr = new ODEBox(world, false, true, "data", "tower.obj", 250, 140, -120, 0, 0, 0);
	objectList.push_back(objectPtr);

	//create long wall section 1
	objectPtr = new ODEBox(world, false, true, "data", "long_brick_piece.obj", 235, 135, -90, 0, 0, 0);
	((ODEObject*)objectPtr)->SetOrientation(1, PI/4);
	objectList.push_back(objectPtr);

	//create fountain
	objectPtr = new ODEBox(world, false, true, "data", "fountain.obj", 200, 133, -120, 0, 0, 0);
	objectList.push_back(objectPtr);

	//create fountain particles
	FountainParticleSystem* waterPtr = new FountainParticleSystem(200, 136, -120, 2, PARTICLE_TEXTURE_FILE);
	particleSystemList.push_back(waterPtr);
	//in Juggler, make the display list dynamically in ContextPreDraw()
	waterPtr->MakeDisplayList();

	//fire particles
	//const dReal* firePos = dGeomGetPosition(plankFireGeom);
	point3d firePosition = burningPlank->GetPosition();
	firePosition.x += burningPlank->GetXDimension()/2;
	plankSmoke = new SmokeParticleSystem(firePosition.x, firePosition.y, 
		firePosition.z, 1, true, PARTICLE_TEXTURE_FILE);
	particleSystemList.push_back(plankSmoke);
	plankFire = new FireParticleSystem(firePosition.x, firePosition.y, 
		firePosition.z, 1, true, PARTICLE_TEXTURE_FILE);
	particleSystemList.push_back(plankFire);


	//create sword
	ODEObject* ODEObjectPtr = new ODEBox(world, true, true, "data", "sword.obj", 188, 135, -130, 0, 0, 0);
	vector3d smallVelocity = {0, 0, -1};
	ODEObjectPtr->SetVelocity(smallVelocity);
	objectList.push_back(ODEObjectPtr);

	//create shield
	ODEObjectPtr = new ODEBox(world, true, true, "data", "shield.obj", 182, 138, -127, 0, 0, 0);
	ODEObjectPtr->SetVelocity(smallVelocity);
	objectList.push_back(ODEObjectPtr);

	////create bridge support1
	//ODEBox* bridgePtr1 = new ODEBox(world, false, true, "data", "bridge_support.obj", -96, 130, -195, 0, 0, 0);
	//((ODEObject*)bridgePtr1)->SetOrientation(1, PI/2);
	//objectList.push_back(bridgePtr1);

	////create bridge support2
	//ODEBox* bridgePtr2 = new ODEBox(world, false, true, "data", "bridge_support.obj", 113, 130, -195, 0, 0, 0);
	//((ODEObject*)bridgePtr2)->SetOrientation(1, 3*PI/2);
	//objectList.push_back(bridgePtr2);

	////create bridge section1
	//bridgeSection1 = new ODEBox(world, true, true, "data", "bridge_section.obj", -37, 133, -195, 0, 0, 0);
	//objectList.push_back(bridgeSection1);

	////create bridge section2
	//bridgeSection2 = new ODEBox(world, true, true, "data", "bridge_section.obj", 51, 133, -195, 0, 0, 0);
	//objectList.push_back(bridgeSection2);

	////create joints for bridge
	//bridgeSupport1 = dJointCreateHinge(world->GetWorldID(), 0);
	//dJointAttach(bridgeSupport1, bridgeSection1->GetBodyID(), 0);
	//point3d jointPoint = bridgeSection1->GetPosition();
	//jointPoint.x -= bridgeSection1->GetXDimension()/2;
	//dJointSetHingeAnchor(bridgeSupport1, jointPoint.x, jointPoint.y, jointPoint.z);
	//dJointSetHingeAxis(bridgeSupport1, 0, 0, 1);
	//dJointSetHingeParam(bridgeSupport1, dParamLoStop, 0);
	//dJointSetHingeParam(bridgeSupport1, dParamHiStop, 0);
	//bridgeSupport2 = dJointCreateHinge(world->GetWorldID(), 0);
	//dJointAttach(bridgeSupport2, bridgeSection2->GetBodyID(), 0);
	//jointPoint = bridgeSection2->GetPosition();
	//jointPoint.x += bridgeSection2->GetXDimension()/2;
	//dJointSetHingeAnchor(bridgeSupport2, jointPoint.x, jointPoint.y, jointPoint.z);
	//dJointSetHingeAxis(bridgeSupport2, 0, 0, 1);
	//dJointSetHingeParam(bridgeSupport2, dParamLoStop, 0);
	//dJointSetHingeParam(bridgeSupport2, dParamHiStop, 0);

	////fire particles2
	////const dReal* firePos = dGeomGetPosition(plankFireGeom);
	//firePosition = bridgeSection1->GetPosition();
	//firePosition.x += bridgeSection1->GetXDimension()/2;
	//bridgeSmoke1 = new SmokeParticleSystem(firePosition.x, firePosition.y, 
	//	firePosition.z, 1, true, PARTICLE_TEXTURE_FILE);
	//particleSystemList.push_back(plankSmoke);
	//bridgeFire1 = new FireParticleSystem(firePosition.x, firePosition.y, 
	//	firePosition.z, 1, true, PARTICLE_TEXTURE_FILE);
	//particleSystemList.push_back(plankFire);

	////fire particles3
	////const dReal* firePos = dGeomGetPosition(plankFireGeom);
	//firePosition = bridgeSection2->GetPosition();
	//firePosition.x -= bridgeSection2->GetXDimension()/2;
	//bridgeSmoke2 = new SmokeParticleSystem(firePosition.x, firePosition.y, 
	//	firePosition.z, 1, true, PARTICLE_TEXTURE_FILE);
	//particleSystemList.push_back(plankSmoke);
	//bridgeFire2 = new FireParticleSystem(firePosition.x, firePosition.y, 
	//	firePosition.z, 1, true, PARTICLE_TEXTURE_FILE);
	//particleSystemList.push_back(plankFire);

	//centerBridgeLink = dJointCreateFixed(world->GetWorldID(), 0);
	//dJointAttach(centerBridgeLink, bridgePtr3->GetBodyID(), bridgePtr4->GetBodyID());
	//dJointSetFixed(centerBridgeLink);

	//centerBridgeLink = dJointCreateHinge(world->GetWorldID(), 0);
	//dJointAttach(centerBridgeLink, bridgePtr3->GetBodyID(), bridgePtr4->GetBodyID());
	//jointPoint = bridgePtr4->GetPosition();
	//jointPoint.x -= bridgePtr4->GetXDimension()/2;
	//dJointSetHingeAnchor(centerBridgeLink, jointPoint.x, jointPoint.y, jointPoint.z);
	//dJointSetHingeAxis(centerBridgeLink, 0, 0, 1);


	//create tower 4
	//objectPtr = new ODECappedCylinder(world, false, true, "data", "tower.obj", 250, 150, -130, 15, 60);
	objectPtr = new ODEBox(world, false, true, "data", "tower.obj", 225, 150, -80, 0, 0, 0);
	objectList.push_back(objectPtr);

	//create tower 5 (the one in the water)
	//objectPtr = new ODECappedCylinder(world, false, true, "data", "tower.obj", 250, 150, -130, 15, 60);
	objectPtr = new ODEBox(world, false, true, "data", "tower.obj", 75, 10, 120, 0, 0, 0);
	((ODEObject*)objectPtr)->SetOrientation(2, -PI/6);
	objectList.push_back(objectPtr);

	//The following must be last to get the transparent list.

	//creating river
	objectPtr = new Base3DObject(false, true, "data", "river.obj", 0, 0, 0);
	objectPtr->SetTransparency(true);
	transparencyList.push_back(objectPtr);

	//creating upside down river surface
	objectPtr = new Base3DObject(false, true, "data", "reversed_river.obj", 0, 0, 0);
	objectPtr->SetTransparency(true);
	transparencyList.push_back(objectPtr);

	//creating moat
	objectPtr = new Base3DObject(false, true, "data", "moat.obj", 0, 0, 0);
	objectPtr->SetTransparency(true);
	transparencyList.push_back(objectPtr);
}

void Reshape(int w, int h)
{
	aspect_ratio = (dReal)w/(dReal)h;
	glViewport(0, 0, w, h);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(40.0, aspect_ratio, 3, 1500.0);
	glMatrixMode( GL_MODELVIEW );

	windowWidth = w;
	windowHeight = h;
	glutPostRedisplay();
}

void KeyboardAction(unsigned char key, int x, int y)
{
	switch(key)
	{
		case ' ':
		{
			humanPtr = new ODEHuman(world, 10, 134, -100, 1.3);
			humanPtr->SetFloats(true);
			//brain = new NeuralNet(15, brainSize, 11, 0);
			FILE* genesFile = fopen("input.nn", "r");
			brain = new NeuralNet(15, brainSize, 11, genesFile, 0);
			fclose(genesFile);
			humanPtr->SetBrain(brain);
			objectList.push_back(humanPtr);
			break;
		}
		//case 'p': //particle system
		//{
		//	//test particle system
		//	BaseParticleSystem* waterPtr = new WaterParticleSystem(-100, 140, -120, 1, false, PARTICLE_TEXTURE_FILE);
		//	particleSystemList.push_back(waterPtr);
		//	
		//	//in Juggler, make the display list dynamically in ContextPreDraw()
		//	waterPtr->MakeDisplayList();
		//	break;
		//}
		case 'f':
		if (humanPtr != NULL)
		{
			humanPtr->AddForce(0, 2000, 0);
			//dBodyAddForce(humanPtr->GetBodyID(), 0, 2000, 0);
			//BreakJoint(centerBridgeLink);
		}
			break;
		case 's':
		{
			ShootObject(0); //sphere
			break;
		}
		case 'c': //print camera position
		{
			std::cout << camera->GetPosition().x << " " << camera->GetPosition().y << " " 
				<< camera->GetPosition().z << std::endl;
			break;
		}
		case 'b':
		{
			ShootObject(1); //bricks
			break;
		}
		case 't':
		{
			ShootObject(2); //tower
			break;
		}
		case 'h':
		{
			ShootObject(3); //limp human
			break;
		}
		case 'l':
		{
			if (trebuchet != NULL)
			{
				if (trebuchet->IsLaunched())
				{
					trebuchet->Reset();
				}
				else
				{
					trebuchet->Launch();
				}
			}
			break;
		}
		case 'r': //reset camera position
		{
			camera->SetPosition(initialCameraPosition);
			dBodySetForce(camera->GetBodyID(), 0, 0, 0);
			dBodySetLinearVel(camera->GetBodyID(), 0, 0, 0);
			break;
		}
		case 'q':
		{
			// Clean up...
			while (!objectList.empty())
			{
				delete objectList.back();
				objectList.pop_back();
			}

			while (!transparencyList.empty())
			{
				delete transparencyList.back();
				transparencyList.pop_back();
			}

			while (!particleSystemList.empty())
			{
				delete particleSystemList.back();
				particleSystemList.pop_back();
			}

			//dJointDestroy(bridgeSupport1);
			//dJointDestroy(bridgeSupport2);

			delete world; // always do this last
			exit(0);
			break;
		}

		default:
			break;
	}
}

void KeyboardSpecialFunc(int key, int x, int y)
{
	switch(key)
	{
		case GLUT_KEY_UP:
			camera->MoveForward();
			break;
		case GLUT_KEY_DOWN:
			camera->MoveBackward();
			break;
		case GLUT_KEY_LEFT:
			camera->MoveLeft();
			break;
		case GLUT_KEY_RIGHT:
			camera->MoveRight();
			break;
		case GLUT_KEY_END:
			camera->Jump();
			break;
		default:
			break;
	}
}

void MouseButtons(int button, int button_state, int x, int y)
{
	if (button == GLUT_LEFT_BUTTON) 
	{
		if (button_state == GLUT_DOWN)
		{
			LeftMouseButton = true;

			if (objectInHand == NULL)
			{
				TryToGrabObject();
			}
			else
			{
				DropObject();
			}
		}
		else
		{
			LeftMouseButton = false;
		}
	}
	if (button == GLUT_RIGHT_BUTTON) 
	{
		if (button_state == GLUT_DOWN)
		{
			RightMouseButton = true;
		}
		else
		{
			RightMouseButton = false;
		}
	}
}

void MouseMovement(int x, int y)
{
	//Smoothing method...
	y = windowHeight - y;
	point2i deltaMouse;
	deltaMouse.x = x - windowWidth/2 + oldDeltaMouse[0];
	deltaMouse.y = y - windowHeight/2 + oldDeltaMouse[1];
	camera->AddRotation(dReal(deltaMouse.y)/12, dReal(-deltaMouse.x)/12);
	oldDeltaMouse[0] = x - windowWidth/2;
	oldDeltaMouse[1] = y - windowHeight/2;
	//Cursor gets reset to center of window in idle function

	////Old, jerky method..
	//y = windowHeight - y;
	//point2i deltaMouse;
	//deltaMouse.x = x - windowWidth/2;
	//deltaMouse.y = y - windowHeight/2;
	//camera->AddRotation(dReal(deltaMouse.y)/4, dReal(-deltaMouse.x)/4);
	////Cursor gets reset to center of window in idle function
}

void ShootObject(int obj)
{
	//obj values; 0=sphere, 1=bricks, 2=tower, 3=human

	switch(obj)
	{
		case 0: //sphere
			{
				point3d startPoint = camera->GetPosition();
				startPoint.y += 15;
				objectPtr = new ODESphere(world, true, false, "", "", startPoint.x, startPoint.y,
					startPoint.z, 2);
				((ODEObject*)(objectPtr))->SetFloats(true);
				objectPtr->MakeDisplayList();
				vector3d forwardVector = camera->GetForwardVector();
				vector3d initialVelocity = {forwardVector.x*20, forwardVector.y*20, forwardVector.z*20};
				((ODEObject*)objectPtr)->SetVelocity(initialVelocity);
				objectList.push_back(objectPtr);
				break;
			}
		case 1: //bricks
			{
				point3d startPoint = camera->GetPosition();
				startPoint.y += 10;
				objectPtr = new ODEBox(world, true, true, "data", "small_brick_piece.obj", 
					startPoint.x, startPoint.y, startPoint.z, 0, 0, 0);
				objectPtr->MakeDisplayList();
				vector3d forwardVector = camera->GetForwardVector();
				vector3d initialVelocity = {forwardVector.x*5, forwardVector.y*5, forwardVector.z*5};
				((ODEObject*)objectPtr)->SetVelocity(initialVelocity);
				objectList.push_back(objectPtr);
				break;
			}
		case 2: //tower
			{
				point3d startPoint = camera->GetPosition();
				startPoint.y += 40;
				//objectPtr = new ODECappedCylinder(world, true, true, "data", "tower.obj", 
				//	startPoint.x, startPoint.y, startPoint.z, 15, 60);
				//((ODEObject*)objectPtr)->SetOrientation(1, PI/2);
				objectPtr = new ODEBox(world, true, true, "data", "tower.obj", 
					startPoint.x, startPoint.y, startPoint.z, 0, 0, 0);
				objectPtr->MakeDisplayList();
				vector3d forwardVector = camera->GetForwardVector();
				vector3d initialVelocity = {forwardVector.x*70, forwardVector.y*70, forwardVector.z*70};
				((ODEObject*)objectPtr)->SetVelocity(initialVelocity);
				objectList.push_back(objectPtr);
				break;
			}
		case 3: //limp human
			{
				point3d startPoint = camera->GetPosition();
				startPoint.y += 20;
				humanPtr = new ODEHuman(world, startPoint.x, startPoint.y, startPoint.z, 1.3);
				humanPtr->SetFloats(true);
				vector3d forwardVector = camera->GetForwardVector();
				vector3d initialVelocity = {forwardVector.x*20, forwardVector.y*20, forwardVector.z*20};
				humanPtr->SetTotalVelocity(initialVelocity);
				objectList.push_back(humanPtr);
			}
		default:
			break;
	}
}

void AdjustUnderwaterForces()
{
	//adjust gravity if in water
	for (iter = objectList.begin(); iter != objectList.end(); iter++)
	{
		if ((*iter)->IsODEObject() && ((ODEObject*)(*iter))->HasBody())
		{
			if ((*iter)->GetPosition().y <= WATER_LEVEL) //under water
			{
				dBodySetGravityMode(((ODEObject*)(*iter))->GetBodyID(), 0);

				((ODEObject*)(*iter))->DampenVelocity(UNDERWATER_DAMPING_FACTOR);

				if (((ODEObject*)(*iter))->Floats())
				{
					((ODEObject*)(*iter))->AddForce(0, UNDERWATER_FORCE, 0);
					//dBodyAddForce(((ODEObject*)(*iter))->GetBodyID(), 0, UNDERWATER_FORCE, 0);
				}
				else
				{
					((ODEObject*)(*iter))->AddForce(0, -UNDERWATER_FORCE/2, 0);
					//dBodyAddForce(((ODEObject*)(*iter))->GetBodyID(), 0, -UNDERWATER_FORCE/2, 0);
				}
			}
			else //above water
			{
				dBodySetGravityMode(((ODEObject*)(*iter))->GetBodyID(), 1);
			}
		}
	}
}

void TryToGrabObject()
{
	vector3d forwardVector = camera->GetForwardVector();
	forwardVector.x *= HAND_DISTANCE_FROM_CAMERA;
	forwardVector.y *= HAND_DISTANCE_FROM_CAMERA;
	forwardVector.z *= HAND_DISTANCE_FROM_CAMERA;
	point3d grabbingPoint = {forwardVector.x, forwardVector.y, forwardVector.z};
	grabbingPoint.x = camera->GetPosition().x + grabbingPoint.x;
	grabbingPoint.y = camera->GetPosition().y + grabbingPoint.y;
	grabbingPoint.z = camera->GetPosition().z + grabbingPoint.z;

	//We will to grab the closest object within the grabbing distance.

	dReal closestObjectDistance = GRABBING_DISTANCE*2;

	for (iter = objectList.begin(); iter != objectList.end(); iter++)
	{
		if ((*iter)->IsODEObject())
		{
			if (((ODEObject*)(*iter))->HasBody())
			{
				dReal distanceToThisObject = Distance3D(grabbingPoint, (*iter)->GetPosition());

				if (distanceToThisObject < closestObjectDistance && 
					distanceToThisObject < GRABBING_DISTANCE)
				{
					objectInHand = ((ODEObject*)(*iter));
					closestObjectDistance = distanceToThisObject;
				}
			}
		}
	}
}

dReal Distance3D(point3d point1, point3d point2)
{
	dReal distance = pow(point1.x - point2.x, 2) + pow(point1.y - point2.y, 2) + 
		pow(point1.z - point2.z, 2);
	return sqrt(distance);
}

void UpdateObjectInHand()
{
	if (objectInHand != NULL)
	{
		dBodySetForce(objectInHand->GetBodyID(), 0, 0, 0);
		dBodySetLinearVel(objectInHand->GetBodyID(), 0, 0, 0);
		dBodySetAngularVel(objectInHand->GetBodyID(), 0, 0, 0);

		vector3d forwardVector = camera->GetForwardVector();
		forwardVector.x *= HAND_DISTANCE_FROM_CAMERA;
		forwardVector.y *= HAND_DISTANCE_FROM_CAMERA;
		forwardVector.z *= HAND_DISTANCE_FROM_CAMERA;
		point3d grabbingPoint = {forwardVector.x, forwardVector.y, forwardVector.z};
		grabbingPoint.x = camera->GetPosition().x + grabbingPoint.x;
		grabbingPoint.y = camera->GetPosition().y + grabbingPoint.y;
		grabbingPoint.z = camera->GetPosition().z + grabbingPoint.z;
		dBodySetPosition(objectInHand->GetBodyID(), grabbingPoint.x, grabbingPoint.y, grabbingPoint.z);
	}
}

void DropObject()
{
	if (objectInHand != NULL)
	{
		//make sure to avoid autodisabling
		dBodyEnable(objectInHand->GetBodyID());
		dBodySetForce(objectInHand->GetBodyID(), 0, 0, 0);
		dBodySetTorque(objectInHand->GetBodyID(), 0, 0, 0);

		vector3d velocity = objectInHand->GetVelocity();
		if (velocity.x <= 0.5 && velocity.y <= 0.5 && velocity.z <= 0.5)
		{
			vector3d smallVelocity = {0, 0.5, 0};
			objectInHand->SetVelocity(smallVelocity);
		}
		else
		{
			point3d currentPosition = objectInHand->GetPosition();
			point3d lastPosition = objectInHand->GetLastPosition();
			vector3d newVelocityVector = {currentPosition.x - lastPosition.x, 
				currentPosition.y - lastPosition.y, currentPosition.z - lastPosition.z};
			newVelocityVector.x *= 100000;
			newVelocityVector.y *= 100000;
			newVelocityVector.z *= 100000;
			objectInHand->SetVelocity(newVelocityVector);
		}
		objectInHand = NULL;
	}
}

void BreakBridgeJoint()
{

}