// karo.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

//#define GLEW_STATIC
#include <GL/glew.h>
#include <GLFW/glfw3.h>

#include <iostream>
#include <vector>

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <PxPhysics.h>
//#include <physx/PxFoundation.h>
#include <PxConfig.h>
#include <PxPhysicsAPI.h> 

#include <vehicle/PxVehicleSDK.h>
//#include <physx/PxFiltering.h>

#include <imgui.h>
#include <imgui_impl_glfw.h>
#include <imgui_impl_opengl3.h>

#include "camera.hpp"

using namespace physx;

struct Vertex {
	glm::vec3 pos;
	glm::vec3 color;
};


static const char* s_vs_src = R"GLSL(
#version 330 core

layout (location = 0) in vec3 a_pos;
layout (location = 1) in vec3 a_color;

out vec3 u_color;

uniform mat4 u_mvp;

void main() {
	gl_Position = u_mvp * vec4(a_pos, 1.0);
	u_color = a_color;
}

)GLSL";

static const char* s_fs_src = R"GLSL(
#version 330 core

in vec3 u_color;

out vec4 o_color;

void main() {
	o_color = vec4(u_color, 1.0);
}
)GLSL";

static void CheckShaderCompileErrors(GLuint shader) {
	GLint success;
	GLchar infoLog[512];
	glGetShaderiv(shader, GL_COMPILE_STATUS, &success);
	if (!success) {
		glGetShaderInfoLog(shader, 512, NULL, infoLog);
		std::cerr << "Shader compilation failed: " << infoLog << std::endl;
		exit(1);
	}
}

static void CheckProgramLinkErrors(GLuint program) {
	GLint success;
	GLchar infoLog[512];
	glGetProgramiv(program, GL_LINK_STATUS, &success);
	if (!success) {
		glGetProgramInfoLog(program, 512, NULL, infoLog);
		std::cerr << "Program link failed: " << infoLog << std::endl;
		exit(1);
	}
}

enum ContactGroup {
	CG_NONE		= 0,
	CG_BOX		= (1 << 1),
	CG_FLOOR	= (1 << 2),
};

//PxFilterFlags FilterShaderExample(
//	PxFilterObjectAttributes attributes0, PxFilterData filterData0,
//	PxFilterObjectAttributes attributes1, PxFilterData filterData1,
//	PxPairFlags& pairFlags, const void* constantBlock, PxU32 constantBlockSize)
//{
//	// let triggers through
//	if (PxFilterObjectIsTrigger(attributes0) || PxFilterObjectIsTrigger(attributes1))
//	{
//		pairFlags = PxPairFlag::eTRIGGER_DEFAULT;
//		return PxFilterFlag::eDEFAULT;
//	}
//	// generate contacts for all that were not filtered above
//	pairFlags = PxPairFlag::eCONTACT_DEFAULT;
//
//	// trigger the contact callback for pairs (A,B) where
//	// the filtermask of A contains the ID of B and vice versa.
//	if ((filterData0.word0 & filterData1.word1) && (filterData1.word0 & filterData0.word1)) {
//		pairFlags |= PxPairFlag::eNOTIFY_TOUCH_FOUND;
//		return PxFilterFlag::eDEFAULT;
//
//	}
//
//	return PxFilterFlag::eSUPPRESS;
//}

//void SetupFiltering(PxShape* shape, PxU32 filterGroup, PxU32 filterMask, PxU32 w3)
//{
//	PxFilterData filterData;
//	filterData.word0 = filterGroup; // word0 = own ID
//	filterData.word1 = filterMask;  // word1 = ID mask to filter pairs that trigger a contact callback
//	filterData.word3 = w3;
//	shape->setSimulationFilterData(filterData);
//}



//Data structure for quick setup of scene queries for suspension queries.
class VehicleSceneQueryData
{
public:
	VehicleSceneQueryData();
	~VehicleSceneQueryData();

	//Allocate scene query data for up to maxNumVehicles and up to maxNumWheelsPerVehicle with numVehiclesInBatch per batch query.
	static VehicleSceneQueryData* allocate
	(const PxU32 maxNumVehicles, const PxU32 maxNumWheelsPerVehicle, const PxU32 maxNumHitPointsPerWheel, const PxU32 numVehiclesInBatch,
		PxBatchQueryPreFilterShader preFilterShader, PxBatchQueryPostFilterShader postFilterShader,
		PxAllocatorCallback& allocator);

	//Free allocated buffers.
	void free(PxAllocatorCallback& allocator);

	//Create a PxBatchQuery instance that will be used for a single specified batch.
	static PxBatchQuery* setUpBatchedSceneQuery(const PxU32 batchId, const VehicleSceneQueryData& vehicleSceneQueryData, PxScene* scene);

	//Return an array of scene query results for a single specified batch.
	PxRaycastQueryResult* getRaycastQueryResultBuffer(const PxU32 batchId);

	//Return an array of scene query results for a single specified batch.
	PxSweepQueryResult* getSweepQueryResultBuffer(const PxU32 batchId);

	//Get the number of scene query results that have been allocated for a single batch.
	PxU32 getQueryResultBufferSize() const;

private:

	//Number of queries per batch
	PxU32 mNumQueriesPerBatch;

	//Number of hit results per query
	PxU32 mNumHitResultsPerQuery;

	//One result for each wheel.
	PxRaycastQueryResult* mRaycastResults;
	PxSweepQueryResult* mSweepResults;

	//One hit for each wheel.
	PxRaycastHit* mRaycastHitBuffer;
	PxSweepHit* mSweepHitBuffer;

	//Filter shader used to filter drivable and non-drivable surfaces
	PxBatchQueryPreFilterShader mPreFilterShader;

	//Filter shader used to reject hit shapes that initially overlap sweeps.
	PxBatchQueryPostFilterShader mPostFilterShader;

};

VehicleSceneQueryData::VehicleSceneQueryData()
	: mNumQueriesPerBatch(0),
	mNumHitResultsPerQuery(0),
	mRaycastResults(NULL),
	mRaycastHitBuffer(NULL),
	mPreFilterShader(NULL),
	mPostFilterShader(NULL)
{
}

VehicleSceneQueryData::~VehicleSceneQueryData()
{
}

VehicleSceneQueryData* VehicleSceneQueryData::allocate
(const PxU32 maxNumVehicles, const PxU32 maxNumWheelsPerVehicle, const PxU32 maxNumHitPointsPerWheel, const PxU32 numVehiclesInBatch,
	PxBatchQueryPreFilterShader preFilterShader, PxBatchQueryPostFilterShader postFilterShader,
	PxAllocatorCallback& allocator)
{
	const PxU32 sqDataSize = ((sizeof(VehicleSceneQueryData) + 15) & ~15);

	const PxU32 maxNumWheels = maxNumVehicles * maxNumWheelsPerVehicle;
	const PxU32 raycastResultSize = ((sizeof(PxRaycastQueryResult) * maxNumWheels + 15) & ~15);
	const PxU32 sweepResultSize = ((sizeof(PxSweepQueryResult) * maxNumWheels + 15) & ~15);

	const PxU32 maxNumHitPoints = maxNumWheels * maxNumHitPointsPerWheel;
	const PxU32 raycastHitSize = ((sizeof(PxRaycastHit) * maxNumHitPoints + 15) & ~15);
	const PxU32 sweepHitSize = ((sizeof(PxSweepHit) * maxNumHitPoints + 15) & ~15);

	const PxU32 size = sqDataSize + raycastResultSize + raycastHitSize + sweepResultSize + sweepHitSize;
	PxU8* buffer = static_cast<PxU8*>(allocator.allocate(size, NULL, NULL, 0));

	VehicleSceneQueryData* sqData = new(buffer) VehicleSceneQueryData();
	sqData->mNumQueriesPerBatch = numVehiclesInBatch * maxNumWheelsPerVehicle;
	sqData->mNumHitResultsPerQuery = maxNumHitPointsPerWheel;
	buffer += sqDataSize;

	sqData->mRaycastResults = reinterpret_cast<PxRaycastQueryResult*>(buffer);
	buffer += raycastResultSize;

	sqData->mRaycastHitBuffer = reinterpret_cast<PxRaycastHit*>(buffer);
	buffer += raycastHitSize;

	sqData->mSweepResults = reinterpret_cast<PxSweepQueryResult*>(buffer);
	buffer += sweepResultSize;

	sqData->mSweepHitBuffer = reinterpret_cast<PxSweepHit*>(buffer);
	buffer += sweepHitSize;

	for (PxU32 i = 0; i < maxNumWheels; i++)
	{
		new(sqData->mRaycastResults + i) PxRaycastQueryResult();
		new(sqData->mSweepResults + i) PxSweepQueryResult();
	}

	for (PxU32 i = 0; i < maxNumHitPoints; i++)
	{
		new(sqData->mRaycastHitBuffer + i) PxRaycastHit();
		new(sqData->mSweepHitBuffer + i) PxSweepHit();
	}

	sqData->mPreFilterShader = preFilterShader;
	sqData->mPostFilterShader = postFilterShader;

	return sqData;
}

