#include "samplegame.h"
#include <gxframework/vertextype.h>
#include <pxtoolkit/PxTkStream.h>
#include <iostream>

SampleGame::SampleGame()
	: effect(NULL), camera(NULL), graphics(NULL), content(NULL), forceAlpha(false)
{
#ifdef _DEBUG
	connectToPvd = true;
	mouseCentering = false;
#else
	connectToPvd = false;
	mouseCentering = true;
#endif

	projectionMtx = modelMtx = PxMat44::createIdentity();
	flatBlackMaterial = Material(Color::Black);

	cameraVelocity = 40.0f;
}

void SampleGame::initialize()
{
	camera = &this->getCamera();
	graphics = &this->getGraphics();
	effect = graphics->getBasicEffect();
	content = &graphics->getContentManager();

	if ( IsDebuggerPresent() != FALSE )
	{
		rootDirectory = "..\\Content\\";
		content->setRootDirectory(rootDirectory);
	}
	else
	{
		rootDirectory = "..\\..\\Content\\";
		content->setRootDirectory(rootDirectory);
	}

	this->initializePhysics();

	defaultPhysicsMaterial = physics->createMaterial(0.8f, 0.7f, 0.1f);

	glEnable(GL_CULL_FACE);
	glEnable(GL_DEPTH_TEST);

	effect->setLightPosition(0, PxVec3(-100, 100, 100));
	effect->setLightAttenuation(0, PxVec3(1.0f, 0.001f, 0.0f));

	boxMesh = MeshHelper::createBox(graphics);
	planeMesh = MeshHelper::createGrid(graphics, 200, 50);
	shpereMesh = MeshHelper::createSphere(graphics, 1.0f, 24, 24);
	capsuleMesh = MeshHelper::createCapsule(graphics, 1.0f, 1.0f, 12, 24);

	if ( connectToPvd )
	{
		this->connectToVisualDebugger("127.0.0.1");

		if ( scene )
		{
			scene->setVisualizationParameter(PxVisualizationParameter::eSCALE, 10.0f);
			scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_SHAPES, 1.0f);
			scene->setVisualizationParameter(PxVisualizationParameter::eJOINT_LOCAL_FRAMES, 1.0f);
			scene->setVisualizationParameter(PxVisualizationParameter::eJOINT_LIMITS, 1.0f);
			scene->setVisualizationParameter(PxVisualizationParameter::eWORLD_AXES, 1.0f);

			if (physics->getVisualDebugger() )
			{
				physics->getVisualDebugger()->setVisualizeConstraints(true);
				physics->getVisualDebugger()->setVisualDebuggerFlag(PxVisualDebuggerFlags::eTRANSMIT_CONTACTS, true);
			}
		}
	}

#ifdef NDEBUG
	// w trybie DEBUG myszka jest zawsze widoczna
	getMouse().setCursorCentring(true);
	getMouse().setCursorVisible(false);
	getMouse().setCursorSmoothing(true);
#endif

	printHelp();
}

void SampleGame::release()
{
	if ( connectToPvd )
	{
		this->disconnectFromVisualDebugger();
	}

	this->realeasePhysics();
}

void SampleGame::input(GxF32 elapsedTime)
{
	const GxF32 cameraDelta = cameraVelocity * elapsedTime;

	GxF32 cameraSpeedUp = 0;
	// po kliknieciu lewego shiftu kamera przyspiesza
	if ( getKeyboard().isKeyDown(Keys::LShift) )
	{
		cameraSpeedUp = 40.0f * elapsedTime;
	}

	if ( getKeyboard().isKeyDown(Keys::W) )
	{
		camera->walk(1.0f, cameraDelta + cameraSpeedUp);
	}
	else if ( getKeyboard().isKeyDown(Keys::S) )
	{
		camera->walk(-1.0f, cameraDelta + cameraSpeedUp);
	}

	if ( getKeyboard().isKeyDown(Keys::D) )
	{
		camera->strafe(1.0f, cameraDelta + cameraSpeedUp);
	}
	else if ( getKeyboard().isKeyDown(Keys::A) )
	{
		camera->strafe(-1.0f, cameraDelta + cameraSpeedUp);
	}

	if ( getKeyboard().isKeyDown(Keys::E) )
	{
		camera->fly(1.0f, cameraDelta + cameraSpeedUp);
	}
	else if ( getKeyboard().isKeyDown(Keys::Q) )
	{
		camera->fly(-1.0f, cameraDelta + cameraSpeedUp);
	}

	if ( mouseCentering || getMouse().isButtonDown(MouseButtons::Left) )
	{
		PxF32 angleX = getMouse().getRelativePositionX() * cameraDelta * 0.005f;
		PxF32 angleY = getMouse().getRelativePositionY() * cameraDelta * 0.005f;

		camera->rotate(-angleX, -angleY);
	}

	if ( getKeyboard().isKeyPressed(Keys::F1) )
	{
		mouseCentering = !mouseCentering;
		getMouse().setCursorCentring(mouseCentering);
		getMouse().setCursorVisible(!mouseCentering);
	}

	if ( getKeyboard().isKeyPressed(Keys::F2) )
	{
		static bool fullscreen = false;
		fullscreen = !fullscreen;
		this->setFullScreen(fullscreen);
	}

	if ( getKeyboard().isKeyPressed(Keys::F3) )
	{
		static bool wireframe = false;
		wireframe = !wireframe;
		glPolygonMode(GL_FRONT_AND_BACK, wireframe ? GL_LINE : GL_FILL);
	}


	// copied from sdk samples - synchronizacja kamery w PVD
	if ( physics )
	{
		if ( physics->getPvdConnectionManager() )
		{
			PxVec3 pos = camera->getPosition();
			PxVec3 dir = camera->getViewDir();
			PxVec3 target = pos + dir * 30.0f;

			physics->getPvdConnectionManager()->setCamera("SampleCamera", pos, PxVec3(0,1,0), target);
		}
	}
}

