#include <iostream>
#include <vector>
#include <time.h>

#define _USE_MATH_DEFINES
#include <math.h>

#include "Frustum.h"

#include <GL/freeglut.h>
#include <PxPhysicsAPI.h> 
#include <PxExtensionsAPI.h> 
#include <PxDefaultErrorCallback.h>
#include <PxDefaultAllocator.h> 
#include <PxDefaultSimulationFilterShader.h>
#include <PxDefaultCpuDispatcher.h>
#include <PxShapeExt.h>
#include <PxSimpleFactory.h>

using namespace std;

#pragma comment(lib, "PhysX3_x86.lib")
#pragma comment(lib, "PxTask.lib")
#pragma comment(lib, "Foundation.lib")
#pragma comment(lib, "PhysX3Extensions.lib")
#pragma comment(lib, "GeomUtils.lib") 

//const int WINDOW_WIDTH=1024, WINDOW_HEIGHT=768;

static PxPhysics* gPhysicsSDK = NULL;
static PxDefaultErrorCallback gDefaultErrorCallback;
static PxDefaultAllocator gDefaultAllocatorCallback;
static PxSimulationFilterShader gDefaultFilterShader = PxDefaultSimulationFilterShader;
static PxMaterial* gDefaultMaterial = NULL;

int windowWidth = 1024;
int windowHeight = 768;
PxScene* gScene = NULL;
PxReal myTimestep = 1.0f/60.0f;
vector<PxActor*> gActors;
unsigned int gGeoType = 1;

bool gDrawWireframe = false;
bool gDrawBoundBox = false;

int gTotalActors = 0;
int gDrawedActors = 0;

const float DEG2RAD = (float)M_PI / 180.0f;

PxParticleFluid* gWater = NULL;

struct WaterParticle
{
	PxVec3 *positions;
	PxVec3 *velocity;
	int numParticles;

	PxU32 *indices;
	int indexCount;
	PxStrideIterator<PxU32> indexBuffer;
};

vector<WaterParticle> gWaterParticles;

// For mouse dragging
int oldX=0, oldY=0;
float rX=15, rY=0;
float fps=0;
int startTime=0;
int totalFrames=0;
int state =1 ;
float dist=-5;

// For culling
CFrustum* gFrustum = NULL;

float Deg2Rad(float degree)
{
	return degree * DEG2RAD;
}

float RandomRadius()
{
	return (float)(rand() % 360);
}

void SetOrthoForFont()
{	
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();

	gluOrtho2D(0, (GLdouble)windowWidth, (GLdouble)windowHeight, 0);

	glMatrixMode(GL_MODELVIEW);	
	glLoadIdentity();
}

void ResetPerspectiveProjection() 
{
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
}

void AddBox(PxVec3 pos, PxVec3 dimensions)
{
	// Create cube	 
	PxReal density = 1.0f;
	PxTransform transform(PxVec3(0.0f, 10.0f, 0.0f), PxQuat(Deg2Rad(RandomRadius()), PxVec3(0,1,0)));
	PxBoxGeometry geometry(dimensions);

	PxRigidDynamic *actor = PxCreateDynamic(*gPhysicsSDK, transform, geometry, *gDefaultMaterial, density);
	actor->setAngularDamping(0.75);
	actor->setLinearVelocity(PxVec3(0,-10,0)); 
	actor->setMass(5.0f);
	if (!actor)
		cerr<<"create box actor failed!"<<endl;
	gScene->addActor(*actor);

	gActors.push_back(actor);
}

 void AddBoxStatic(PxVec3 pos, PxVec3 dimensions)
{
	// Create cube	 
	PxTransform transform(pos, PxQuat::createIdentity());
	PxBoxGeometry geometry(dimensions);

	PxRigidStatic *actor = PxCreateStatic(*gPhysicsSDK, transform, geometry, *gDefaultMaterial);
	if (!actor)
		cerr<<"create box static actor failed!"<<endl;
	gScene->addActor(*actor);

	gActors.push_back(actor);
}