void VehicleSceneQueryData::free(PxAllocatorCallback& allocator)
{
	allocator.deallocate(this);
}

PxBatchQuery* VehicleSceneQueryData::setUpBatchedSceneQuery(const PxU32 batchId, const VehicleSceneQueryData& vehicleSceneQueryData, PxScene* scene)
{
	const PxU32 maxNumQueriesInBatch = vehicleSceneQueryData.mNumQueriesPerBatch;
	const PxU32 maxNumHitResultsInBatch = vehicleSceneQueryData.mNumQueriesPerBatch * vehicleSceneQueryData.mNumHitResultsPerQuery;

	PxBatchQueryDesc sqDesc(maxNumQueriesInBatch, maxNumQueriesInBatch, 0);

	sqDesc.queryMemory.userRaycastResultBuffer = vehicleSceneQueryData.mRaycastResults + batchId * maxNumQueriesInBatch;
	sqDesc.queryMemory.userRaycastTouchBuffer = vehicleSceneQueryData.mRaycastHitBuffer + batchId * maxNumHitResultsInBatch;
	sqDesc.queryMemory.raycastTouchBufferSize = maxNumHitResultsInBatch;

	sqDesc.queryMemory.userSweepResultBuffer = vehicleSceneQueryData.mSweepResults + batchId * maxNumQueriesInBatch;
	sqDesc.queryMemory.userSweepTouchBuffer = vehicleSceneQueryData.mSweepHitBuffer + batchId * maxNumHitResultsInBatch;
	sqDesc.queryMemory.sweepTouchBufferSize = maxNumHitResultsInBatch;

	sqDesc.preFilterShader = vehicleSceneQueryData.mPreFilterShader;

	sqDesc.postFilterShader = vehicleSceneQueryData.mPostFilterShader;

	return scene->createBatchQuery(sqDesc);
}

PxRaycastQueryResult* VehicleSceneQueryData::getRaycastQueryResultBuffer(const PxU32 batchId)
{
	return (mRaycastResults + batchId * mNumQueriesPerBatch);
}

PxSweepQueryResult* VehicleSceneQueryData::getSweepQueryResultBuffer(const PxU32 batchId)
{
	return (mSweepResults + batchId * mNumQueriesPerBatch);
}


PxU32 VehicleSceneQueryData::getQueryResultBufferSize() const
{
	return mNumQueriesPerBatch;
}


static std::vector<Vertex> s_draw_lines;
static std::vector<Vertex> s_draw_points;

static PxDefaultErrorCallback gDefaultErrorCallback;
static PxDefaultAllocator gDefaultAllocatorCallback;

static PxFoundation* s_foundation;
static PxPhysics* s_physics;
static PxCooking* s_cooking;
static PxScene* s_scene;
static PxDefaultCpuDispatcher* s_cpu_dispatcher;

static PxMaterial* s_material;

static PxRigidDynamic* s_box1;

VehicleSceneQueryData* gVehicleSceneQueryData = NULL;
PxBatchQuery* gBatchQuery = NULL;

PxVehicleDrivableSurfaceToTireFrictionPairs* gFrictionPairs = NULL;

PxVehicleDrive4W* gVehicle4W = NULL;

enum
{
	DRIVABLE_SURFACE = 0xffff0000,
	UNDRIVABLE_SURFACE = 0x0000ffff
};

//EngineDriveVehicle gVehicle;
void setupDrivableSurface(PxFilterData& filterData)
{
	filterData.word3 = static_cast<PxU32>(DRIVABLE_SURFACE);
}

void setupNonDrivableSurface(PxFilterData& filterData)
{
	filterData.word3 = UNDRIVABLE_SURFACE;
}

PxQueryHitType::Enum WheelSceneQueryPreFilterBlocking
(PxFilterData filterData0, PxFilterData filterData1,
	const void* constantBlock, PxU32 constantBlockSize,
	PxHitFlags& queryFlags)
{
	//filterData0 is the vehicle suspension query.
	//filterData1 is the shape potentially hit by the query.
	PX_UNUSED(filterData0);
	PX_UNUSED(constantBlock);
	PX_UNUSED(constantBlockSize);
	PX_UNUSED(queryFlags);
	return ((0 == (filterData1.word3 & DRIVABLE_SURFACE)) ? PxQueryHitType::eNONE : PxQueryHitType::eBLOCK);
}

//Drivable surface types.
enum
{
	SURFACE_TYPE_TARMAC,
	MAX_NUM_SURFACE_TYPES
};

//Tire types.
enum
{
	TIRE_TYPE_NORMAL = 0,
	TIRE_TYPE_WORN,
	MAX_NUM_TIRE_TYPES
};


//Tire model friction for each combination of drivable surface type and tire type.
static PxF32 gTireFrictionMultipliers[MAX_NUM_SURFACE_TYPES][MAX_NUM_TIRE_TYPES] =
{
	//NORMAL,	WORN
	{30.0f,		0.1f}//TARMAC
};

PxVehicleDrivableSurfaceToTireFrictionPairs* createFrictionPairs(const PxMaterial* defaultMaterial)
{
	PxVehicleDrivableSurfaceType surfaceTypes[1];
	surfaceTypes[0].mType = SURFACE_TYPE_TARMAC;

	const PxMaterial* surfaceMaterials[1];
	surfaceMaterials[0] = defaultMaterial;

	PxVehicleDrivableSurfaceToTireFrictionPairs* surfaceTirePairs =
		PxVehicleDrivableSurfaceToTireFrictionPairs::allocate(MAX_NUM_TIRE_TYPES, MAX_NUM_SURFACE_TYPES);

	surfaceTirePairs->setup(MAX_NUM_TIRE_TYPES, MAX_NUM_SURFACE_TYPES, surfaceMaterials, surfaceTypes);

	for (PxU32 i = 0; i < MAX_NUM_SURFACE_TYPES; i++)
	{
		for (PxU32 j = 0; j < MAX_NUM_TIRE_TYPES; j++)
		{
			surfaceTirePairs->setTypePairFriction(i, j, gTireFrictionMultipliers[i][j]);
		}
	}
	return surfaceTirePairs;
}

enum
{
	COLLISION_FLAG_GROUND = 1 << 0,
	COLLISION_FLAG_WHEEL = 1 << 1,
	COLLISION_FLAG_CHASSIS = 1 << 2,
	COLLISION_FLAG_OBSTACLE = 1 << 3,
	COLLISION_FLAG_DRIVABLE_OBSTACLE = 1 << 4,

	COLLISION_FLAG_GROUND_AGAINST = COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE | COLLISION_FLAG_DRIVABLE_OBSTACLE,
	COLLISION_FLAG_WHEEL_AGAINST = COLLISION_FLAG_WHEEL | COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE,
	COLLISION_FLAG_CHASSIS_AGAINST = COLLISION_FLAG_GROUND | COLLISION_FLAG_WHEEL | COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE | COLLISION_FLAG_DRIVABLE_OBSTACLE,
	COLLISION_FLAG_OBSTACLE_AGAINST = COLLISION_FLAG_GROUND | COLLISION_FLAG_WHEEL | COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE | COLLISION_FLAG_DRIVABLE_OBSTACLE,
	COLLISION_FLAG_DRIVABLE_OBSTACLE_AGAINST = COLLISION_FLAG_GROUND | COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE | COLLISION_FLAG_DRIVABLE_OBSTACLE
};