void SampleGame::update(GxF32 elapsedTime)
{
	scene->simulate(elapsedTime);

	scene->fetchResults(true);
}

void SampleGame::render(GxF32 elapsedTime)
{

}

void SampleGame::reshape(GxU32 width, GxU32 height)
{
	if ( height == 0 ) height = 1;

	glViewport(0, 0, width, height);

	GxF32 aspectRatio = width / static_cast<GxF32>(height);

	projectionMtx = MathHelper::createPerspective(45.0f, aspectRatio, 1.0f, 1000.0f);
}

void SampleGame::initializePhysics()
{
	static PxDefaultErrorCallback defaultErrorCallback;
	static PxDefaultAllocator defaultAllocator;

	foundation = PxCreateFoundation(PX_PHYSICS_VERSION, defaultAllocator, defaultErrorCallback);
	if ( foundation != NULL )
	{
		physics = PxCreatePhysics(PX_PHYSICS_VERSION, *foundation, PxTolerancesScale());
		if ( physics != NULL )
		{
			PxInitExtensions(*physics);

			cooking = PxCreateCooking(PX_PHYSICS_VERSION, *foundation, PxCookingParams());
			if ( cooking )
			{
				const PxU32 numberOfThreads = 1;

				PxSceneDesc desc(physics->getTolerancesScale());
				desc.gravity = PxVec3(0, -9.81f, 0);
				desc.filterShader = PxDefaultSimulationFilterShader;
				desc.cpuDispatcher = PxDefaultCpuDispatcherCreate(numberOfThreads);

				this->customizeScene(desc);

				scene = physics->createScene(desc);
				if ( scene != NULL )
				{
					// gotowa scena
				}
			}
		}
	}
}

void SampleGame::realeasePhysics()
{
	if ( foundation )
	{
		if ( physics )
		{
			if ( cooking )
			{
				cooking->release();
				cooking = NULL;
			}

			PxCloseExtensions();

			physics->release();
			physics = NULL;
		}

		foundation->release();
		foundation = NULL;
	}
}

void SampleGame::customizeScene(PxSceneDesc& desc)
{

}

void SampleGame::renderScene(const PxScene* scene)
{
	const GxU32 bufferSize = 2048;
	static PxActor* buffer[bufferSize];

	const PxActorTypeSelectionFlags selectionFlags = PxActorTypeSelectionFlag::eRIGID_DYNAMIC | PxActorTypeSelectionFlag::eRIGID_STATIC;
	const PxU32 numberOfRigidActors = scene->getNbActors(selectionFlags);

	if ( numberOfRigidActors > 0 )
	{
		if ( numberOfRigidActors <= bufferSize )
		{
			scene->getActors(selectionFlags, buffer, bufferSize);

			for (PxU32 i = 0 ; i < numberOfRigidActors ; ++i)
			{
				PxRigidActor* actor = buffer[i]->isRigidActor();
				if (actor != NULL)
				{
					this->renderRigidActor(actor);
				}
			}
		}
		else
		{
			printf("Actors buffer too small.\n");
		}
	}
}

void SampleGame::renderRigidActor(const PxRigidActor* actor)
{
	const PxU32 bufferSize = 16;
	static PxShape* buffer[bufferSize];

	PxU32 numberOfShapes = actor->getNbShapes();
	if ( numberOfShapes > 0 )
	{
		if ( numberOfShapes <= bufferSize )
		{
			actor->getShapes(buffer, bufferSize);

			for (GxU32 i = 0 ; i < numberOfShapes ; ++i)
			{
				PxShape* shape = buffer[i];

				this->renderShape(shape);
			}
		}
		else
		{
			printf("Shapes buffer too small.\n");
		}
	}
}