void AddSphere()
{
	// Create sphere	 
	PxReal density = 1.0f;
	PxTransform transform(PxVec3(0.0f, 10.0f, 0.0f), PxQuat::createIdentity());
	PxSphereGeometry geometry(0.5);

	PxRigidDynamic *actor = PxCreateDynamic(*gPhysicsSDK, transform, geometry, *gDefaultMaterial, density);
	actor->setAngularDamping(0.75);
	actor->setLinearVelocity(PxVec3(0,-10,0)); 
	actor->setMass(5.0f);
	if (!actor)
		cerr<<"create sphere actor failed!"<<endl;
	gScene->addActor(*actor);

	gActors.push_back(actor);
}

void AddCapsule()
{
	// Create capsule	 
	PxReal density = 1.0f;
	PxTransform transform(PxVec3(0.0f, 10.0f, 0.0f), PxQuat(Deg2Rad(RandomRadius()), PxVec3(0,1,0)));
	PxCapsuleGeometry geometry(0.5, 1.0);

	PxRigidDynamic *actor = PxCreateDynamic(*gPhysicsSDK, transform, geometry, *gDefaultMaterial, density);
	actor->setAngularDamping(0.75);
	actor->setLinearVelocity(PxVec3(0,-10,0)); 
	actor->setMass(5.0f);
	if (!actor)
		cerr<<"create capsule actor failed!"<<endl;
	gScene->addActor(*actor);

	gActors.push_back(actor);
}

void AddWater()
{
	WaterParticle wp;
	wp.numParticles = 1;
	wp.indexCount = 1;
	wp.indices = new PxU32[1];
	wp.indices[0] = 0;

	wp.positions = new PxVec3[1];
	wp.positions[0].x = 0.0f;
	wp.positions[0].y = 3.0f;
	wp.positions[0].z = 0.0f;

	wp.velocity = new PxVec3[1];
	wp.velocity[0].x = 0.0f;
	wp.velocity[0].y = -1.0f;
	wp.velocity[0].z = 0.0f;

	PxParticleCreationData particleCreationData;
	particleCreationData.numParticles = wp.numParticles;
	particleCreationData.positionBuffer = PxStrideIterator<const PxVec3>(wp.positions);
	particleCreationData.velocityBuffer = PxStrideIterator<const PxVec3>(wp.velocity);

	wp.indexBuffer = PxStrideIterator<PxU32>(wp.indices);

	PxU32 numCreated = gWater->createParticles(particleCreationData, &wp.indexBuffer);
	if (numCreated > 0)
	{
  	  gWaterParticles.push_back(wp);
	}
	else
	{
		delete wp.velocity;
		delete wp.positions;
		delete wp.indices;
	}
}

void AddPlane()
{
	// Create ground plane
	PxReal d = 0.0f;	 
	PxTransform pose = PxTransform(PxVec3(0.0f, 0, 0.0f),PxQuat(PxHalfPi, PxVec3(0.0f, 0.0f, 1.0f)));

	PxRigidStatic* plane = gPhysicsSDK->createRigidStatic(pose);
	if (!plane)
		cerr<<"create plane failed!"<<endl;

	PxShape* shape = plane->createShape(PxPlaneGeometry(), *gDefaultMaterial);
	if (!shape)
		cerr<<"create shape failed!"<<endl;
	gScene->addActor(*plane);
	gActors.push_back(plane);
}

void ClearScene()
{
	for (long index=0; index<(long)gActors.size(); ++index) 
	{
		PxActor* act = gActors.at(index);
		gScene->removeActor(*act);	
		act->release();
	}
	gActors.clear();
}

void ClearWater()
{
	// Release water particles
	for (long index=0; index<(long)gWaterParticles.size(); ++index) 
	{
		delete gWaterParticles[index].indices;
		delete gWaterParticles[index].positions;
		delete gWaterParticles[index].velocity;
	}
	gWaterParticles.clear();
}