PxRigidStatic* createDrivablePlane(const PxFilterData& simFilterData, PxMaterial* material, PxPhysics* physics)
{
	//Add a plane to the scene.
	PxRigidStatic* groundPlane = PxCreatePlane(*physics, PxPlane(0, 1, 0, 0), *material);

	//Get the plane shape so we can set query and simulation filter data.
	PxShape* shapes[1];
	groundPlane->getShapes(shapes, 1);

	//Set the query filter data of the ground plane so that the vehicle raycasts can hit the ground.
	PxFilterData qryFilterData;
	setupDrivableSurface(qryFilterData);
	shapes[0]->setQueryFilterData(qryFilterData);

	//Set the simulation filter data of the ground plane so that it collides with the chassis of a vehicle but not the wheels.
	shapes[0]->setSimulationFilterData(simFilterData);

	return groundPlane;
}

//struct ActorUserData
//{
//	ActorUserData()
//		: vehicle(NULL),
//		actor(NULL)
//	{
//	}
//
//	const PxVehicleWheels* vehicle;
//	const PxActor* actor;
//};
//
//struct ShapeUserData
//{
//	ShapeUserData()
//		: isWheel(false),
//		wheelId(0xffffffff)
//	{
//	}
//
//	bool isWheel;
//	PxU32 wheelId;
//};


struct VehicleDesc
{
	VehicleDesc()
		: chassisMass(0.0f),
		chassisDims(PxVec3(0.0f, 0.0f, 0.0f)),
		chassisMOI(PxVec3(0.0f, 0.0f, 0.0f)),
		chassisCMOffset(PxVec3(0.0f, 0.0f, 0.0f)),
		chassisMaterial(NULL),
		wheelMass(0.0f),
		wheelWidth(0.0f),
		wheelRadius(0.0f),
		wheelMOI(0.0f),
		wheelMaterial(NULL)
		//actorUserData(NULL),
		//shapeUserDatas(NULL)
	{
	}

	PxF32 chassisMass;
	PxVec3 chassisDims;
	PxVec3 chassisMOI;
	PxVec3 chassisCMOffset;
	PxMaterial* chassisMaterial;
	PxFilterData chassisSimFilterData;  //word0 = collide type, word1 = collide against types, word2 = PxPairFlags

	PxF32 wheelMass;
	PxF32 wheelWidth;
	PxF32 wheelRadius;
	PxF32 wheelMOI;
	PxMaterial* wheelMaterial;
	PxU32 numWheels;
	PxFilterData wheelSimFilterData;	//word0 = collide type, word1 = collide against types, word2 = PxPairFlags

	//ActorUserData* actorUserData;
	//ShapeUserData* shapeUserDatas;
};


VehicleDesc initVehicleDesc()
{
	//Set up the chassis mass, dimensions, moment of inertia, and center of mass offset.
	//The moment of inertia is just the moment of inertia of a cuboid but modified for easier steering.
	//Center of mass offset is 0.65m above the base of the chassis and 0.25m towards the front.
	const PxF32 chassisMass = 1500.0f;
	const PxVec3 chassisDims(2.5f, 2.0f, 5.0f);
	const PxVec3 chassisMOI
	((chassisDims.y * chassisDims.y + chassisDims.z * chassisDims.z) * chassisMass / 12.0f,
		(chassisDims.x * chassisDims.x + chassisDims.z * chassisDims.z) * 0.8f * chassisMass / 12.0f,
		(chassisDims.x * chassisDims.x + chassisDims.y * chassisDims.y) * chassisMass / 12.0f);
	const PxVec3 chassisCMOffset(0.0f, -chassisDims.y * 0.5f + 0.65f, 0.25f);

	//Set up the wheel mass, radius, width, moment of inertia, and number of wheels.
	//Moment of inertia is just the moment of inertia of a cylinder.
	const PxF32 wheelMass = 20.0f;
	const PxF32 wheelRadius = 0.5f;
	const PxF32 wheelWidth = 0.4f;
	const PxF32 wheelMOI = 0.5f * wheelMass * wheelRadius * wheelRadius;
	const PxU32 nbWheels = 4;

	VehicleDesc vehicleDesc;

	vehicleDesc.chassisMass = chassisMass;
	vehicleDesc.chassisDims = chassisDims;
	vehicleDesc.chassisMOI = chassisMOI;
	vehicleDesc.chassisCMOffset = chassisCMOffset;
	vehicleDesc.chassisMaterial = s_material;
	vehicleDesc.chassisSimFilterData = PxFilterData(COLLISION_FLAG_CHASSIS, COLLISION_FLAG_CHASSIS_AGAINST, 0, 0);

	vehicleDesc.wheelMass = wheelMass;
	vehicleDesc.wheelRadius = wheelRadius;
	vehicleDesc.wheelWidth = wheelWidth;
	vehicleDesc.wheelMOI = wheelMOI;
	vehicleDesc.numWheels = nbWheels;
	vehicleDesc.wheelMaterial = s_material;
	vehicleDesc.wheelSimFilterData = PxFilterData(COLLISION_FLAG_WHEEL, COLLISION_FLAG_WHEEL_AGAINST, 0, 0);

	return vehicleDesc;
}

static PxConvexMesh* createConvexMesh(const PxVec3* verts, const PxU32 numVerts, PxPhysics& physics, PxCooking& cooking)
{
	// Create descriptor for convex mesh
	PxConvexMeshDesc convexDesc;
	convexDesc.points.count = numVerts;
	convexDesc.points.stride = sizeof(PxVec3);
	convexDesc.points.data = verts;
	convexDesc.flags = PxConvexFlag::eCOMPUTE_CONVEX;

	PxConvexMesh* convexMesh = NULL;
	PxDefaultMemoryOutputStream buf;
	if (cooking.cookConvexMesh(convexDesc, buf))
	{
		PxDefaultMemoryInputData id(buf.getData(), buf.getSize());
		convexMesh = physics.createConvexMesh(id);
	}

	return convexMesh;
}

PxConvexMesh* createWheelMesh(const PxF32 width, const PxF32 radius, PxPhysics& physics, PxCooking& cooking)
{
	PxVec3 points[2 * 16];
	for (PxU32 i = 0; i < 16; i++)
	{
		const PxF32 cosTheta = PxCos(i * PxPi * 2.0f / 16.0f);
		const PxF32 sinTheta = PxSin(i * PxPi * 2.0f / 16.0f);
		const PxF32 y = radius * cosTheta;
		const PxF32 z = radius * sinTheta;
		points[2 * i + 0] = PxVec3(-width / 2.0f, y, z);
		points[2 * i + 1] = PxVec3(+width / 2.0f, y, z);
	}

	return createConvexMesh(points, 32, physics, cooking);
}


PxConvexMesh* createChassisMesh(const PxVec3 dims, PxPhysics& physics, PxCooking& cooking)
{
	const PxF32 x = dims.x * 0.5f;
	const PxF32 y = dims.y * 0.5f;
	const PxF32 z = dims.z * 0.5f;
	PxVec3 verts[8] =
	{
		PxVec3(x,y,-z),
		PxVec3(x,y,z),
		PxVec3(x,-y,z),
		PxVec3(x,-y,-z),
		PxVec3(-x,y,-z),
		PxVec3(-x,y,z),
		PxVec3(-x,-y,z),
		PxVec3(-x,-y,-z)
	};

	return createConvexMesh(verts, 8, physics, cooking);
}


