#pragma once

#ifndef PHYSICS_H
#define PHYSICS_H
#include <bullet_3.25/btBulletDynamicsCommon.h>
namespace Physics
{
	enum ShapeType { CUBE, SPHERE, CYLINDER };
	class GamePhysicsManager
	{
		public:
			//Setup
			btDefaultCollisionConfiguration* mCollisionConfiguration = new btDefaultCollisionConfiguration();
			btCollisionDispatcher* mDispatcher = new btCollisionDispatcher(mCollisionConfiguration);
			btBroadphaseInterface* mOverlappingPairCache = new btDbvtBroadphase();
			btSequentialImpulseConstraintSolver* mSolver = new btSequentialImpulseConstraintSolver;
			btDiscreteDynamicsWorld* mDynamicsWorld = new btDiscreteDynamicsWorld(mDispatcher, mOverlappingPairCache, mSolver, mCollisionConfiguration);

			// Shapes
			std::vector<btCollisionShape*> mCollisionShapes;
			
			// 1st argument is y coordinate at which ground lies, 
			GamePhysicsManager(const glm::vec3 inputGravity = glm::vec3(0,-10, 0 ))
			{
				
				// Set Gravity
				mDynamicsWorld->setGravity(btVector3(inputGravity.x, inputGravity.y, inputGravity.z));			
			}

			~GamePhysicsManager()
			{
				// Delete Rigid Bodies and their motion states
				for (int i = mDynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
				{
					btCollisionObject* obj = mDynamicsWorld->getCollisionObjectArray()[i];
					btRigidBody* body = btRigidBody::upcast(obj);
					if (body && body->getMotionState())
					{
						delete body->getMotionState();
					}
					mDynamicsWorld->removeCollisionObject(obj);
					delete obj;
				}

				// Delete Collision Shapes
				for (std::vector<btCollisionShape*>::iterator it = mCollisionShapes.begin(); it != mCollisionShapes.end(); it++)
				{
					btCollisionShape* ptrToBeDeleted  = *it;
					delete ptrToBeDeleted;
					*it = nullptr;
				}

				//delete dynamics world
				delete mDynamicsWorld;

				//delete solver
				delete mSolver;

				//delete broadphase
				delete mOverlappingPairCache;

				//delete dispatcher
				delete mDispatcher;

				delete mCollisionConfiguration;
			}

			// Input: side length of cube and position of cube. Vreates a Collisionobject
			// Sphere only uses inputHalfSideLengths.x as radius
			// Cylinder uses inputHalfSideLengths.x as radius and inputHalfSideLengths.y as height along y axis
			btRigidBody* addRigidBody(const glm::vec3& inputPosition, const glm::vec3& inputHalfSideLengths, const float& inputMass, ShapeType shapeType = CUBE)
			{
				// Create Collision Shape
				btCollisionShape* collisionShape;
				if(shapeType == CUBE) collisionShape = new btBoxShape(btVector3(inputHalfSideLengths.x, inputHalfSideLengths.y, inputHalfSideLengths.z));
				if (shapeType == SPHERE) collisionShape = new btSphereShape(inputHalfSideLengths.x);
				if (shapeType == CYLINDER) collisionShape = new btCylinderShape(btVector3(inputHalfSideLengths.x, inputHalfSideLengths.y, inputHalfSideLengths.z));
				mCollisionShapes.push_back(collisionShape);

				/// Create Dynamic Objects
				btTransform startTransform;
				startTransform.setIdentity();
				startTransform.setOrigin(btVector3(inputPosition.x, inputPosition.y, inputPosition.z));

				//rigidbody is dynamic if and only if mass is non zero, otherwise static
				btScalar mass(inputMass);
				bool isDynamic = (mass != 0.f);

				btVector3 localInertia(0, 0, 0);
				if (isDynamic) {
					collisionShape->calculateLocalInertia(mass, localInertia);
				}
				// Create rigid body
				btDefaultMotionState* motionState = new btDefaultMotionState(startTransform);
				btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, motionState, collisionShape, localInertia);
				btRigidBody* body = new btRigidBody(rbInfo);
				body->setAngularFactor(btVector3(0.f, 1.f, 0.f));
				mDynamicsWorld->addRigidBody(body);
				
				// Return body pointer

				return body;
			}
			
			void runStepSimulation(const float& timeSinceLastFrame)
			{
				const btScalar timeStep = (btScalar)timeSinceLastFrame;
				mDynamicsWorld->stepSimulation(timeStep);
			}

			void removeRigidBody(const unsigned int id)
			{
				//TODO
			}
			
	};

	class PhysicsObject
	{
		public:
			glm::vec3 mBoundingBoxExtends;
			btRigidBody* mRigidBody;
			unsigned int mIdRigidBody;


			// Set mass to zero for static object
			PhysicsObject(GamePhysicsManager& inputPhysicsManager, const glm::vec3& inputPosition, const glm::vec3& inputHalfSideLengths, const float& inputMass, ShapeType inputShapeType = CUBE) :
				mRigidBody{ inputPhysicsManager.addRigidBody(inputPosition, inputHalfSideLengths, inputMass, inputShapeType) }
			{
				mRigidBody->forceActivationState(DISABLE_DEACTIVATION);
				//mRigidBody->getMotionState()->setWorldTransform(mRigidBody->getWorldTransform());

			}

			glm::vec3 getPosition() const
			{
				btTransform trafo;
				mRigidBody->getMotionState()->getWorldTransform(trafo);
				float x = (float)trafo.getOrigin().getX();
				float y = (float)trafo.getOrigin().getY();
				float z = (float)trafo.getOrigin().getZ();
				return glm::vec3(x, y, z);
			}

			