void ResetScene()
{
	// Release actors
	ClearScene();
	ClearWater();

	// Add plane
	AddPlane();

	// Add becken
	AddBoxStatic(PxVec3(0.0f, 0.1f, 0.0f), PxVec3(3.0f,0.1f,3.0f));
	AddBoxStatic(PxVec3(2.9f, 0.3f, 0.0f), PxVec3(0.1f, 0.3f, 3.0f));
	AddBoxStatic(PxVec3(-2.9f, 0.3f, 0.0f), PxVec3(0.1f, 0.3f, 3.0f));
	AddBoxStatic(PxVec3(0.0f, 0.3f, -2.9f), PxVec3(3.0f, 0.3f, 0.1f));
	AddBoxStatic(PxVec3(0.0f, 0.3f, 2.9f), PxVec3(3.0f, 0.3f, 0.1f));

	// Create fluid particle system
	PxParticleFluidDesc particleSystemDesc(gPhysicsSDK->getTolerancesScale());
	particleSystemDesc.maxParticles = 10000;
	particleSystemDesc.particleBaseFlags = PxParticleBaseFlag::eGPU | PxParticleBaseFlag::eCOLLISION_TWOWAY;
	gWater = gPhysicsSDK->createParticleFluid(particleSystemDesc);
	if (gWater)
	{
		gScene->addActor(*gWater);
		gActors.push_back(gWater);
	}

}

void InitializePhysX() {
	// Create Physics SDK
	gPhysicsSDK = PxCreatePhysics(PX_PHYSICS_VERSION, gDefaultAllocatorCallback, gDefaultErrorCallback, PxTolerancesScale() );
	if(gPhysicsSDK == NULL) {
		cerr<<"Error creating PhysX3 device."<<endl;
		cerr<<"Exiting..."<<endl;
		exit(1);
	}

	// Initialize PhysX Extensions
	if(!PxInitExtensions(*gPhysicsSDK))
		cerr<< "PxInitExtensions failed!" <<endl;

	// Connect to visual debugger
	PxExtensionVisualDebugger::connect(gPhysicsSDK->getPvdConnectionManager(),"localhost",5425, 10000, true);

	// Create the scene
	PxSceneDesc sceneDesc(gPhysicsSDK->getTolerancesScale());
	sceneDesc.gravity=PxVec3(0.0f, -9.8f, 0.0f);

	if(!sceneDesc.cpuDispatcher) {
		PxDefaultCpuDispatcher* mCpuDispatcher = PxDefaultCpuDispatcherCreate(1);
		if(!mCpuDispatcher)
			cerr<<"PxDefaultCpuDispatcherCreate failed!"<<endl;
		sceneDesc.cpuDispatcher = mCpuDispatcher;
	} 
	if(!sceneDesc.filterShader)
		sceneDesc.filterShader  = gDefaultFilterShader;

	gScene = gPhysicsSDK->createScene(sceneDesc);
	if (!gScene)
		cerr<<"createScene failed!"<<endl;

	gScene->setVisualizationParameter(PxVisualizationParameter::eSCALE,				 1.0);
	gScene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_SHAPES,	1.0f);

	gDefaultMaterial = gPhysicsSDK->createMaterial(0.5,0.5,0.5);

	// Reset scene
	ResetScene();
}

void getColumnMajor(PxMat33 m, PxVec3 t, float* mat) {
	mat[0] = m.column0[0];
	mat[1] = m.column0[1];
	mat[2] = m.column0[2];
	mat[3] = 0;

	mat[4] = m.column1[0];
	mat[5] = m.column1[1];
	mat[6] = m.column1[2];
	mat[7] = 0;

	mat[8] = m.column2[0];
	mat[9] = m.column2[1];
	mat[10] = m.column2[2];
	mat[11] = 0;

	mat[12] = t[0];
	mat[13] = t[1];
	mat[14] = t[2];
	mat[15] = 1;
}