void SampleGame::renderShape(const PxShape* shape)
{
	bool isAlpha = false;
	if ( shape->userData )
	{
		isAlpha = ((Material*)shape->userData)->alpha < 1.0f;
	}

	if (!isAlpha || forceAlpha)
	{
		switch( shape->getGeometryType() )
		{
		case PxGeometryType::eSPHERE:
			this->renderSphereShape(shape);
			break;

		case PxGeometryType::ePLANE:
			this->renderPlaneShape(shape);
			break;

		case PxGeometryType::eCAPSULE:
			this->renderCapsuleShape(shape);
			break;

		case PxGeometryType::eBOX:
			this->renderBoxShape(shape);
			break;
		}
	}
}

void SampleGame::renderSphereShape(const PxShape* shape)
{
	PxTransform pose = PxShapeExt::getGlobalPose(*shape);
	PxMat44 mat = PxMat44(pose);

	PxSphereGeometry geometry;
	shape->getSphereGeometry(geometry);
	mat = mat * MathHelper::createScale(geometry.radius);

	Material* graphicsMaterial = static_cast<Material*>(shape->userData);
	if ( !graphicsMaterial )
	{
		graphicsMaterial = &Material::Default;
	}

	effect->setModelMatrix(mat);
	graphics->renderMesh(effect, shpereMesh, *graphicsMaterial);
}

void SampleGame::renderPlaneShape(const PxShape* shape)
{
	PxTransform pose = PxShapeExt::getGlobalPose(*shape); // to samo co: shape->getActor().getGlobalPos() * shape->getLocalPos();
	pose.q = pose.q * PxQuat(-PxHalfPi, PxVec3(0, 0, 1));

	Material* graphicsMaterial = static_cast<Material*>(shape->userData);
	if ( !graphicsMaterial )
	{
		graphicsMaterial = &Material::Default;
	}

	effect->setModelMatrix(PxMat44(pose));
	graphics->renderMesh(effect, planeMesh, *graphicsMaterial);
}

void SampleGame::renderCapsuleShape(const PxShape* shape)
{
	PxTransform pose = PxShapeExt::getGlobalPose(*shape);
	pose.q = pose.q * PxQuat(-PxHalfPi, PxVec3(0, 0, 1)); // orientacja kapsuły w PhysX jest pozioma (wzdłóz osi X), a w GxFramework pionowa wzdłóż osi Y
	PxMat44 mat = PxMat44(pose);

	PxCapsuleGeometry geometry;
	shape->getCapsuleGeometry(geometry);

	const GxF32 yscale = (geometry.halfHeight + geometry.radius) / 2.0f; // siatka kapsuly: polowa walca 1 + promien 1 - patrz metoda initialize() i tworzenie siatki
	mat = mat * MathHelper::createScale(geometry.radius, yscale, geometry.radius);

	Material* graphicsMaterial = static_cast<Material*>(shape->userData);
	if ( !graphicsMaterial )
	{
		graphicsMaterial = &Material::Default;
	}

	effect->setModelMatrix(mat);
	graphics->renderMesh(effect, capsuleMesh, *graphicsMaterial);
}

void SampleGame::renderBoxShape(const PxShape* shape)
{
	PxTransform pose = PxShapeExt::getGlobalPose(*shape);
	PxMat44 mat = PxMat44(pose);

	PxBoxGeometry geometry;
	shape->getBoxGeometry(geometry);
	mat = mat * MathHelper::createScale(geometry.halfExtents);

	Material* graphicsMaterial = static_cast<Material*>(shape->userData);
	if ( !graphicsMaterial )
	{
		graphicsMaterial = &Material::Default;
	}

	effect->setModelMatrix(mat);
	graphics->renderMesh(effect, boxMesh, *graphicsMaterial);
}

bool SampleGame::raycastFromCamera(PxVec3& impact, const PxRigidActor* target)
{
	PxRaycastHit hit;

	const PxSceneQueryFlags flags = PxSceneQueryFlag::eIMPACT;
	if ( scene->raycastSingle(camera->getPosition(), camera->getViewDir(), 350, flags, hit, PxSceneQueryFilterData(PxSceneQueryFilterFlag::eSTATIC) ) )
	{
		if ( target != NULL )
		{
			if ( &hit.shape->getActor() != target )
			{
				return false;
			}
		}

		impact = hit.impact;
		return true;
	}

	return false;
}

PxRigidStatic* SampleGame::createStaticActor(const PxTransform& pose, const PxGeometry& geometry, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial)
{
	PxRigidStatic* staticActor = physics->createRigidStatic(pose);
	PxShape* shape = staticActor->createShape(geometry, *physicsMaterial);
	shape->userData = (void*)graphicsMaterial;
	return staticActor;
}