PxRigidDynamic* createVehicleActor
(const PxVehicleChassisData& chassisData,
	PxMaterial** wheelMaterials, PxConvexMesh** wheelConvexMeshes, const PxU32 numWheels, const PxFilterData& wheelSimFilterData,
	PxMaterial** chassisMaterials, PxConvexMesh** chassisConvexMeshes, const PxU32 numChassisMeshes, const PxFilterData& chassisSimFilterData,
	PxPhysics& physics)
{
	//We need a rigid body actor for the vehicle.
	//Don't forget to add the actor to the scene after setting up the associated vehicle.
	PxRigidDynamic* vehActor = physics.createRigidDynamic(PxTransform(PxIdentity));

	//Wheel and chassis query filter data.
	//Optional: cars don't drive on other cars.
	PxFilterData wheelQryFilterData;
	setupNonDrivableSurface(wheelQryFilterData);
	PxFilterData chassisQryFilterData;
	setupNonDrivableSurface(chassisQryFilterData);

	//Add all the wheel shapes to the actor.
	for (PxU32 i = 0; i < numWheels; i++)
	{
		PxConvexMeshGeometry geom(wheelConvexMeshes[i]);
		PxShape* wheelShape = PxRigidActorExt::createExclusiveShape(*vehActor, geom, *wheelMaterials[i]);
		wheelShape->setQueryFilterData(wheelQryFilterData);
		wheelShape->setSimulationFilterData(wheelSimFilterData);
		wheelShape->setLocalPose(PxTransform(PxIdentity));
	}

	//Add the chassis shapes to the actor.
	for (PxU32 i = 0; i < numChassisMeshes; i++)
	{
		PxShape* chassisShape = PxRigidActorExt::createExclusiveShape(*vehActor, PxConvexMeshGeometry(chassisConvexMeshes[i]), *chassisMaterials[i]);
		chassisShape->setQueryFilterData(chassisQryFilterData);
		chassisShape->setSimulationFilterData(chassisSimFilterData);
		chassisShape->setLocalPose(PxTransform(PxIdentity));
	}

	vehActor->setMass(chassisData.mMass);
	vehActor->setMassSpaceInertiaTensor(chassisData.mMOI);
	vehActor->setCMassLocalPose(PxTransform(chassisData.mCMOffset, PxQuat(PxIdentity)));

	return vehActor;
}


