#include "SampleJointsGame.h"

#include <gxframework/vertextype.h>

#include <sstream>
#include <iomanip>
#include <algorithm>

SampleJointsGame::SampleJointsGame()
	: planeGraphicsMaterial(Color::Black)
{
	preferAntialising(4);
}

void SampleJointsGame::customizeScene(PxSceneDesc& desc)
{
}

void SampleJointsGame::initialize()
{
	SampleGame::initialize();

	// przygotowanie materialu cegly
	brickGraphicsMaterial.texture = content->loadTexture("brick.tga");

	// tworzenie podlogi
	planeActor = createStaticActor(PxTransformFromPlaneEquation(PxPlane(0, 1, 0, 0)), PxPlaneGeometry(), defaultPhysicsMaterial, &planeGraphicsMaterial);
	scene->addActor(*planeActor);

	// dzwigar
	girderActor = createStaticActor(PxTransform(PxVec3(0, 30, 0)), PxBoxGeometry(45.0f, 0.5f, 1.0f), defaultPhysicsMaterial, &Material::Obsidian);
	scene->addActor(*girderActor);

	//pomocnicze
	PxTransform pose(PxVec3(-40,10,0));
	PxVec3 boxFrame=PxVec3(0,2.0f,0);
	const PxBoxGeometry geometry(PxVec3(2.0f));
	const PxVec3 offsetRight(20,0,0);
	PxVec3 girderFrame = PxVec3(-40,-0.5f,0);

	//fixed joint
	PxRigidDynamic* actorDynamic=createDynamicActor(pose, geometry, defaultPhysicsMaterial, &Material::Ruby);
	scene->addActor(*actorDynamic);
	PxFixedJoint* fixedJoint=PxFixedJointCreate(*physics, girderActor, PxTransform(girderFrame), actorDynamic, PxTransform(boxFrame));
	joints.push_back(fixedJoint);

	//staw spefyryczny
	pose.p+=offsetRight;
	pose.q*=PxQuat(PxHalfPi, PxVec3(0,0,1));
	actorDynamic=createDynamicActor(pose,PxCapsuleGeometry(2.0f,2.0f),defaultPhysicsMaterial,&Material::Ruby);
	actorDynamic->setLinearDamping(0.2f);
	actorDynamic->setAngularDamping(0.2f);
	scene->addActor(*actorDynamic);

	girderFrame+=offsetRight;
	PxSphericalJoint* sphericalJoint=PxSphericalJointCreate(
		*physics, 
		girderActor, PxTransform(girderFrame, PxQuat(PxHalfPi, PxVec3(0,0,-1))), 
		actorDynamic, PxTransform(PxVec3(4.0f,0,0), PxQuat(PxPi, PxVec3(0,0,-1))));
	sphericalJoint->setLimitCone(PxJointLimitCone(PxPi/6.0f, PxPi/6.0f, 0.01f));
	sphericalJoint->setSphericalJointFlag(PxSphericalJointFlag::eLIMIT_ENABLED, true);
	joints.push_back(sphericalJoint);

	//zawias
	pose.p+=offsetRight;
	actorDynamic=createDynamicActor(pose, geometry, defaultPhysicsMaterial, &Material::Ruby);
	actorDynamic->setLinearDamping(0.2f);
	actorDynamic->setAngularDamping(0.2f);
	scene->addActor(*actorDynamic);

	girderFrame+=offsetRight;
	PxRevoluteJoint* revoluteJoint=PxRevoluteJointCreate(
		*physics,
		girderActor, PxTransform(girderFrame), 
		actorDynamic, PxTransform(boxFrame, PxQuat(PxHalfPi/2, PxVec3(0,-1,0))));
	revoluteJoint->setLimit(PxJointLimitPair(0,PxHalfPi,0.01f));
	revoluteJoint->setRevoluteJointFlag(PxRevoluteJointFlag::eLIMIT_ENABLED,true);	
	joints.push_back(revoluteJoint);

	//pryzmatyczny
	pose.p+=offsetRight;
	actorDynamic=createDynamicActor(pose, geometry, defaultPhysicsMaterial, &Material::Ruby);
	actorDynamic->setLinearDamping(0.2f);
	actorDynamic->setAngularDamping(0.2f);
	scene->addActor(*actorDynamic);

	girderFrame+=offsetRight;
	PxPrismaticJoint* prismaticJoint = PxPrismaticJointCreate(
		*physics,
		girderActor, PxTransform(girderFrame), 
		actorDynamic, PxTransform(boxFrame));
	prismaticJoint->setLimit(PxJointLimitPair(-3.0f,3.0f,0.01f));
	prismaticJoint->setPrismaticJointFlag(PxPrismaticJointFlag::eLIMIT_ENABLED,true);
	joints.push_back(prismaticJoint);

	//distance joint
	pose.p+=offsetRight;
	actorDynamic=createDynamicActor(pose, geometry, defaultPhysicsMaterial, &Material::Ruby);
	actorDynamic->setLinearDamping(0.2f);
	actorDynamic->setAngularDamping(0.2f);
	scene->addActor(*actorDynamic);

	girderFrame+=offsetRight;
	PxDistanceJoint* distanceJoint=PxDistanceJointCreate(
		*physics,
		girderActor, PxTransform(girderFrame), 
		actorDynamic, PxTransform(boxFrame));
	distanceJoint->setMaxDistance(10.0f);
	distanceJoint->setDistanceJointFlag(PxDistanceJointFlag::eMAX_DISTANCE_ENABLED,true);

	/*
	distanceJoint->setSpring(100.0f);
	distanceJoint->setDamping(20.0f);
	distanceJoint->setDistanceJointFlag(PxDistanceJointFlag::eSPRING_ENABLED,true);
	*/

	joints.push_back(distanceJoint);

	//mosty niezrywalne
	this->createBridge(PxTransform(PxVec3(-160,30,30)), true);
	this->createBridge(PxTransform(PxVec3(-140,30,30)));

	//most zrywalny
	this->createBridge(PxTransform(PxVec3(100,30,30)), true, true);
	this->createBridge(PxTransform(PxVec3(120,30,30)), false, true);

	// sciana
	this->createBreakableWall(PxTransform(PxVec3(0, 0, -100)));

	// lina
	this->createRope(PxTransform(PxVec3(80, 40, 30)));

	// kamera
	camera->lookAt(PxVec3(0, 20, 105), PxVec3(0, 15, 0));
}