PxRigidDynamic* SampleGame::createDynamicActor(const PxTransform& pose, const PxGeometry& geometry, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial, PxReal density)
{
	PxRigidDynamic* dynamicActor = physics->createRigidDynamic(pose);
	PxShape* shape = dynamicActor->createShape(geometry, *physicsMaterial);
	shape->userData = (void*)graphicsMaterial;
	PxRigidBodyExt::updateMassAndInertia(*dynamicActor, density);
	return dynamicActor;
}

void SampleGame::addStackedBoxesAt(const PxVec3& position, GxU32 count, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial, PxReal density)
{
	GX_ASSERT(scene, "Before create any actors must initialize physics.");

	const PxF32 halfDim = 2.0f;
	const PxF32 spacing = 0.02f;

	PxTransform pose(position);
	pose.p.y += halfDim;

	while(count--)
	{
		PxRigidDynamic* actor = this->createDynamicActor(pose, PxBoxGeometry(halfDim, halfDim, halfDim), physicsMaterial, graphicsMaterial, density);
		scene->addActor(*actor);

		pose.p.y += 2 * halfDim + spacing;

		const PxF32 angle = ( rand() % 100 ) / 100.0f * 30.0f;
		pose.q *= PxQuat(angle, PxVec3(0, 1, 0));
	}
}

void SampleGame::addPiramidBoxesAt(const PxVec3& position, GxU32 count, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial, PxReal density /*= 1*/)
{
	GX_ASSERT(scene, "Before create any actors must initialize physics.");

	const PxF32 halfDim = 2.0f;
	const PxF32 spacing = 0.02f;

	PxTransform pose(position);
	pose.p.y += halfDim + spacing;

	const PxF32 offsetBB = 2.0f * halfDim + spacing; // box -- box
	PxF32 offsetOrigin = pose.p.x - (count - 1) * 0.5f * offsetBB;

	while(count)
	{
		for (GxU32 i = 0 ; i < count ; ++i )
		{
			pose.p.x = offsetOrigin + i * offsetBB;

			PxRigidDynamic* actor = this->createDynamicActor(pose, PxBoxGeometry(halfDim, halfDim, halfDim), physicsMaterial, graphicsMaterial, density);
			scene->addActor(*actor);
		}

		pose.p.y += offsetBB;
		offsetOrigin += halfDim;
		count--;
	}
}

void SampleGame::addWallBoxesAt(const PxVec3& position, GxU32 countHeight, GxU32 countWidth, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial, PxReal density)
{
	const PxF32 halfDim = 2.0f;
	const PxF32 spacing = 0.02f;

	PxTransform pose(position);
	pose.p.y += halfDim + spacing;

	const PxF32 offsetBB = 2.0f * halfDim + spacing; // box -- box
	PxF32 offsetOrigin = pose.p.x - (countWidth - 1) * 0.5f * offsetBB;

	for (GxU32 i = 0 ; i < countHeight ; ++i)
	{
		for (GxU32 j = 0 ; j < countWidth ; ++j )
		{
			pose.p.x = offsetOrigin + j * offsetBB;

			PxRigidDynamic* actor = this->createDynamicActor(pose, PxBoxGeometry(halfDim, halfDim, halfDim), physicsMaterial, graphicsMaterial, density);
			scene->addActor(*actor);
		}

		pose.p.y += offsetBB;
	}
}

void SampleGame::throwBox(const PxVec3& position, GxF32 halfExtents, const PxVec3& initialVelocity, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial, PxReal density)
{
	PxTransform pose(position);
	if ( pose.p.y < halfExtents ) pose.p.y = halfExtents + 0.02f;

	PxRigidDynamic* actor = this->createDynamicActor(pose, PxBoxGeometry(halfExtents, halfExtents, halfExtents), physicsMaterial, graphicsMaterial, density);
	actor->setLinearVelocity(initialVelocity);
	actor->setLinearDamping(0.2f);

	scene->addActor(*actor);
}

void SampleGame::throwBall(const PxVec3& position, GxF32 radius, const PxVec3& initialVelocity, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial /*= NULL*/, PxReal density /*= 1*/)
{
	PxTransform pose(position);
	PxRigidDynamic* actor = this->createDynamicActor(pose, PxSphereGeometry(radius), physicsMaterial, graphicsMaterial, density);
	actor->setLinearVelocity(initialVelocity);
	actor->setLinearDamping(0.2f);

	scene->addActor(*actor);
}