void computeWheelCenterActorOffsets4W(const PxF32 wheelFrontZ, const PxF32 wheelRearZ, const PxVec3& chassisDims, const PxF32 wheelWidth, const PxF32 wheelRadius, const PxU32 numWheels, PxVec3* wheelCentreOffsets)
{
	//chassisDims.z is the distance from the rear of the chassis to the front of the chassis.
	//The front has z = 0.5*chassisDims.z and the rear has z = -0.5*chassisDims.z.
	//Compute a position for the front wheel and the rear wheel along the z-axis.
	//Compute the separation between each wheel along the z-axis.
	const PxF32 numLeftWheels = numWheels / 2.0f;
	const PxF32 deltaZ = (wheelFrontZ - wheelRearZ) / (numLeftWheels - 1.0f);
	//Set the outside of the left and right wheels to be flush with the chassis.
	//Set the top of the wheel to be just touching the underside of the chassis.
	//Begin by setting the rear-left/rear-right/front-left,front-right wheels.
	wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eREAR_LEFT] = PxVec3((-chassisDims.x + wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + 0 * deltaZ * 0.5f);
	wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eREAR_RIGHT] = PxVec3((+chassisDims.x - wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + 0 * deltaZ * 0.5f);
	wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eFRONT_LEFT] = PxVec3((-chassisDims.x + wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + (numLeftWheels - 1) * deltaZ);
	wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT] = PxVec3((+chassisDims.x - wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + (numLeftWheels - 1) * deltaZ);
	//Set the remaining wheels.
	for (PxU32 i = 2, wheelCount = 4; i < numWheels - 2; i += 2, wheelCount += 2)
	{
		wheelCentreOffsets[wheelCount + 0] = PxVec3((-chassisDims.x + wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + i * deltaZ * 0.5f);
		wheelCentreOffsets[wheelCount + 1] = PxVec3((+chassisDims.x - wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + i * deltaZ * 0.5f);
	}
}

void setupWheelsSimulationData
(const PxF32 wheelMass, const PxF32 wheelMOI, const PxF32 wheelRadius, const PxF32 wheelWidth,
	const PxU32 numWheels, const PxVec3* wheelCenterActorOffsets,
	const PxVec3& chassisCMOffset, const PxF32 chassisMass,
	PxVehicleWheelsSimData* wheelsSimData)
{
	//Set up the wheels.
	PxVehicleWheelData wheels[PX_MAX_NB_WHEELS];
	{
		//Set up the wheel data structures with mass, moi, radius, width.
		for (PxU32 i = 0; i < numWheels; i++)
		{
			wheels[i].mMass = wheelMass;
			wheels[i].mMOI = wheelMOI;
			wheels[i].mRadius = wheelRadius;
			wheels[i].mWidth = wheelWidth;

			wheels[i].mMaxBrakeTorque = 2000.0f;
		}

		//Enable the handbrake for the rear wheels only.
		wheels[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mMaxHandBrakeTorque = 4000.0f;
		wheels[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mMaxHandBrakeTorque = 4000.0f;
		//Enable steering for the front wheels only.
		wheels[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mMaxSteer = PxPi * 0.3333f;
		wheels[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mMaxSteer = PxPi * 0.3333f;
	}

	//Set up the tires.
	PxVehicleTireData tires[PX_MAX_NB_WHEELS];
	{
		//Set up the tires.
		for (PxU32 i = 0; i < numWheels; i++)
		{
			tires[i].mType = TIRE_TYPE_NORMAL;
		}
	}

	//Set up the suspensions
	PxVehicleSuspensionData suspensions[PX_MAX_NB_WHEELS];
	{
		//Compute the mass supported by each suspension spring.
		PxF32 suspSprungMasses[PX_MAX_NB_WHEELS];
		PxVehicleComputeSprungMasses
		(numWheels, wheelCenterActorOffsets,
			chassisCMOffset, chassisMass, 1, suspSprungMasses);

		//Set the suspension data.
		for (PxU32 i = 0; i < numWheels; i++)
		{
			suspensions[i].mMaxCompression = 0.3f;
			suspensions[i].mMaxDroop = 0.1f;
			suspensions[i].mSpringStrength = 35000.0f;
			suspensions[i].mSpringDamperRate = 4500.0f;
			suspensions[i].mSprungMass = suspSprungMasses[i];
		}

		//Set the camber angles.
		const PxF32 camberAngleAtRest = 0.0;
		const PxF32 camberAngleAtMaxDroop = 0.01f;
		const PxF32 camberAngleAtMaxCompression = -0.01f;
		for (PxU32 i = 0; i < numWheels; i += 2)
		{
			suspensions[i + 0].mCamberAtRest = camberAngleAtRest;
			suspensions[i + 1].mCamberAtRest = -camberAngleAtRest;
			suspensions[i + 0].mCamberAtMaxDroop = camberAngleAtMaxDroop;
			suspensions[i + 1].mCamberAtMaxDroop = -camberAngleAtMaxDroop;
			suspensions[i + 0].mCamberAtMaxCompression = camberAngleAtMaxCompression;
			suspensions[i + 1].mCamberAtMaxCompression = -camberAngleAtMaxCompression;
		}
	}

	//Set up the wheel geometry.
	PxVec3 suspTravelDirections[PX_MAX_NB_WHEELS];
	PxVec3 wheelCentreCMOffsets[PX_MAX_NB_WHEELS];
	PxVec3 suspForceAppCMOffsets[PX_MAX_NB_WHEELS];
	PxVec3 tireForceAppCMOffsets[PX_MAX_NB_WHEELS];
	{
		//Set the geometry data.
		for (PxU32 i = 0; i < numWheels; i++)
		{
			//Vertical suspension travel.
			suspTravelDirections[i] = PxVec3(0, -1, 0);

			//Wheel center offset is offset from rigid body center of mass.
			wheelCentreCMOffsets[i] =
				wheelCenterActorOffsets[i] - chassisCMOffset;

			//Suspension force application point 0.3 metres below 
			//rigid body center of mass.
			suspForceAppCMOffsets[i] =
				PxVec3(wheelCentreCMOffsets[i].x, -0.3f, wheelCentreCMOffsets[i].z);

			//Tire force application point 0.3 metres below 
			//rigid body center of mass.
			tireForceAppCMOffsets[i] =
				PxVec3(wheelCentreCMOffsets[i].x, -0.3f, wheelCentreCMOffsets[i].z);
		}
	}

	//Set up the filter data of the raycast that will be issued by each suspension.
	PxFilterData qryFilterData;
	setupNonDrivableSurface(qryFilterData);

	//Set the wheel, tire and suspension data.
	//Set the geometry data.
	//Set the query filter data
	for (PxU32 i = 0; i < numWheels; i++)
	{
		wheelsSimData->setWheelData(i, wheels[i]);
		wheelsSimData->setTireData(i, tires[i]);
		wheelsSimData->setSuspensionData(i, suspensions[i]);
		wheelsSimData->setSuspTravelDirection(i, suspTravelDirections[i]);
		wheelsSimData->setWheelCentreOffset(i, wheelCentreCMOffsets[i]);
		wheelsSimData->setSuspForceAppPointOffset(i, suspForceAppCMOffsets[i]);
		wheelsSimData->setTireForceAppPointOffset(i, tireForceAppCMOffsets[i]);
		wheelsSimData->setSceneQueryFilterData(i, qryFilterData);
		wheelsSimData->setWheelShapeMapping(i, PxI32(i));
	}

	//Add a front and rear anti-roll bar
	PxVehicleAntiRollBarData barFront;
	barFront.mWheel0 = PxVehicleDrive4WWheelOrder::eFRONT_LEFT;
	barFront.mWheel1 = PxVehicleDrive4WWheelOrder::eFRONT_RIGHT;
	barFront.mStiffness = 10000.0f;
	wheelsSimData->addAntiRollBarData(barFront);
	PxVehicleAntiRollBarData barRear;
	barRear.mWheel0 = PxVehicleDrive4WWheelOrder::eREAR_LEFT;
	barRear.mWheel1 = PxVehicleDrive4WWheelOrder::eREAR_RIGHT;
	barRear.mStiffness = 10000.0f;
	wheelsSimData->addAntiRollBarData(barRear);
}

//void configureUserData(PxVehicleWheels* vehicle, ActorUserData* actorUserData, ShapeUserData* shapeUserDatas)
//{
//	if (actorUserData)
//	{
//		vehicle->getRigidDynamicActor()->userData = actorUserData;
//		actorUserData->vehicle = vehicle;
//	}
//
//	if (shapeUserDatas)
//	{
//		PxShape* shapes[PX_MAX_NB_WHEELS + 1];
//		vehicle->getRigidDynamicActor()->getShapes(shapes, PX_MAX_NB_WHEELS + 1);
//		for (PxU32 i = 0; i < vehicle->mWheelsSimData.getNbWheels(); i++)
//		{
//			const PxI32 shapeId = vehicle->mWheelsSimData.getWheelShapeMapping(i);
//			shapes[shapeId]->userData = &shapeUserDatas[i];
//			shapeUserDatas[i].isWheel = true;
//			shapeUserDatas[i].wheelId = i;
//		}
//	}
//}


PxVehicleDrive4W* createVehicle4W(const VehicleDesc& vehicle4WDesc, PxPhysics* physics, PxCooking* cooking)
{
	const PxVec3 chassisDims = vehicle4WDesc.chassisDims;
	const PxF32 wheelWidth = vehicle4WDesc.wheelWidth;
	const PxF32 wheelRadius = vehicle4WDesc.wheelRadius;
	const PxU32 numWheels = vehicle4WDesc.numWheels;

	const PxFilterData& chassisSimFilterData = vehicle4WDesc.chassisSimFilterData;
	const PxFilterData& wheelSimFilterData = vehicle4WDesc.wheelSimFilterData;

	//Construct a physx actor with shapes for the chassis and wheels.
	//Set the rigid body mass, moment of inertia, and center of mass offset.
	PxRigidDynamic* veh4WActor = NULL;
	{
		//Construct a convex mesh for a cylindrical wheel.
		PxConvexMesh* wheelMesh = createWheelMesh(wheelWidth, wheelRadius, *physics, *cooking);
		//Assume all wheels are identical for simplicity.
		PxConvexMesh* wheelConvexMeshes[PX_MAX_NB_WHEELS];
		PxMaterial* wheelMaterials[PX_MAX_NB_WHEELS];

		//Set the meshes and materials for the driven wheels.
		for (PxU32 i = PxVehicleDrive4WWheelOrder::eFRONT_LEFT; i <= PxVehicleDrive4WWheelOrder::eREAR_RIGHT; i++)
		{
			wheelConvexMeshes[i] = wheelMesh;
			wheelMaterials[i] = vehicle4WDesc.wheelMaterial;
		}
		//Set the meshes and materials for the non-driven wheels
		for (PxU32 i = PxVehicleDrive4WWheelOrder::eREAR_RIGHT + 1; i < numWheels; i++)
		{
			wheelConvexMeshes[i] = wheelMesh;
			wheelMaterials[i] = vehicle4WDesc.wheelMaterial;
		}

		//Chassis just has a single convex shape for simplicity.
		PxConvexMesh* chassisConvexMesh = createChassisMesh(chassisDims, *physics, *cooking);
		PxConvexMesh* chassisConvexMeshes[1] = { chassisConvexMesh };
		PxMaterial* chassisMaterials[1] = { vehicle4WDesc.chassisMaterial };

		//Rigid body data.
		PxVehicleChassisData rigidBodyData;
		rigidBodyData.mMOI = vehicle4WDesc.chassisMOI;
		rigidBodyData.mMass = vehicle4WDesc.chassisMass;
		rigidBodyData.mCMOffset = vehicle4WDesc.chassisCMOffset;

		veh4WActor = createVehicleActor
		(rigidBodyData,
			wheelMaterials, wheelConvexMeshes, numWheels, wheelSimFilterData,
			chassisMaterials, chassisConvexMeshes, 1, chassisSimFilterData,
			*physics);
	}

	//Set up the sim data for the wheels.
	PxVehicleWheelsSimData* wheelsSimData = PxVehicleWheelsSimData::allocate(numWheels);
	{
		//Compute the wheel center offsets from the origin.
		PxVec3 wheelCenterActorOffsets[PX_MAX_NB_WHEELS];
		const PxF32 frontZ = chassisDims.z * 0.3f;
		const PxF32 rearZ = -chassisDims.z * 0.3f;
		computeWheelCenterActorOffsets4W(frontZ, rearZ, chassisDims, wheelWidth, wheelRadius, numWheels, wheelCenterActorOffsets);

		//Set up the simulation data for all wheels.
		setupWheelsSimulationData
		(vehicle4WDesc.wheelMass, vehicle4WDesc.wheelMOI, wheelRadius, wheelWidth,
			numWheels, wheelCenterActorOffsets,
			vehicle4WDesc.chassisCMOffset, vehicle4WDesc.chassisMass,
			wheelsSimData);
	}

	//Set up the sim data for the vehicle drive model.
	PxVehicleDriveSimData4W driveSimData;
	{
		//Diff
		PxVehicleDifferential4WData diff;
		//diff.mType = PxVehicleDifferential4WData::eDIFF_TYPE_LS_4WD;
		diff.mType = PxVehicleDifferential4WData::eDIFF_TYPE_OPEN_FRONTWD;
		driveSimData.setDiffData(diff);

		//Engine
		PxVehicleEngineData engine;
		engine.mPeakTorque = 500.0f;
		engine.mMaxOmega = 600.0f;//approx 6000 rpm
		driveSimData.setEngineData(engine);

		//Gears
		PxVehicleGearsData gears;
		gears.mSwitchTime = 0.5f;
		driveSimData.setGearsData(gears);

		//Clutch
		PxVehicleClutchData clutch;
		clutch.mStrength = 10.0f;
		driveSimData.setClutchData(clutch);

		//Ackermann steer accuracy
		PxVehicleAckermannGeometryData ackermann;
		ackermann.mAccuracy = 1.0f;
		ackermann.mAxleSeparation =
			wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_LEFT).z -
			wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_LEFT).z;
		ackermann.mFrontWidth =
			wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_RIGHT).x -
			wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_LEFT).x;
		ackermann.mRearWidth =
			wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_RIGHT).x -
			wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_LEFT).x;
		driveSimData.setAckermannGeometryData(ackermann);
	}

	//Create a vehicle from the wheels and drive sim data.
	PxVehicleDrive4W* vehDrive4W = PxVehicleDrive4W::allocate(numWheels);
	vehDrive4W->setup(physics, veh4WActor, *wheelsSimData, driveSimData, numWheels - 4);

	//Configure the userdata
	//configureUserData(vehDrive4W, vehicle4WDesc.actorUserData, vehicle4WDesc.shapeUserDatas);

	//Free the sim data because we don't need that any more.
	wheelsSimData->free();

	return vehDrive4W;
}