void RenderSpacedBitmapString(
	int x, 
	int y,
	int spacing, 
	void *font,
	char *string) 
{
	char *c;
	int x1=x;
	for (c=string; *c != '\0'; c++) {
		glRasterPos2i(x1,y);
		glutBitmapCharacter(font, *c);
		x1 = x1 + glutBitmapWidth(font,*c) + spacing;
	}
}

void DrawGLCube(float hx, float hy, float hz)
{
	glBegin(GL_QUADS);		// Draw The Cube Using quads

	glNormal3f(0.0f, 1.0f, 0.0f);
	glVertex3f( hx, hy,-hz);	// Top Right Of The Quad (Top)
	glVertex3f(-hx, hy,-hz);	// Top Left Of The Quad (Top)
	glVertex3f(-hx, hy, hz);	// Bottom Left Of The Quad (Top)
	glVertex3f( hx, hy, hz);	// Bottom Right Of The Quad (Top)

	glNormal3f(0.0f, -1.0f, 0.0f);
	glVertex3f( hx,-hy, hz);	// Top Right Of The Quad (Bottom)
	glVertex3f(-hx,-hy, hz);	// Top Left Of The Quad (Bottom)
	glVertex3f(-hx,-hy,-hz);	// Bottom Left Of The Quad (Bottom)
	glVertex3f( hx,-hy,-hz);	// Bottom Right Of The Quad (Bottom)

	glNormal3f(0.0f, 0.0, 1.0f);
	glVertex3f( hx, hy, hz);	// Top Right Of The Quad (Front)
	glVertex3f(-hx, hy, hz);	// Top Left Of The Quad (Front)
	glVertex3f(-hx,-hy, hz);	// Bottom Left Of The Quad (Front)
	glVertex3f( hx,-hy, hz);	// Bottom Right Of The Quad (Front)

	glNormal3f(0.0f, 0.0, -1.0f);
	glVertex3f( hx,-hy,-hz);	// Top Right Of The Quad (Back)
	glVertex3f(-hx,-hy,-hz);	// Top Left Of The Quad (Back)
	glVertex3f(-hx, hy,-hz);	// Bottom Left Of The Quad (Back)
	glVertex3f( hx, hy,-hz);	// Bottom Right Of The Quad (Back)

	glNormal3f(-1.0f, 0.0, 0.0f);
	glVertex3f(-hx, hy, hz);	// Top Right Of The Quad (Left)
	glVertex3f(-hx, hy,-hz);	// Top Left Of The Quad (Left)
	glVertex3f(-hx,-hy,-hz);	// Bottom Left Of The Quad (Left)
	glVertex3f(-hx,-hy, hz);	// Bottom Right Of The Quad (Left)

	glNormal3f(1.0f, 0.0, 0.0f);
	glVertex3f( hx, hy,-hz);	// Top Right Of The Quad (Right)
	glVertex3f( hx, hy, hz);	// Top Left Of The Quad (Right)
	glVertex3f( hx,-hy, hz);	// Bottom Left Of The Quad (Right)
	glVertex3f( hx,-hy,-hz);	// Bottom Right Of The Quad (Right)

	glEnd();			// End Drawing The Cube
}

void DrawAxes()
{	 
	//To prevent the view from disturbed on repaint
	//this push matrix call stores the current matrix state
	//and restores it once we are done with the arrow rendering
	glPushMatrix();
	glColor3f(0,0,1);
	glPushMatrix();
	glTranslatef(0,0, 0.8f);
	glutSolidCone(0.0325,0.2, 4,1);
	//Draw label			
	glTranslatef(0,0.0625,0.225f);
	RenderSpacedBitmapString(0,0,0,GLUT_BITMAP_HELVETICA_10, "Z");
	glPopMatrix();					

	glColor3f(1,0,0);
	glRotatef(90,0,1,0);	
	glPushMatrix();
	glTranslatef(0,0,0.8f);
	glutSolidCone(0.0325,0.2, 4,1);
	//Draw label
	glTranslatef(0,0.0625,0.225f);
	RenderSpacedBitmapString(0,0,0,GLUT_BITMAP_HELVETICA_10, "X");
	glPopMatrix();					
	glutSolidCone(0.0225,1, 4,1);

	glColor3f(0,1,0);
	glRotatef(90,-1,0,0);	
	glPushMatrix();
	glTranslatef(0,0, 0.8f);
	glutSolidCone(0.0325,0.2, 4,1);
	//Draw label
	glTranslatef(0,0.0625,0.225f);
	RenderSpacedBitmapString(0,0,0,GLUT_BITMAP_HELVETICA_10, "Y");
	glPopMatrix();					
	glutSolidCone(0.0225,1, 4,1);	
	glPopMatrix();
}