void SampleJointsGame::release()
{
	size_t nbJoints = joints.size();
	for (size_t i = 0 ; i < nbJoints ; ++i)
	{
		joints[i]->release();
	}
	joints.clear();

	SampleGame::release();
}

void SampleJointsGame::input(GxF32 elapsedTime)
{
	SampleGame::input(elapsedTime);

	if ( getKeyboard().isKeyPressed(Keys::Space) )
	{
		PxVec3 velocity = camera->getViewDir();
		velocity *= 70.0f;

		if ( (rand() % 2) == 0 )
			this->throwBox(camera->getPosition(), 2.0f, velocity, defaultPhysicsMaterial, 0, 2.0f);
		else
			this->throwBall(camera->getPosition(), 1.5f, velocity, defaultPhysicsMaterial);
	}

	if ( getKeyboard().isKeyPressed(Keys::Z) )
	{
		PxVec3 position;
		if ( raycastFromCamera(position, planeActor) )
		{
			this->addStackedBoxesAt(position, 8, defaultPhysicsMaterial, &Material::Obsidian);
		}
	}

	if ( getKeyboard().isKeyPressed(Keys::X) )
	{
		PxVec3 position;
		if ( raycastFromCamera(position, planeActor) )
		{
			this->addWallBoxesAt(position, 5, 8, defaultPhysicsMaterial, &Material::Turquoise);
		}
	}

	if ( getKeyboard().isKeyPressed(Keys::C) )
	{
		PxVec3 position;
		if ( raycastFromCamera(position, planeActor) )
		{
			position.y += 10.0f;
			this->addCapsuleAt(position, 2, 1, defaultPhysicsMaterial, &Material::Turquoise);
		}
	}

	if ( getKeyboard().isKeyPressed(Keys::V) )
	{
		PxVec3 position;
		if ( raycastFromCamera(position, planeActor) )
		{
			position.y += 10.0f;
			this->addSphereAt(position, 2, defaultPhysicsMaterial, &Material::PolishedGold);
		}
	}

	const GxF32 fps = getStepper().getFps();
	std::stringstream stream;
	stream << "Sample Joints | Fps: " << std::fixed << std::setprecision(1) << fps 
		<< " | Dynamic actors: " << scene->getNbActors(PxActorTypeSelectionFlag::eRIGID_DYNAMIC)
		<< " | Static actors: " << scene->getNbActors(PxActorTypeSelectionFlag::eRIGID_STATIC)
		<< " | Joints: " << joints.size()
		<< " | Camera position: " << camera->getPosition().x << " ; " << camera->getPosition().y << " ; " << camera->getPosition().z;

	this->setTitle(stream.str());
}