Mesh* SampleGame::createGraphicsMeshFromConvexMesh(PxConvexMesh* convexMesh)
{
	PxU32 numberOfPolygons = convexMesh->getNbPolygons();
	PxU32 totalNumberOfVertices = 0;
	PxU32 totalNumberOfTriangles = 0;

	for (PxU32 i = 0 ; i < numberOfPolygons ; ++i)
	{
		PxHullPolygon polygon;
		convexMesh->getPolygonData(i, polygon);

		totalNumberOfVertices += polygon.mNbVerts;
		totalNumberOfTriangles += polygon.mNbVerts - 2;
	}

	const PxVec3* convexVertices = convexMesh->getVertices();
	const PxU8* convexIndices = convexMesh->getIndexBuffer();

	VertexBuffer* vertexBuffer = graphics->createVertexBuffer<VertexPositionNormalTexture>(totalNumberOfVertices, GL_STATIC_DRAW);
	VertexPositionNormalTexture* vertices = vertexBuffer->lock<VertexPositionNormalTexture>(GL_WRITE_ONLY);

	IndexBuffer* indexBuffer = graphics->createIndexBuffer(IndexElementType::UnsignedShort, totalNumberOfVertices * 3, GL_STATIC_DRAW);
	GxU16* indices = indexBuffer->lock<GxU16>(GL_WRITE_ONLY);

	GxU32 offset = 0;
	for (PxU32 i = 0 ; i < numberOfPolygons ; ++i)
	{
		PxHullPolygon polygon;
		convexMesh->getPolygonData(i, polygon);

		const PxU8* polygonIndices = convexIndices + polygon.mIndexBase;

		for(PxU32 j = 0 ; j < polygon.mNbVerts ; ++j)
		{
			vertices[offset + j].position = convexVertices[polygonIndices[j]];
			vertices[offset + j].normal = PxVec3(polygon.mPlane[0], polygon.mPlane[1], polygon.mPlane[2]);
		}

		for (PxU32 j = 2 ; j < polygon.mNbVerts ; ++j)
		{
			*(indices++) = PxU16(offset);
			*(indices++) = PxU16(offset + j - 1);
			*(indices++) = PxU16(offset + j);
		}

		offset += polygon.mNbVerts;
	}

	for (PxU32 i = 0 ; i < totalNumberOfVertices ; ++i)
	{
		switch(i % 3)
		{
		case 0:
			(vertices++)->texcoord = PxVec3(0, 0, 0);
			break;
		case 1:
			(vertices++)->texcoord = PxVec3(0, 1, 0);
			break;
		case 2:
			(vertices++)->texcoord = PxVec3(1, 0, 0);
			break;
		default:
			PX_ASSERT(false);
		}
	}

	PX_ASSERT(offset == totalNumberOfVertices);

	indexBuffer->unlock();
	vertexBuffer->unlock();

	Mesh* graphicsMesh = new Mesh();
	graphics->addResource(graphicsMesh);
	graphicsMesh->setVertexBuffer(vertexBuffer);
	graphicsMesh->setIndexBuffer(indexBuffer);

	return graphicsMesh;
}

Mesh* SampleGame::createGraphicsMeshFromTriangleMesh(PxTriangleMesh* triangleMesh)
{
	const PxU32 numberOfVertices = triangleMesh->getNbVertices();
	const PxVec3* positions = triangleMesh->getVertices();

	const PxU32 numberOfTriangles = triangleMesh->getNbTriangles();
	const void* triangles = triangleMesh->getTriangles();

	PX_ASSERT(triangleMesh->has16BitTriangleIndices()); // tylko 16bit indeksy

	IndexBuffer* indexBuffer = graphics->createIndexBuffer(IndexElementType::UnsignedShort, numberOfTriangles * 3, GL_STATIC_DRAW);
	indexBuffer->setData<PxU16>((PxU16*)triangles);

	VertexBuffer* vertexBuffer = graphics->createVertexBuffer<VertexPositionNormalTexture>(numberOfVertices, GL_STATIC_DRAW);
	VertexPositionNormalTexture* vertices = vertexBuffer->lock<VertexPositionNormalTexture>(GL_WRITE_ONLY);

	for (PxU32 i = 0 ; i < numberOfTriangles ; ++i)
	{
		const PxU16* triangleIndices = (PxU16*)triangles + (i * 3);

		const PxVec3* vertex0 = positions + triangleIndices[0];
		const PxVec3* vertex1 = positions + triangleIndices[1];
		const PxVec3* vertex2 = positions + triangleIndices[2];

		const PxVec3 edge1 = (*vertex1) - (*vertex0);
		const PxVec3 edge2 = (*vertex2) - (*vertex0);

		const PxVec3 normal = edge1.cross(edge2).getNormalized();

		VertexPositionNormalTexture* vertex = &vertices[triangleIndices[0]];
		vertex->position = *vertex0;
		vertex->normal = normal;
		vertex->texcoord = PxVec3(0.0f); // na razie bez tekstur

		vertex = &vertices[triangleIndices[1]];
		vertex->position = *vertex1;
		vertex->normal = normal;
		vertex->texcoord = PxVec3(0.0f);

		vertex = &vertices[triangleIndices[2]];
		vertex->position = *vertex2;
		vertex->normal = normal;
		vertex->texcoord = PxVec3(0.0f);
	}

	vertexBuffer->unlock();

	Mesh* graphicsMesh = new Mesh();
	graphics->addResource(graphicsMesh);
	graphicsMesh->setVertexBuffer(vertexBuffer);
	graphicsMesh->setIndexBuffer(indexBuffer);

	return graphicsMesh;
}