void DrawGrid(int GRID_SIZE)
{
	glBegin(GL_LINES);
	glColor3f(0.25f, 0.25f, 0.25f);
	for(int i=-GRID_SIZE;i<=GRID_SIZE;i++)
	{
		glVertex3f((float)i,0,(float)-GRID_SIZE);
		glVertex3f((float)i,0,(float)GRID_SIZE);

		glVertex3f((float)-GRID_SIZE,0,(float)i);
		glVertex3f((float)GRID_SIZE,0,(float)i);
	}
	glEnd();
}

void UpdatePhysX() 
{ 
	gScene->simulate(myTimestep);      

	// Fetch results from scene
	while(!gScene->fetchResults() )     
	{
		// Nothing todo
	}
} 

void DrawBox(PxShape* pShape) {
	GLfloat mat_diffuse_1[4]={0,0,1.0f,0};
	GLfloat mat_diffuse_2[4]={0.85f,0,0,0};
	if (pShape->getActor().isRigidStatic())
	{
		glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat_diffuse_1);
		glColor4fv(mat_diffuse_1);
	}
	else
	{
		glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat_diffuse_2);
		glColor4fv(mat_diffuse_2);
	}

	PxTransform pT = PxShapeExt::getGlobalPose(*pShape);
	PxBoxGeometry bg;
	pShape->getBoxGeometry(bg);
	PxMat33 m = PxMat33(pT.q );
	float mat[16];
	getColumnMajor(m,pT.p, mat);
	glPushMatrix(); 
	glMultMatrixf(mat);
	DrawGLCube(bg.halfExtents.x,bg.halfExtents.y, bg.halfExtents.z);
	glPopMatrix(); 
}

void DrawSphere(PxShape* pShape) {
	GLfloat mat_diffuse[4]={0,0.85f,0,0};
	glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat_diffuse);
	glColor4fv(mat_diffuse);

	PxTransform pT = PxShapeExt::getGlobalPose(*pShape);
	PxSphereGeometry sg;
	pShape->getSphereGeometry(sg);
	PxMat33 m = PxMat33(pT.q );
	float mat[16];
	getColumnMajor(m,pT.p, mat);
	glPushMatrix(); 
	glMultMatrixf(mat);
	glutSolidSphere(sg.radius, 16, 16);
	glPopMatrix(); 
}

void DrawCapsule(PxShape* pShape) {
	GLfloat mat_diffuse[4]={0.85f,0,0.85f,0};
	glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat_diffuse);
	glColor4fv(mat_diffuse);

	PxTransform pT = PxShapeExt::getGlobalPose(*pShape);
	PxCapsuleGeometry cg;
	pShape->getCapsuleGeometry(cg);
	PxMat33 m = PxMat33(pT.q );
	float mat[16];
	getColumnMajor(m,pT.p, mat);
	glPushMatrix(); 
	glMultMatrixf(mat);

	glRotatef(90,0,1,0);
	glTranslatef(0, 0, -cg.halfHeight);
	glutSolidCylinder(cg.radius, 2*cg.halfHeight, 16, 16);
	glutSolidSphere(cg.radius, 16, 16);
	glTranslatef(0, 0, 2*cg.halfHeight);
	glutSolidSphere(cg.radius, 16, 16);

	glPopMatrix(); 
}