void SampleJointsGame::update(GxF32 elapsedTime)
{
	SampleGame::update(elapsedTime);

	// usuwamy zniszczone zlacza
	std::vector<PxJoint*> temp(joints);
	joints.clear();
	size_t nbJoints = temp.size();
	for (size_t i = 0 ; i < nbJoints ; ++i)
	{
		if ( temp[i]->getConstraintFlags() & PxConstraintFlag::eBROKEN )
		{
			temp[i]->release();
		}
		else
		{
			joints.push_back(temp[i]);
		}
	}
}

void SampleJointsGame::render(GxF32 elapsedTime)
{
	SampleGame::render(elapsedTime);

	graphics->clear(Color::CornflowerBlue);

	effect->bind();
	effect->setProjMatrix(projectionMtx);
	effect->setViewMatrix(camera->getViewMatrix());

	this->renderScene(scene);

	effect->unbind();
}

void SampleJointsGame::printHelp()
{
	SampleGame::printHelp();

	std::cout << std::endl << "# Dodawanie ksztaltow" << std::endl
		<< "Spacja - rzucenie pudelkiem lub sfera w kierunku patrzenia" << std::endl
		<< "Z - dodanie stosu pudelek" << std::endl
		<< "X - dodanie sciany zbudowanej z pudelek" << std::endl
		<< "C - dodanie kapsuly" << std::endl
		<< "V - dodanie sfery" << std::endl;
}

void SampleJointsGame::createBreakableWall(const PxTransform& transform)
{
	const PxReal brickHalfWidth = 3.0f;
	const PxReal brickHalfHeight = 1.0f;
	const PxReal brickHalfDepth = 1.5f;
	const PxReal brickBreakForce = 10000.0f;

	const PxU32 countOfBricksX = 10;
	const PxU32 countOfBricksY = 10;

	const PxVec3 brickDims = PxVec3(brickHalfWidth, brickHalfHeight, brickHalfDepth);

	PxTransform pose = PxTransform::createIdentity();
	pose.p.x -= (countOfBricksX - 1) * brickHalfWidth;
	pose.p.y += brickHalfHeight;

	std::vector<PxRigidDynamic*> bricks;

	for (PxU32 i = 0 ; i < countOfBricksY ; ++i) // numeruje Y
	{
		for (PxU32 j = 0 ; j < countOfBricksX ; ++j)
		{
			PxRigidDynamic* brick = this->createDynamicActor(transform * pose, PxBoxGeometry(brickDims), defaultPhysicsMaterial, &brickGraphicsMaterial, 0.5f);
			brick->setSolverIterationCounts(8, 8);

			bricks.push_back(brick);

			pose.p.x += 2 * brickHalfWidth;
		}
		pose.p.y += 2 * brickHalfHeight;
		pose.p.x -= 2 * countOfBricksX * brickHalfWidth;
	}

	for (PxU32 i = 0 ; i < countOfBricksY ; ++i)
	{
		for (PxU32 j = 0 ; j < countOfBricksX ; ++j)
		{
			const PxU32 k = i * countOfBricksX + j;
			const PxU32 p = (i + 1) * countOfBricksX + j;

			if ( j < countOfBricksX - 1 )
			{
				PxFixedJoint* jointRight = PxFixedJointCreate(*physics, bricks[k], PxTransform(PxVec3(brickHalfWidth, 0, 0)),
					bricks[k+1], PxTransform(PxVec3(-brickHalfWidth, 0, 0)));

				jointRight->setBreakForce(brickBreakForce, brickBreakForce);
				jointRight->setProjectionLinearTolerance(0.5f);
				joints.push_back(jointRight);
			}

			if ( i < countOfBricksY - 1 )
			{
				PxFixedJoint* jointUp = PxFixedJointCreate(*physics, bricks[k], PxTransform(PxVec3(0, brickHalfHeight, 0)),
					bricks[p], PxTransform(PxVec3(0, -brickHalfHeight, 0)));

				jointUp->setBreakForce(brickBreakForce, brickBreakForce);
				jointUp->setProjectionLinearTolerance(0.5f);
				joints.push_back(jointUp);
			}

			scene->addActor(*bricks[k]);
		}
	}
}