PxFilterFlags VehicleFilterShader
(PxFilterObjectAttributes attributes0, PxFilterData filterData0,
	PxFilterObjectAttributes attributes1, PxFilterData filterData1,
	PxPairFlags& pairFlags, const void* constantBlock, PxU32 constantBlockSize)
{
	PX_UNUSED(attributes0);
	PX_UNUSED(attributes1);
	PX_UNUSED(constantBlock);
	PX_UNUSED(constantBlockSize);

	if ((0 == (filterData0.word0 & filterData1.word1)) && (0 == (filterData1.word0 & filterData0.word1)))
		return PxFilterFlag::eSUPPRESS;

	pairFlags = PxPairFlag::eCONTACT_DEFAULT;
	pairFlags |= PxPairFlags(PxU16(filterData0.word2 | filterData1.word2));

	return PxFilterFlags();
}

static void InitPhysics() {
	s_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, gDefaultAllocatorCallback, gDefaultErrorCallback);
	
	if (!s_foundation)
		exit(1);

	bool recordMemoryAllocations = false;

	s_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *s_foundation, PxTolerancesScale(), recordMemoryAllocations, nullptr);
	if (!s_physics)
		exit(2);

	s_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *s_foundation, PxCookingParams(PxTolerancesScale()));
	if (!s_cooking)
		exit(3);

	PxSceneDesc sceneDesc(s_physics->getTolerancesScale());
	sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);
	// create CPU dispatcher which mNbThreads worker threads
	s_cpu_dispatcher = PxDefaultCpuDispatcherCreate(1);
	if (!s_cpu_dispatcher)
		exit(3);

	sceneDesc.cpuDispatcher = s_cpu_dispatcher;
	sceneDesc.filterShader = VehicleFilterShader;

	//if (!sceneDesc.filterShader)
	//	sceneDesc.filterShader = &PxDefaultSimulationFilterShader;

	s_scene = s_physics->createScene(sceneDesc);
	if (!s_scene)
		exit(4);

	auto m_scene = s_scene;
	bool enable = true;

	m_scene->setVisualizationParameter(PxVisualizationParameter::eSCALE, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eACTOR_AXES, enable ? 2.0f : 0.0f);
	////m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_EDGES, enable ? 1.0f : 0.0f);
	////m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_COMPOUNDS, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AABBS, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eMBP_REGIONS, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eACTOR_AXES, enable ? 0.5f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_ANG_VELOCITY, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_AXES, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_LIN_VELOCITY, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_MASS_AXES, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AABBS, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AXES, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_COMPOUNDS, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_DYNAMIC, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_EDGES, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_FNORMALS, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_SHAPES, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_STATIC, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_ERROR, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_FORCE, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_NORMAL, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_POINT, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eCULL_BOX, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eFORCE_DWORD, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eJOINT_LIMITS, enable ? 1.0f : 0.0f);
	//m_scene->setVisualizationParameter(PxVisualizationParameter::eJOINT_LOCAL_FRAMES, enable ? 1.0f : 0.0f);
	m_scene->setVisualizationParameter(PxVisualizationParameter::eWORLD_AXES, enable ? 1.0f : 0.0f);

	PxShape* shape;

	auto floor_material = s_physics->createMaterial(1.0f, 1.0f, 0.0f);
	s_material = s_physics->createMaterial(0.5f, 0.5f, 0.6f);

	//auto floor = PxCreatePlane(*s_physics, PxPlane(PxVec3(0.f, 1.f, 0.f), 0.f), *floor_material);
	//floor->getShapes(&shape, 1);
	//SetupFiltering(shape, CG_FLOOR, CG_BOX | CG_FLOOR, DRIVABLE_SURFACE);
	//s_scene->addActor(*floor);


	//s_box1 = PxCreateDynamic(*s_physics, PxTransform(PxVec3(0.f, 5.0f, 0.f)), PxBoxGeometry(0.5f, 0.5f, 0.5f), *s_material, 1.f);
	//s_box1->getShapes(&shape, 1);
	//SetupFiltering(shape, CG_BOX, CG_FLOOR, DRIVABLE_SURFACE);
	//s_scene->addActor(*s_box1);

	PxInitVehicleSDK(*s_physics);
	PxVehicleSetBasisVectors(PxVec3(0, 1, 0), PxVec3(0, 0, 1));
	PxVehicleSetUpdateMode(PxVehicleUpdateMode::eVELOCITY_CHANGE);

	//Create the batched scene queries for the suspension raycasts.
	gVehicleSceneQueryData = VehicleSceneQueryData::allocate(1, PX_MAX_NB_WHEELS, 1, 1, WheelSceneQueryPreFilterBlocking, NULL, gDefaultAllocatorCallback);
	gBatchQuery = VehicleSceneQueryData::setUpBatchedSceneQuery(0, *gVehicleSceneQueryData, s_scene);

	//Create the friction table for each combination of tire and surface type.
	gFrictionPairs = createFrictionPairs(s_material);

	//Create a plane to drive on.
	PxFilterData groundPlaneSimFilterData(COLLISION_FLAG_GROUND, COLLISION_FLAG_GROUND_AGAINST, 0, 0);
	auto floor = createDrivablePlane(groundPlaneSimFilterData, s_material, s_physics);
	s_scene->addActor(*floor);

	//Create a vehicle that will drive on the plane.
	VehicleDesc vehicleDesc = initVehicleDesc();
	gVehicle4W = createVehicle4W(vehicleDesc, s_physics, s_cooking);
	PxTransform startTransform(PxVec3(0, (vehicleDesc.chassisDims.y * 0.5f + vehicleDesc.wheelRadius + 1.0f) + 2.0, 0), PxQuat(PxIdentity));
	gVehicle4W->getRigidDynamicActor()->setGlobalPose(startTransform);
	s_scene->addActor(*gVehicle4W->getRigidDynamicActor());

	//Set the vehicle to rest in first gear.
	//Set the vehicle to use auto-gears.
	gVehicle4W->setToRestState();
	gVehicle4W->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST);
	gVehicle4W->mDriveDynData.setUseAutoGears(true);

}

//PxVehicleKeySmoothingData gKeySmoothingData =
//{
//	{
//		6.0f,	//rise rate eANALOG_INPUT_ACCEL
//		6.0f,	//rise rate eANALOG_INPUT_BRAKE		
//		6.0f,	//rise rate eANALOG_INPUT_HANDBRAKE	
//		2.5f,	//rise rate eANALOG_INPUT_STEER_LEFT
//		2.5f,	//rise rate eANALOG_INPUT_STEER_RIGHT
//	},
//	{
//		10.0f,	//fall rate eANALOG_INPUT_ACCEL
//		10.0f,	//fall rate eANALOG_INPUT_BRAKE		
//		10.0f,	//fall rate eANALOG_INPUT_HANDBRAKE	
//		5.0f,	//fall rate eANALOG_INPUT_STEER_LEFT
//		5.0f	//fall rate eANALOG_INPUT_STEER_RIGHT
//	}
//};

PxVehicleKeySmoothingData gKeySmoothingData =
{
	{
		4.0f,	//rise rate eANALOG_INPUT_ACCEL
		10.0f,	//rise rate eANALOG_INPUT_BRAKE		
		6.0f,	//rise rate eANALOG_INPUT_HANDBRAKE	
		5.0f,	//rise rate eANALOG_INPUT_STEER_LEFT
		5.0f,	//rise rate eANALOG_INPUT_STEER_RIGHT
	},
	{
		10.0f,	//fall rate eANALOG_INPUT_ACCEL
		10.0f,	//fall rate eANALOG_INPUT_BRAKE		
		10.0f,	//fall rate eANALOG_INPUT_HANDBRAKE	
		7.0f,	//fall rate eANALOG_INPUT_STEER_LEFT
		7.0f	//fall rate eANALOG_INPUT_STEER_RIGHT
	}
};