void DrawShape(PxShape* shape) 
{ 
	PxGeometryType::Enum type = shape->getGeometryType();
	switch(type) 
	{          
	case PxGeometryType::eBOX:
		DrawBox(shape);
		break;
	case PxGeometryType::eSPHERE:
		DrawSphere(shape);
		break;
	case PxGeometryType::eCAPSULE:
		DrawCapsule(shape);
		break;
	} 
}

void DrawBounds(const PxBounds3 &bounds)
{
	PxVec3 center = bounds.getCenter();
	PxVec3 scale = bounds.getDimensions();

	GLfloat mat_diffuse[4]={0.0f,1,1,0};
	glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat_diffuse);
	glColor4fv(mat_diffuse);

	if (!gDrawWireframe)
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glPushMatrix();
	glTranslatef(center.x, center.y, center.z);
	DrawGLCube(scale.x / 2, scale.y / 2, scale.z / 2);
	glPopMatrix();
	if (!gDrawWireframe)
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}

void DrawParticleFluid(PxParticleFluid* fluid)
{
	GLfloat mat_diffuse[4]={0.34f,0.85f,0,0};
	glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat_diffuse);
	glColor4fv(mat_diffuse);

	PxParticleFluidReadData* rd = fluid->lockParticleFluidReadData();
	if (rd)
	{
		PxStrideIterator<const PxParticleFlags> flagsIt(rd->flagsBuffer);
		PxStrideIterator<const PxVec3> positionIt(rd->positionBuffer);
		for (unsigned i = 0; i < rd->validParticleRange; ++i, ++flagsIt, ++positionIt)
		{
			if (*flagsIt & PxParticleFlag::eVALID)
			{
				const PxVec3& position = *positionIt;

				glPushMatrix(); 
				glTranslatef(position.x, position.y, position.z);
				glutSolidSphere(0.1f, 16, 16);
				glPopMatrix(); 

			}
		}

		rd->unlock();
	}
}

void DrawActor(PxActor* actor) 
{  
	PxBounds3 bounds = actor->getWorldBounds();
	if (gFrustum->ContainsBounds(bounds))
	{
		gDrawedActors++;
		if (actor->isRigidActor())
		{
			PxRigidActor* rigActor = (PxRigidActor*)actor;
			PxU32 nShapes = rigActor->getNbShapes(); 
			PxShape** shapes = new PxShape*[nShapes];

			rigActor->getShapes(shapes, nShapes);     
			while (nShapes--) 
			{ 
				DrawShape(shapes[nShapes]); 
			} 
			delete [] shapes;
		}
		else if (actor->isParticleFluid())
		{
			DrawParticleFluid((PxParticleFluid*)actor);
		}

		if (gDrawBoundBox)
		{	
			DrawBounds(bounds);
		}

	}
} 

void RenderActors() 
{ 
	// Render all the actors in the scene 
	for (long index=0; index<(long)gActors.size(); ++index) 
	{
		PxActor* act = gActors.at(index);
		DrawActor(act);
	}
} 


void ShutdownPhysX() {
	ClearScene();
	ClearWater();
	gScene->release();

	gDefaultMaterial->release();

	gPhysicsSDK->release();
}

void InitGL() { 
	glShadeModel(GL_SMOOTH);
	glEnable(GL_CULL_FACE);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	GLfloat ambient[4]={0.25f,0.25f,0.25f,0.25f};
	GLfloat diffuse[4]={1,1,1,1};
	GLfloat specular[4] = {1.0, 1.0, 1.0, 1.0};

	glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, specular);

	glDisable(GL_LIGHTING);
	glDisable(GL_DEPTH_TEST);
}

void OnReshape(int nw, int nh) 
{
	glViewport(0,0,nw, nh);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60, (GLfloat)nw / (GLfloat)nh, 0.1f, 1000.0f);
	glMatrixMode(GL_MODELVIEW);
}

const char* GetGeometryType(unsigned int state)
{
	switch (state)
	{
	case 1:
		return "Box\0";
	case 2:
		return "Sphere\0";
	case 3:
		return "Capsule\0";
	case 4:
		return "Water\0";
	default:
		return "Unknown";
	}
}