			void setVelocity(const glm::vec3& inputVelocity)
			{
				mRigidBody->activate(true);
				mRigidBody->setLinearVelocity(btVector3(inputVelocity.x, inputVelocity.y, inputVelocity.z));
			}

			glm::mat4 getModelMatrix() const
			{
				btTransform trafo;
				mRigidBody->getMotionState()->getWorldTransform(trafo);

				// Position
				float x = (float)trafo.getOrigin().getX();
				float y = (float)trafo.getOrigin().getY();
				float z = (float)trafo.getOrigin().getZ();
				glm::vec3 position(x, y, z);

				// Rotation
				btQuaternion rot = trafo.getRotation();
				glm::quat rotation(rot.getW(), rot.getX(), rot.getY(), rot.getZ());
				glm::mat4 rotMatrix = glm::mat4_cast(rotation);

				// Uniform scale, update if necessary
				glm::vec3 scale(1.0f);
				glm::mat4 scaleMatrix = glm::scale(glm::mat4(1.0f), scale);

				// Combine translation, rotation, and scale
				return glm::translate(glm::mat4(1.0f), position) * rotMatrix * scaleMatrix;
			}

			//Ill leave this one for base case, we dont have any rotation or scale physics updates yet
			// glm::mat4 getModelMatrix()
			// {
			// 	return glm::translate(glm::mat4(1), getPosition());
			// }

			void makeKinematic(btRigidBody* body) {
				body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
				body->setActivationState(DISABLE_DEACTIVATION);
			}

			void updateKinematicPosition(btRigidBody* body, glm::vec3 newPosition) {
				btTransform trans;
				body->getMotionState()->getWorldTransform(trans);
				trans.setOrigin(btVector3(newPosition.x, 0.5, newPosition.z)); //0.5 seems hardcody
				body->getMotionState()->setWorldTransform(trans);
				body->setWorldTransform(trans);
			}

			void makeDynamic(btRigidBody* body) {
				body->setCollisionFlags(body->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT);
				body->setActivationState(ACTIVE_TAG);
			}





	

	};
	
	class Player : public PhysicsObject
	{
		enum JumpState {NOT_JUMPING, GOING_UP, DECELLARATION, GOING_DOWN, LANDING};
	public:
		Student::CameraForGLFW mCamera;
		glm::vec3 mLocalCameraOffset; // Offset from player center of Mass
		float mMaxSpeedFromUserInput;
		JumpState mJumpState = NOT_JUMPING;
		
		Player(
			GamePhysicsManager& inputPhysicsManager,
			const glm::vec3& inputPosition,
			const glm::vec3& inputBBExtends,
			const float& inputMass,
			const glm::vec3& inputLocalCamOffset,
			const float& inputSpeed,
			const float& inputJumpPower,
			GLFWwindow* window,
			const float& inputAspectRatio,
			const glm::vec3& inputWorldUp,
			const glm::vec3& inputFront
		):
			PhysicsObject(inputPhysicsManager, inputPosition, inputBBExtends, inputMass, CYLINDER),
			mLocalCameraOffset(inputLocalCamOffset),
			mCamera(window, inputAspectRatio, inputPosition + inputLocalCamOffset, inputWorldUp, inputFront, inputSpeed, inputJumpPower),
			mMaxSpeedFromUserInput(inputSpeed)
		{
			// Nothin happening here
		}

		// Get new camera Velocity and Orientation from Keyboard / mouse Inputs
		void processUserInputs()
		{
			mCamera.updateCameraAtNewFrame();

			// Move player according to keyboard key input
			mRigidBody->setLinearVelocity(btVector3(mCamera.ForcedVelocity.x, mRigidBody->getLinearVelocity().getY(), mCamera.ForcedVelocity.z));

			// Process Jumping commands
			//If player is not  jumping
			float verticalSpeed = (float)mRigidBody->getLinearVelocity().getY();
			float totalVerticalForce = (float)mRigidBody->getTotalForce().getY();
			if (mJumpState == NOT_JUMPING)
			{
				// Check if jump key was pressed
				if (glm::length(mCamera.Force) > 0.0001)
				{
					// Apply jump Force from user input
					mJumpState = GOING_UP;
				}
			}
			else if(mJumpState== GOING_UP)// Player is already jumping (flying in air)
			{
				// player still allowed to gain jump speed: keep applying jump force
				if(verticalSpeed < 5)
				{
					mRigidBody->applyCentralForce(btVector3(mCamera.Force.x, mCamera.Force.y, mCamera.Force.z));
				}
				else // player has reached max  jumping speed, stop applying jump force and reset it
				{
					mJumpState = DECELLARATION;
				}
			}
			else if (mJumpState == DECELLARATION)
			{
				// player is at highest point: Change jump state to going down
				if (verticalSpeed < 0)
				{
					mJumpState = GOING_DOWN;
				}
			}
			else if (mJumpState == GOING_DOWN)
			{
				if (verticalSpeed < -0.1)
				{
					mJumpState =LANDING;
				}
			}
			else if (mJumpState == LANDING)
			{
				if (verticalSpeed > -0.00001)
				{
					mJumpState = NOT_JUMPING;
					mCamera.Force = glm::vec3(0, 0, 0);
				}
			}


		}

		void updateCameraPosition()
		{
			mCamera.Position = getPosition() + mLocalCameraOffset;
		}
	};


}
#endif