PxConvexMesh* SampleGame::createConvexMesh(const PxVec3* verts, PxU32 vertCount)
{
	PxConvexMeshDesc convexDesc;
	convexDesc.points.data = verts;
	convexDesc.points.stride = sizeof(PxVec3);
	convexDesc.points.count = vertCount;
	convexDesc.flags = PxConvexFlag::eCOMPUTE_CONVEX;

	PxToolkit::MemoryOutputStream outputStream;
	if ( cooking->cookConvexMesh(convexDesc, outputStream) )
	{
		PxToolkit::MemoryInputData inputData(outputStream.getData(), outputStream.getSize());
		PxConvexMesh* convexMesh = physics->createConvexMesh(inputData);

		return convexMesh;
	}
	else
	{
		printf("Unable to cook convex mesh.\n");
	}

	return NULL;
}

void SampleGame::addBoxAt(const PxVec3& position, GxF32 halfX, GxF32 halfY, GxF32 halfZ, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial /*= NULL*/, PxReal density /*= 1*/)
{
	PxTransform pose(position);
	if ( pose.p.y < halfY ) pose.p.y = halfY + 0.02f; // zakladam ze plane jest na y = 0

	PxRigidDynamic* actor = this->createDynamicActor(pose, PxBoxGeometry(halfX, halfY, halfZ), physicsMaterial, graphicsMaterial, density);
	scene->addActor(*actor);
}

void SampleGame::addCapsuleAt(const PxVec3& position, GxF32 halfHeight, GxF32 radius, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial /*= NULL*/, PxReal density /*= 1*/)
{
	PxTransform pose(position);
	if ( pose.p.y < radius ) pose.p.y = radius + 0.02f; // zakladam ze plane jest na y = 0

	PxRigidDynamic* actor = this->createDynamicActor(pose, PxCapsuleGeometry(radius, halfHeight), physicsMaterial, graphicsMaterial, density);
	scene->addActor(*actor);
}

void SampleGame::addSphereAt(const PxVec3& position, GxF32 radius, const PxMaterial* physicsMaterial, Gx::Material* graphicsMaterial /*= NULL*/, PxReal density /*= 1*/)
{
	PxTransform pose(position);
	if ( pose.p.y < radius ) pose.p.y = radius + 0.02f; // zakladam ze plane jest na y = 0

	PxRigidDynamic* actor = this->createDynamicActor(pose, PxSphereGeometry(radius), physicsMaterial, graphicsMaterial, density);
	scene->addActor(*actor);
}

PxTriangleMesh* SampleGame::createTriangleMesh(const void* vertices, GxU32 vertexCount, GxU32 vertexStride, const GxU16* indices, GxU32 indexCount)
{
	PxTriangleMeshDesc triangleDesc;
	triangleDesc.points.data = vertices;
	triangleDesc.points.count = vertexCount;
	triangleDesc.points.stride = vertexStride;

	triangleDesc.triangles.data = indices;
	triangleDesc.triangles.count = indexCount / 3;
	triangleDesc.triangles.stride = 3 * sizeof(GxU16);
	triangleDesc.flags = PxMeshFlag::e16_BIT_INDICES;

	PxToolkit::MemoryOutputStream outputStream;
	if ( cooking->cookTriangleMesh(triangleDesc, outputStream) )
	{
		PxToolkit::MemoryInputData inputData(outputStream.getData(), outputStream.getSize());
		PxTriangleMesh* triangleMesh = physics->createTriangleMesh(inputData);

		return triangleMesh;
	}
	else
	{
		printf("Unable to cook triangle mesh.\n");
	}

	return NULL;
}