void SampleJointsGame::createRope(const PxTransform& transform)
{
	const PxReal scale = 5.0f;
	const PxReal capsuleRadius = 0.1f;
	const PxReal capsuleHalf = 0.4f;

	const PxReal segmentLength = 2 * (capsuleRadius + capsuleHalf);
	const PxReal segmentHalfLen = capsuleRadius + capsuleHalf;

	PxRigidDynamic* lastSegment = 0;
	for (PxReal z = 10.0f ; z > -10.0f ; z -= segmentLength)
	{
		const PxVec3 pos = PxVec3(0, 0, z - (segmentHalfLen)) * scale;

		PxTransform pose = transform * PxTransform(pos, PxQuat(PxHalfPi, PxVec3(0, 1, 0)));
		PxCapsuleGeometry geometry(capsuleRadius * scale, capsuleHalf * scale);

		PxRigidDynamic* segment = createDynamicActor(pose, geometry, defaultPhysicsMaterial, &Material::Emerald, 0.05f);
		segment->setLinearDamping(1.0f);
		segment->setAngularDamping(1.0f);
		segment->setSolverIterationCounts(8, 8);

		if (!lastSegment)
		{
			// first
			segment->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);
		}
		else
		{
			PxSphericalJoint* j = PxSphericalJointCreate(*physics, lastSegment, PxTransform(PxVec3(segmentHalfLen * scale, 0, 0)),
				segment, PxTransform(PxVec3(-segmentHalfLen * scale, 0, 0)));

			if ( j )
			{
				j->setProjectionLinearTolerance(0.02f);
				j->setConstraintFlag(PxConstraintFlag::ePROJECTION, true);
				joints.push_back(j);
			}
		}

		scene->addActor(*segment);
		lastSegment = segment;
	}
}

void SampleJointsGame::createBridge(const PxTransform& transform, bool fixedJoint, bool breakable)
{
	const PxReal scale = 5;
	const PxReal plankHalfWidth=1.0f;
	const PxReal plankHalfHeight=0.02f;
	const PxReal plankHalfDepth=0.5f;
	const PxReal plankDensity=1.0f;

	const PxReal boxHalfWidth = 1.0f;
	const PxReal boxHalfHeight = 1.0f;
	const PxReal boxHalfDepth = 1.0f;

	std::vector<PxJoint*> localJoints;

	const PxVec3 boxSize = PxVec3(boxHalfWidth, boxHalfHeight, boxHalfDepth)*scale;
	PxTransform pose = 
		transform*
		PxTransform(
			PxVec3(0,-boxHalfHeight, 10+boxHalfHeight)*scale,
			PxQuat::createIdentity());
	PxRigidStatic* rigidStatic=createStaticActor(pose, PxBoxGeometry(boxSize), defaultPhysicsMaterial);
	scene->addActor(*rigidStatic);

	PxRigidDynamic* previousPlank = 0;
	PxU32 i = 0;
	for(PxReal z = 10.0f - plankHalfDepth; z>-10.0f; z-=plankHalfDepth*2)
	{
		const PxVec3 plankSize=PxVec3(plankHalfWidth, plankHalfHeight, plankHalfDepth)*scale;
		pose=transform*PxTransform(PxVec3(0,-plankHalfHeight,z)*scale);

		Material* graphicsMaterial = ((i+1)%2==0)? &Material::BlackPlastic : &Material::Emerald;

		PxRigidDynamic* plank = createDynamicActor(pose, PxBoxGeometry(plankSize), defaultPhysicsMaterial, graphicsMaterial, plankDensity);
		if(plank)
		{
			plank->setSolverIterationCounts(8,8);
			plank->setLinearDamping(1.0f);
			plank->setAngularDamping(1.0f);
			if(!previousPlank)
			{
				plank->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);
			}
			else
			{
				if(fixedJoint)
				{
					PxFixedJoint* j=PxFixedJointCreate(
						*physics,
						previousPlank, PxTransform(PxVec3(0,0,-plankHalfDepth)*scale),
						plank, PxTransform(PxVec3(0,0,plankHalfDepth)*scale));
					if(j)
					{
						localJoints.push_back(j);
					}
				}
				else
				{
					PxRevoluteJoint* j = PxRevoluteJointCreate(
						*physics,
						previousPlank, PxTransform(PxVec3(0,0,-plankHalfDepth)*scale),
						plank, PxTransform(PxVec3(0,0,plankHalfDepth)*scale));
					if(j)
					{
						j->setProjectionLinearTolerance(0.5f);
						localJoints.push_back(j);
					}
				}
			}

			scene->addActor(*plank);
			previousPlank=plank;
			i++;
		}
	}
	
	if(previousPlank)
	{
		previousPlank->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);
	}

	if(breakable)
	{
		size_t nbJoints = localJoints.size();
		if(nbJoints>0)
		{
			const PxReal breakForce = 10000.0f;
			for(size_t i=0;i<nbJoints;++i)
			{
				localJoints[i]->setBreakForce(breakForce*scale, breakForce*scale);
			}
		}
	}

	pose = transform * PxTransform(
		PxVec3(0,-boxHalfHeight, -10-boxHalfHeight)*scale,
		PxQuat::createIdentity());
	rigidStatic=createStaticActor(pose,PxBoxGeometry(boxSize), defaultPhysicsMaterial);
	scene->addActor(*rigidStatic);

	joints.insert(joints.cend(), localJoints.begin(), localJoints.end());
}