static PxMat44 Mat44Rotate(const float Angle, const PxVec3& Axis)
{
    PxMat44 M;
    float CosinusAngle,SinusAngle;
    M = PxMat44::createIdentity();
    CosinusAngle = cos(Angle);
    SinusAngle = sin(Angle);
    M[0][0] = CosinusAngle+(1-CosinusAngle)*Axis.x*Axis.x;
    M[1][0] = (1-CosinusAngle)*Axis.x*Axis.y-Axis.z*SinusAngle;
    M[2][0] = (1-CosinusAngle)*Axis.x*Axis.z+Axis.y*SinusAngle;
    M[0][1] = (1-CosinusAngle)*Axis.x*Axis.z+Axis.z*SinusAngle;
    M[1][1] = CosinusAngle+(1-CosinusAngle)*Axis.y*Axis.y;
    M[2][1] = (1-CosinusAngle)*Axis.y*Axis.z-Axis.x*SinusAngle;
    M[0][2] = (1-CosinusAngle)*Axis.x*Axis.z-Axis.y*SinusAngle;
    M[1][2] = (1-CosinusAngle)*Axis.y*Axis.z+Axis.x*SinusAngle;
    M[2][2] = CosinusAngle+(1-CosinusAngle)*Axis.z*Axis.z;
    return M;
}

void Update()
{
	float proj[16];
	float modl[16];	
	glGetFloatv( GL_PROJECTION_MATRIX, proj );
	glGetFloatv( GL_MODELVIEW_MATRIX, modl );
	gFrustum->Update(proj, modl);
}

char buffer[MAX_PATH];
void OnRender() {
	if (!gScene) return;

	//Calculate fps
	totalFrames++;
	int current = glutGet(GLUT_ELAPSED_TIME);
	if((current-startTime)>1000)
	{		
		float elapsedTime = float(current-startTime);
		fps = ((totalFrames * 1000.0f)/ elapsedTime) ;
		myTimestep = 1.0f / fps;
		startTime = current;
		totalFrames=0;
	}

	// Update counters
	gTotalActors = gActors.size();
	gDrawedActors = 0;

	// Update PhysX	
	UpdatePhysX(); 

	// Clear color and depth buffer
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Reset modelview matrix
	glLoadIdentity();
	
	/*
	PxMat44 modelview = PxMat44::createIdentity();
	modelview.setPosition(PxVec3(0,0,dist));
	PxMat44 rotation = Mat44Rotate(Deg2Rad(rX), PxVec3(1,0,0));
	rotation = rotation * Mat44Rotate(Deg2Rad(rY), PxVec3(0,1,0));
	modelview = modelview * rotation;
	glLoadMatrixf(modelview.front());
	*/

	// Set Camera
	glTranslatef(0,0,dist);
	glRotatef(rX, 1, 0, 0);
	glRotatef(rY, 0, 1, 0);

	// Update
	Update();

	// Enable depth testing
	glEnable(GL_DEPTH_TEST);

	// Draw the grid and axes
	DrawAxes();	
	DrawGrid(40);

	// Wireframe or not? Depends if lighting is enabled or not
	if (gDrawWireframe)
	{
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		glDisable(GL_LIGHTING);
	}
	else
	{
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		glEnable(GL_LIGHTING);
	}

	// Render actors
	RenderActors();

	// Disable lighting and depth test for font rendering
	glDisable(GL_LIGHTING);
	glDisable(GL_DEPTH_TEST);

	// Prepare for font rendering
	SetOrthoForFont();		
	glColor3f(1,1,1);

	// Render text
	sprintf_s(buffer, "FPS: %3.2f",fps);
	RenderSpacedBitmapString(20,20,0,GLUT_BITMAP_HELVETICA_12,buffer);
	sprintf_s(buffer, "Drawed actors: %d of %d", gDrawedActors, gTotalActors);
	RenderSpacedBitmapString(20,32,0,GLUT_BITMAP_HELVETICA_12,buffer);
	sprintf_s(buffer, "Water particles: %d", gWaterParticles.size());
	RenderSpacedBitmapString(20,44,0,GLUT_BITMAP_HELVETICA_12,buffer);

	sprintf_s(buffer, "Controls:");
	RenderSpacedBitmapString(20,80,0,GLUT_BITMAP_HELVETICA_12,buffer);
	sprintf_s(buffer, "Geometry type (1-3): %s", GetGeometryType(gGeoType));
	RenderSpacedBitmapString(20,92,0,GLUT_BITMAP_HELVETICA_12,buffer);
	sprintf_s(buffer, "Draw Wireframe (W): %s", gDrawWireframe ? "enabled" : "disabled");
	RenderSpacedBitmapString(20,104,0,GLUT_BITMAP_HELVETICA_12,buffer);
	sprintf_s(buffer, "Draw Boundbox (B): %s", gDrawBoundBox ? "enabled" : "disabled");
	RenderSpacedBitmapString(20,116,0,GLUT_BITMAP_HELVETICA_12,buffer);
	sprintf_s(buffer, "New actor (Space)");
	RenderSpacedBitmapString(20,128,0,GLUT_BITMAP_HELVETICA_12,buffer);

	// Reset projection for 3d rendering
	ResetPerspectiveProjection();
	
	glutSwapBuffers();
}