PxTriangleMesh* SampleGame::createTriangleMeshFromGraphicsMesh(Mesh* mesh)
{
	VertexBuffer* vertexBuffer = mesh->getVertexBuffer(); // to są bufory na GPU
	IndexBuffer* indexBuffer = mesh->getIndexBuffer();

	GX_ASSERT(vertexBuffer && indexBuffer, "Invalid mesh");
	GX_ASSERT(vertexBuffer->getStride() == sizeof(VertexPositionNormalTexture), "Unsupported type of vertex data.");

	PxTriangleMeshDesc desc;

	const void* vertices = vertexBuffer->lock<void>(GL_READ_ONLY); // z GPU do CPU
	
	if ( vertices )
	{
		desc.points.data = vertices;
		desc.points.count = vertexBuffer->getVertexCount();
		desc.points.stride = vertexBuffer->getStride();
	}

	const void* indices = indexBuffer->lock<void>(GL_READ_ONLY);;
	if ( indices )
	{
		GX_ASSERT((indexBuffer->getIndexCount() % 3) == 0, "Only triangle mesh supporetd.");

		desc.triangles.data = indices;
		desc.triangles.count = indexBuffer->getIndexCount() / 3;
		desc.triangles.stride = 3 * indexBuffer->getIndexSize();

		if ( indexBuffer->getIndexSize() == 2 )
		{
			desc.flags = PxMeshFlag::e16_BIT_INDICES;
		}
	}

	PxTriangleMesh* triangleMesh = NULL;
	PxToolkit::MemoryOutputStream outputStream;
	if ( cooking->cookTriangleMesh(desc, outputStream) )
	{
		PxToolkit::MemoryInputData inputData(outputStream.getData(), outputStream.getSize());
		triangleMesh = physics->createTriangleMesh(inputData);
	}
	else
	{
		printf("Unable to cook triangle mesh from graphics mesh.\n");
	}

	vertexBuffer->unlock();
	indexBuffer->unlock();

	return triangleMesh;
}

PxConvexMesh* SampleGame::createConvexMeshFromGraphicsMesh(Mesh* mesh)
{
	VertexBuffer* vertexBuffer = mesh->getVertexBuffer();

	GX_ASSERT(vertexBuffer, "Invalid mesh");
	GX_ASSERT(vertexBuffer->getStride() == sizeof(VertexPositionNormalTexture), "Unsupported type of vertex data.");

	const void* vertices = vertexBuffer->lock<void>(GL_READ_ONLY);

	PxConvexMeshDesc convexDesc;
	convexDesc.points.data = vertices;
	convexDesc.points.stride = vertexBuffer->getStride();
	convexDesc.points.count = vertexBuffer->getVertexCount();
	convexDesc.flags = PxConvexFlag::eCOMPUTE_CONVEX;

	PxConvexMesh* convexMesh = NULL;
	PxToolkit::MemoryOutputStream outputStream;
	if ( cooking->cookConvexMesh(convexDesc, outputStream) )
	{
		PxToolkit::MemoryInputData inputData(outputStream.getData(), outputStream.getSize());
		PxConvexMesh* convexMesh = physics->createConvexMesh(inputData);
	}
	else
	{
		printf("Cooking convex mesh failed.\n");
	}

	vertexBuffer->unlock();

	return convexMesh;
}

PxVec3 computeNormal(const PxVec3& a, const PxVec3& b, const PxVec3& c)
{
	PxVec3 edge1 = b - a;
	PxVec3 edge2 = c - a;

	return edge1.cross(edge2).getNormalized();
}