//PxF32 gSteerVsForwardSpeedData[2 * 8] =
//{
//	0.0f,		0.75f,
//	5.0f,		0.75f,
//	30.0f,		0.125f,
//	120.0f,		0.06f,
//	PX_MAX_F32, PX_MAX_F32,
//	PX_MAX_F32, PX_MAX_F32,
//	PX_MAX_F32, PX_MAX_F32,
//	PX_MAX_F32, PX_MAX_F32
//};

PxF32 gSteerVsForwardSpeedData[2 * 8] =
{
	0.0f,		0.75f,
	5.0f,		0.35f,
	30.0f,		0.06f,
	120.0f,		0.02f,
	PX_MAX_F32, PX_MAX_F32,
	PX_MAX_F32, PX_MAX_F32,
	PX_MAX_F32, PX_MAX_F32,
	PX_MAX_F32, PX_MAX_F32
};
PxFixedSizeLookupTable<8> gSteerVsForwardSpeedTable(gSteerVsForwardSpeedData, 4);

PxVehicleDrive4WRawInputData gVehicleInputData;

bool					gIsVehicleInAir = true;

static TSR::Camera s_cam;


static void PhysicsFrame() {
	auto timestep = 1.0f / 75.0f;


	//Update the control inputs for the vehicle.
	//if (gMimicKeyInputs)
	//{
	//	PxVehicleDrive4WSmoothDigitalRawInputsAndSetAnalogInputs(gKeySmoothingData, gSteerVsForwardSpeedTable, gVehicleInputData, timestep, gIsVehicleInAir, *gVehicle4W);
	//}
	//else
	//{
	//	PxVehicleDrive4WSmoothAnalogRawInputsAndSetAnalogInputs(gPadSmoothingData, gSteerVsForwardSpeedTable, gVehicleInputData, timestep, gIsVehicleInAir, *gVehicle4W);
	//}

	PxVehicleDrive4WSmoothDigitalRawInputsAndSetAnalogInputs(gKeySmoothingData, gSteerVsForwardSpeedTable, gVehicleInputData, timestep, gIsVehicleInAir, *gVehicle4W);
	//Raycasts.
	PxVehicleWheels* vehicles[1] = { gVehicle4W };
	PxRaycastQueryResult* raycastResults = gVehicleSceneQueryData->getRaycastQueryResultBuffer(0);
	const PxU32 raycastResultsSize = gVehicleSceneQueryData->getQueryResultBufferSize();
	PxVehicleSuspensionRaycasts(gBatchQuery, 1, vehicles, raycastResultsSize, raycastResults);

	//Vehicle update.
	const PxVec3 grav = s_scene->getGravity();
	PxWheelQueryResult wheelQueryResults[PX_MAX_NB_WHEELS];
	PxVehicleWheelQueryResult vehicleQueryResults[1] = { {wheelQueryResults, gVehicle4W->mWheelsSimData.getNbWheels()} };
	PxVehicleUpdates(timestep, grav, *gFrictionPairs, 1, vehicles, vehicleQueryResults);

	//Work out if the vehicle is in the air.
	gIsVehicleInAir = gVehicle4W->getRigidDynamicActor()->isSleeping() ? false : PxVehicleIsInAir(vehicleQueryResults[0]);

	s_scene->simulate(timestep);
	s_scene->fetchResults(true);

	
}

static glm::vec4 U32ColorToVec4(uint32_t rgba_value) {
	float red = static_cast<float>((rgba_value >> 24) & 0xFF) / 255.0f;
	float green = static_cast<float>((rgba_value >> 16) & 0xFF) / 255.0f;
	float blue = static_cast<float>((rgba_value >> 8) & 0xFF) / 255.0f;
	float alpha = static_cast<float>(rgba_value & 0xFF) / 255.0f;

	return glm::vec4(red, green, blue, alpha);
}

static void DrawPhysxDebug() {
	const PxRenderBuffer& rb = s_scene->getRenderBuffer();
	for(PxU32 i=0; i < rb.getNbPoints(); i++)
	{
		const PxDebugPoint& point = rb.getPoints()[i];
		// render the point
		s_draw_points.push_back({ glm::vec3(point.pos.x, point.pos.y, point.pos.z), U32ColorToVec4(point.color) });

	}

	for(PxU32 i=0; i < rb.getNbLines(); i++)
	{
		const PxDebugLine& line = rb.getLines()[i];
		// render the line
		s_draw_lines.push_back({ glm::vec3(line.pos0.x, line.pos0.y, line.pos0.z), U32ColorToVec4(line.color0) });
		s_draw_lines.push_back({ glm::vec3(line.pos1.x, line.pos1.y, line.pos1.z), U32ColorToVec4(line.color1) });
	}

}

static void ShutdownPhysics() {
	PxCloseVehicleSDK();

	s_physics->release();
	s_foundation->release();
}

static int s_count = 0;

static bool s_first_person = false;