void Mouse(int button, int s, int x, int y)
{
	if (s == GLUT_DOWN) 
	{
		oldX = x; 
		oldY = y; 
	}	

	if(button == GLUT_MIDDLE_BUTTON)
		state = 0;
	else
		state = 1;
}

void Motion(int x, int y)
{
	if (state == 0)
		dist *= (1 + (y - oldY)/60.0f); 
	else
	{
		rY += (x - oldX)/5.0f; 
		rX += (y - oldY)/5.0f; 
	} 
	oldX = x; 
	oldY = y; 

	glutPostRedisplay(); 
}

void AddActorByState(unsigned int state)
{
	switch (state)
	{
	case 1:
		AddBox(PxVec3(0.0f, 10.0f, 0.0f), PxVec3(0.5,0.5,0.5));
		break;
	case 2:
		AddSphere();
		break;
	case 3:
		AddCapsule();
		break;
	case 4:
		AddWater();
		break;
	default:
		break;
	}
}

void KeyUp(unsigned char key, int x, int y)
{
	switch (key)
	{
	case 27:
		{
			exit(0);
			break;
		}
	case 49:
	case 50:
	case 51:
	case 52:
		{
			gGeoType = 1 + (key - 49);
			break;
		}
	case 114:
		{
			ResetScene();
			break;
		}
	case 98:
		{
			gDrawBoundBox = !gDrawBoundBox;
			break;
		}
	case 119:
		{
			gDrawWireframe = !gDrawWireframe;
			break;
		}
	default:
		break;
	}
}

void KeyDown(unsigned char key, int x, int y)
{
	switch (key)
	{
	case 32:
		{
			AddActorByState(gGeoType);
			break;
		}
	default:
		break;
	}
}

void OnIdle() {
	glutPostRedisplay();
}

void OnShutdown() {
	printf("Shutdown PhysX\n");
	ShutdownPhysX();

	delete gFrustum;
}

void main(int argc, char** argv) {
	atexit(OnShutdown);

	srand ( (unsigned int)time(NULL) );

	printf("Initialize Window\n");
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowSize(windowWidth, windowHeight);
	glutCreateWindow("Playing around with physx");

	glutDisplayFunc(OnRender);
	glutIdleFunc(OnIdle);
	glutReshapeFunc(OnReshape);

	glutMouseFunc(Mouse);
	glutMotionFunc(Motion);
	glutKeyboardUpFunc(KeyUp);
	glutKeyboardFunc(KeyDown);

	printf("Initialize OpenGL\n");
	InitGL();

	gFrustum = new CFrustum();

	printf("Initialize PhysX\n");
	InitializePhysX();

	printf("Main loop\n");
	glutMainLoop();		
}