Mesh* SampleGame::createGraphicsMeshFromHeightField(PxHeightField* heightfield, GxF32 heightScale, GxF32 rowScale, GxF32 columnScale)
{
	const PxU32 nbRows = heightfield->getNbRows();
	const PxU32 nbColumns = heightfield->getNbColumns();
	
	const PxU32 nbVertices = nbRows * nbColumns;
	const PxU32 nbTriangles = (nbRows - 1) * (nbColumns - 1) * 2;
	const PxU32 nbIndices = nbTriangles * 3;

	PxHeightFieldSample* samples = new PxHeightFieldSample[nbVertices];
	heightfield->saveCells(samples, nbVertices * sizeof(PxHeightFieldSample));

	VertexBuffer* vertexBuffer = graphics->createVertexBuffer<VertexPositionNormalTexture>(nbVertices, GL_STATIC_DRAW);
	VertexPositionNormalTexture* vertices = vertexBuffer->lock<VertexPositionNormalTexture>(GL_WRITE_ONLY);

	for (GxU32 i = 0 ; i < nbRows ; ++i)
	{
		for (GxU32 j = 0 ; j < nbColumns ; ++j)
		{
			const GxU32 k = i * nbColumns + j;

			vertices[k].position = PxVec3(GxF32(i) * rowScale, samples[k].height * heightScale, GxF32(j) * columnScale);
			vertices[k].texcoord = PxVec3(0); // bez tekstur
		}
	}

	IndexBuffer* indexBuffer = graphics->createIndexBuffer(IndexElementType::UnsignedShort, nbIndices, GL_STATIC_DRAW);
	GxU16* indices = indexBuffer->lock<GxU16>(GL_WRITE_ONLY);

	for (GxU32 i = 0 ; i < nbColumns - 1 ; ++i)
	{
		for (GxU32 j = 0 ; j < nbRows - 1 ; ++j)
		{
			const GxU16 k = GxU16(i * nbRows + j);
			const GxU16 p = GxU16((i+1) * nbRows + j);

			/*
			from samples SDK
			PxU8 tessFlag = sampleBuffer[i+j*nbCols].tessFlag();
			PxU32 i0 = i * nbRows + j;
			PxU32 i1 = i * nbRows + j + 1;
			PxU32 i2 = (i+1) * nbRows + j;
			PxU32 i3 = (i+1) * nbRows + j+1;
			// i2---i3
			// |    |
			// |    |
			// i0---i1
			// this is really a corner vertex index, not triangle index
			PxU32 mat0 = hf->getTriangleMaterialIndex((j*nbCols+i)*2);
			PxU32 mat1 = hf->getTriangleMaterialIndex((j*nbCols+i)*2+1);
			bool hole0 = (mat0 == PxHeightFieldMaterial::eHOLE);
			bool hole1 = (mat1 == PxHeightFieldMaterial::eHOLE);
			// first triangle
			indices[6 * (i * (nbRows - 1) + j) + 0] = hole0 ? i0 : i2; // duplicate i0 to make a hole
			indices[6 * (i * (nbRows - 1) + j) + 1] = i0;
			indices[6 * (i * (nbRows - 1) + j) + 2] = tessFlag ? i3 : i1;
			// second triangle
			indices[6 * (i * (nbRows - 1) + j) + 3] = hole1 ? i1 : i3; // duplicate i1 to make a hole
			indices[6 * (i * (nbRows - 1) + j) + 4] = tessFlag ? i0 : i2;
			indices[6 * (i * (nbRows - 1) + j) + 5] = i1;
			*/

			// i2---i3
			// |    |
			// |    |
			// i0---i1
			
			const GxU16 s = i + j * nbColumns;
			const PxU8 tessFlag = samples[s].tessFlag();

			const GxU16 i0 = k;
			const GxU16 i1 = k + 1;
			const GxU16 i2 = p;
			const GxU16 i3 = p + 1;

			*(indices++) = i2;
			*(indices++) = i0;
			GxU16 t = tessFlag ? i3 : i1;
			*(indices++) = t;

			PxVec3 normal = computeNormal(vertices[i2].position, vertices[i0].position, vertices[t].position);
			vertices[i2].normal = normal;
			vertices[i0].normal = normal;
			vertices[t].normal = normal;

			*(indices++) = i3;
			t = tessFlag ? i0 : i2;
			*(indices++) = t;
			*(indices++) = i1;

			normal = computeNormal(vertices[i3].position, vertices[t].position, vertices[i1].position);
			vertices[i3].normal = normal;
			vertices[t].normal = normal;
			vertices[i1].normal = normal;
		}
	}

	delete[] samples;
	samples = NULL;

	vertexBuffer->unlock();
	indexBuffer->unlock();

	Mesh* graphicsMesh = new Mesh();
	graphics->addResource(graphicsMesh);
	graphicsMesh->setVertexBuffer(vertexBuffer);
	graphicsMesh->setIndexBuffer(indexBuffer);

	return graphicsMesh;
}

void SampleGame::connectToVisualDebugger(const char* ip, GxU16 port)
{
	if ( physics != NULL )
	{
		physx::debugger::comm::PvdConnectionManager* manager = physics->getPvdConnectionManager();
		if ( manager != NULL )
		{
			PxVisualDebuggerConnectionFlags connectionFlags = PxVisualDebuggerExt::getDefaultConnectionFlags();

			debugger::comm::PvdConnection* pvdConnection = PxVisualDebuggerExt::createConnection(manager, ip, port, 100, connectionFlags);
			if ( pvdConnection )
			{
				pvdConnection->release();
			}
		}
	}
}

void SampleGame::disconnectFromVisualDebugger()
{
	if ( physics->getPvdConnectionManager() )
	{
		if ( physics->getPvdConnectionManager()->isConnected() )
		{
			physics->getPvdConnectionManager()->disconnect();
		}
	}
}

void SampleGame::printHelp()
{
	std::cout << "# Sterowanie kamera:" << std::endl
		<< "W / S - przesuwa kamere do przodu / do tylu" << std::endl
		<< "A / D - przesuwa kamere w lewo / w prawo" << std::endl
		<< "Q / E - przesuwa kamere w dol / w gore" << std::endl
		<< "Lewy Shift - zwieksza predkosc przesuwania kamery" << std::endl << std::endl
		<< "# Inne:" << std::endl
		<< "F1 - przelacza tryb sterowania kamera" << std::endl
		<< "F2 - przelacza na tryb pelnoekranowy" << std::endl
		<< "F3 - przelacza tryb renderingu na wireframe" << std::endl;
}