int main() {
	if (!glfwInit()) {
		std::cout << "Failed to initialize GLFW\n";
		return 1;
	}

	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);
	glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
	glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
	glfwWindowHint(GLFW_RESIZABLE, GL_TRUE);
	glfwWindowHint(GLFW_MAXIMIZED, GL_TRUE);
	glfwWindowHint(GLFW_DEPTH_BITS, 0);
	//glfwWindowHint(GLFW_DEPTH_BITS, 0);
	glfwWindowHint(GLFW_STENCIL_BITS, 0);
	glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);

	//GLFWwindow* window = glfwCreateWindow(1920, 1080, "Karo", glfwGetPrimaryMonitor(), NULL);
	GLFWwindow* window = glfwCreateWindow(800, 600, "Karo", NULL, NULL);
	if (!window) {
		std::cerr << "Failed to create window\n";
		glfwTerminate();
		return 1;
	}

	glfwMakeContextCurrent(window);

	if (glewInit() != GLEW_OK) {
		std::cerr << "Failed to initialize GLEW\n";
		glfwTerminate();
		return 1;
	}

	std::cout << "OpenGL version: " << glGetString(GL_VERSION) << std::endl;

	GLuint shader;
	shader = glCreateProgram();

	GLuint vs = glCreateShader(GL_VERTEX_SHADER);
	glShaderSource(vs, 1, &s_vs_src, NULL);
	glCompileShader(vs);
	CheckShaderCompileErrors(vs);

	GLuint fs = glCreateShader(GL_FRAGMENT_SHADER);
	glShaderSource(fs, 1, &s_fs_src, NULL);
	glCompileShader(fs);
	CheckShaderCompileErrors(fs);

	glAttachShader(shader, vs);
	glAttachShader(shader, fs);
	glLinkProgram(shader);
	CheckProgramLinkErrors(shader);

	glDeleteShader(vs);
	glDeleteShader(fs);

	glUseProgram(shader);

	GLuint u_mvp = glGetUniformLocation(shader, "u_mvp");

	GLuint vao;
	glGenVertexArrays(1, &vao);
	glBindVertexArray(vao);

	GLuint vbo;
	glGenBuffers(1, &vbo);
	glBindBuffer(GL_ARRAY_BUFFER, vbo);
	
	//setup for stream draw
	glBufferData(GL_ARRAY_BUFFER, 0, NULL, GL_STREAM_DRAW);

	glEnableVertexAttribArray(0);
	glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, pos));
	glEnableVertexAttribArray(1);
	glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, color));

	glBindVertexArray(0);

	//glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	glfwShowWindow(window);

	InitPhysics();


	glfwSetKeyCallback(window, [](GLFWwindow* window, int key, int scancode, int action, int mods) {
		if (action != GLFW_PRESS)
			return;

		switch (key) {
			case GLFW_KEY_ESCAPE:
				glfwSetWindowShouldClose(window, true);
				break;

			//case GLFW_KEY_SPACE:
			//	s_box1->addForce(PxVec3(0.0f, 100.0f, 0.0f));
			//	break;

			case GLFW_KEY_E:
				//for (int i = 0; i < 1; i++) {

				//	auto newbox = PxCreateDynamic(*s_physics, PxTransform(PxVec3(0.f, 5.0f * i, 0.f)), PxBoxGeometry(0.5f, 0.5f, 0.5f), *s_material, 1.f);
				//	PxShape* shape;
				//	newbox->getShapes(&shape, 1);
				//	//SetupFiltering(shape, CG_BOX, CG_FLOOR | CG_BOX, 0);
				//	s_scene->addActor(*newbox);
				//	s_count++;
				//}
				break;

			case GLFW_KEY_W:
				printf("pocet: %d\n", s_count);
				break;

			case GLFW_KEY_V:
				s_first_person = !s_first_person;
				break;

			default:
				break;
		}
	});



	glfwSetCursorPosCallback(window, [](GLFWwindow* window, double x, double y) {
		static double prev_x = x;
		static double prev_y = y;

		
		s_cam.ProcessMouse(x - prev_x, prev_y - y);
		prev_x = x;
		prev_y = y;
	});

	//glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED);

	auto KeyDown = [window](int key) {
		return glfwGetKey(window, key) == GLFW_PRESS;
	};

	glfwSwapInterval(1);

	// Setup Dear ImGui context
	IMGUI_CHECKVERSION();
	ImGui::CreateContext();
	ImGuiIO& io = ImGui::GetIO(); (void)io;
	io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard;     // Enable Keyboard Controls
	io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad;      // Enable Gamepad Controls

	// Setup Dear ImGui style
	ImGui::StyleColorsDark();
	//ImGui::StyleColorsLight();

	// Setup Platform/Renderer backends
	ImGui_ImplGlfw_InitForOpenGL(window, true);
	ImGui_ImplOpenGL3_Init("#version 130");

	while (!glfwWindowShouldClose(window)) {
		glfwPollEvents();
	
		// Start the Dear ImGui frame
		ImGui_ImplOpenGL3_NewFrame();
		ImGui_ImplGlfw_NewFrame();
		ImGui::NewFrame();

		int width, height;
		glfwGetFramebufferSize(window, &width, &height);

		glViewport(0, 0, width, height);

		auto pos = gVehicle4W->getRigidDynamicActor()->getGlobalPose().p;
		glm::vec3 pos_glm(pos.x, pos.y + 2.0f, pos.z);

		glm::mat4 proj = glm::perspective(glm::radians(60.0f), (float)width / (float)height, 0.1f, 1000.0f);
		//glm::mat4 view = glm::lookAt(glm::vec3(1.0f, 10.0f, 20.0f) * 5.0f /*pos_glm*/, glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(0.0, 1.0, 0.0));

		glm::mat4 view;

		if (!s_first_person) {
			s_cam.position = pos_glm;
			s_cam.third_person_distance = 15.0f;
			view = s_cam.GetViewMatrix();
		}
		else {
			//auto trans = gVehicle4W->getRigidDynamicActor()->getGlobalPose();

			auto actor = gVehicle4W->getRigidDynamicActor();
			PxShape* shapes[8];

			actor->getShapes(shapes, 8);
			
			PxMat44 trans_mat = PxShapeExt::getGlobalPose(*shapes[4], *actor);

			auto fd = shapes[4]->getSimulationFilterData();

			glm::mat4& trans = *((glm::mat4*)(&trans_mat));

			//
			//shapes[0]->getLocalPose
			//
			//glm::vec3 position(trans.p.x, trans.p.y, trans.p.z);
			//glm::quat rotation(trans.q.x, trans.q.y, trans.q.z, trans.q.w);

			//glm::mat4 rotationMatrix = glm::mat4_cast(rotation);

			//// Step 2: Create translation matrix
			//glm::mat4 translationMatrix = glm::translate(glm::mat4(1.0f), position);

			//// Step 3: Combine rotation and translation to get the final transformation matrix
			//glm::mat4 transformationMatrix = translationMatrix * rotationMatrix;

			s_cam.position = glm::vec3(0.0f);
			//s_cam.up_vector.y = -1.0f;
			view = s_cam.GetViewMatrix(true) * glm::inverse(trans);
		}

		auto& dyndata = gVehicle4W->mDriveDynData;

		/*gVehicle4W->computeForwardSpeed()*/

		//printf("e: %9.2f  g: %u\n", , dyndata.getCurrentGear());

		auto speed = gVehicle4W->computeForwardSpeed() * 3.6f;
		auto enginespeed = dyndata.getEngineRotationSpeed() / (PxPi * 2.0) * 60.0f;
		auto gear = (int)dyndata.getCurrentGear();
		auto dist = glm::length(pos_glm);

		static const char* gearname[] = {
			"R", "N", "1", "2", "3", "4", "5"
		};


		ImGui::Begin("state");

		ImGui::Text("dist: %.1f m", dist);
		ImGui::Text("speed: %.1f km/h", speed);
		ImGui::Text("rpm: %.1f", enginespeed);
		ImGui::ProgressBar(enginespeed / 6000.0f);
		ImGui::SliderInt("gear", &gear, 0, 6, gearname[gear]);

		ImGui::End();

		ImGui::ShowDemoWindow();

		glm::mat4 mvp = proj * view;

		glUniformMatrix4fv(u_mvp, 1, GL_FALSE, glm::value_ptr(mvp));

		//glClearColor(0.2f, 0.3f, 0.3f, 1.0f);
		glClear(GL_COLOR_BUFFER_BIT);

		//// test data
		//s_draw_verts.push_back({ glm::vec3(-1, 0, 0), glm::vec3(1.0f, 0.0f, 0.0f) });
		//s_draw_verts.push_back({ glm::vec3(1, 0, 0), glm::vec3(0.0f, 1.0f, 0.0f) });

		gVehicleInputData.setDigitalAccel(KeyDown(GLFW_KEY_W));
		gVehicleInputData.setDigitalSteerLeft(KeyDown(GLFW_KEY_D));
		gVehicleInputData.setDigitalSteerRight(KeyDown(GLFW_KEY_A));
		gVehicleInputData.setDigitalBrake(KeyDown(GLFW_KEY_S));
		gVehicleInputData.setDigitalHandbrake(KeyDown(GLFW_KEY_SPACE));

		gVehicleInputData.setGearUp(KeyDown(GLFW_KEY_R));
		gVehicleInputData.setGearDown(KeyDown(GLFW_KEY_F));

		auto grid_center = glm::floor(s_cam.position * 0.2f) * 5.0f;
		grid_center.y = 0.0f;

		for (int i = -50; i <= 50; i++) {
			s_draw_lines.push_back({ grid_center + glm::vec3(-250.0f, 0.0f, i * 5.0f), glm::vec4(0.5f, 0.5f, 0.5f, 1.0f)});
			s_draw_lines.push_back({ grid_center + glm::vec3(250.0f, 0.0f, i * 5.0f), glm::vec4(0.5f, 0.5f, 0.5f, 1.0f) });
		}

		for (int i = -50; i <= 50; i++) {
			s_draw_lines.push_back({ grid_center + glm::vec3(i * 5.0f, 0.0f, -250.0f), glm::vec4(0.5f, 0.5f, 0.5f, 1.0f) });
			s_draw_lines.push_back({ grid_center + glm::vec3(i * 5.0f, 0.0f, 250.0f), glm::vec4(0.5f, 0.5f, 0.5f, 1.0f) });
		}

		PhysicsFrame();

		DrawPhysxDebug();

		glBindVertexArray(vao);
		glBindBuffer(GL_ARRAY_BUFFER, vbo);

		glBufferData(GL_ARRAY_BUFFER, s_draw_lines.size() * sizeof(Vertex), s_draw_lines.data(), GL_STREAM_DRAW);
		glDrawArrays(GL_LINES, 0, s_draw_lines.size());

		//glBufferData(GL_ARRAY_BUFFER, s_draw_points.size() * sizeof(Vertex), s_draw_points.data(), GL_STREAM_DRAW);
		//glDrawArrays(GL_POINTS, 0, s_draw_points.size());

		s_draw_lines.clear();
		s_draw_points.clear();

		ImGui::Render();

		ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());

		glfwSwapBuffers(window);
	}

	ShutdownPhysics();
}

// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu

// Tips for Getting Started: 
//   1. Use the Solution Explorer window to add/manage files
//   2. Use the Team Explorer window to connect to source control
//   3. Use the Output window to see build output and other messages
//   4. Use the Error List window to view errors
//   5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
//   6. In the future, to open this project again, go to File > Open > Project and select the .sln file