#pragma once
#include "../../Common/Vector3.h"
#include "../../Common/Matrix3.h"
#include <deque>

using namespace NCL::Maths;

namespace NCL {

	enum CollisionResolution { Impulse = 1, Spring = 2, Collect = 4, Jelly = 8, Snow = 16 };

	class CollisionVolume;
	
	namespace CSC8503 {
		class Transform;

		class PhysicsObject {
		public:
			PhysicsObject(Transform* parentTransform, const CollisionVolume* parentVolume);
			~PhysicsObject();

			Vector3 GetLinearVelocity() const {
				return linearVelocity;
			}

			Vector3 GetAngularVelocity() const {
				return angularVelocity;
			}

			float GetElasticity() const
			{
				return elasticity;
			}

			float GetBuoyancy() const
			{
				return buoyancy;
			}

			Vector3 GetTorque() const {
				return torque;
			}

			Vector3 GetForce() const {
				return force;
			}

			void SetInverseMass(float invMass) {
				inverseMass = invMass;
			}

			float GetInverseMass() const {
				return inverseMass;
			}

			void ApplyAngularImpulse(const Vector3& force);
			void ApplyLinearImpulse(const Vector3& force);

			void AddForce(const Vector3& force);

			void AddForceAtPosition(const Vector3& force, const Vector3& position);
			void AddForceAtRelativePosition(const Vector3& addedForce, const Vector3& position);

			void AddTorque(const Vector3& torque);


			void ClearForces();

			void SetLinearVelocity(const Vector3& v) {
				linearVelocity = v;
			}

			void SetAngularVelocity(const Vector3& v) {
				angularVelocity = v;
			}

			void SetElasticity(const float& e)
			{
				elasticity = e;
			}

			void SetBuoyancy(const float& b)
			{
				buoyancy = b;
			}

			const bool IsAffectedByGravity() const
			{
				return affectedByGravity;
			}

			void SetGravityAffinity(bool value)
			{
				affectedByGravity = value;
			}

			void SetCollisionResolution(const int value)
			{
				collisionResolution = value;
			}

			int GetCollisionResolution() const
			{
				return collisionResolution;
			}

			bool IsSleeping() const {
				return isSleeping;
			}

			void SetSleep(bool sleepState) {
				isSleeping = sleepState;

				if (!sleepState) {
					positionDeltaQueue.clear();
				}
			}

			void SetSnow(const bool& s) { isSnow = s; }
			bool GetSnow() const { return isSnow; }

			void AddPositionDelta(const Vector3& positionDelta) {
				if (inverseMass - FLT_EPSILON < 0.0f) {
					return;
				}

				positionDeltaQueue.push_front(positionDelta);
				Vector3 posDeltaAvg = Vector3(0.0f, 0.0f, 0.0f);

				for (Vector3 posDelta : positionDeltaQueue) {
					posDeltaAvg += posDelta;
				}

				const size_t queueSize = positionDeltaQueue.size();

				if (queueSize >= 5) {
					posDeltaAvg /= queueSize;
					isSleeping = posDeltaAvg.GetAbsMaxElement() < 0.0005f;

					positionDeltaQueue.pop_back();
				}
			}

			void InitCubeInertia();
			void InitSphereInertia();
			void InitHollowSphereInertia();

			void UpdateInertiaTensor();

			Matrix3 GetInertiaTensor() const {
				return inverseInteriaTensor;
			}

		protected:
			const CollisionVolume* volume;
			Transform*		transform;

			float inverseMass;
			float elasticity;
			float friction;
			float buoyancy;
			int collisionResolution;
			bool affectedByGravity;

			//linear stuff
			Vector3 linearVelocity;
			Vector3 force;
			

			//angular stuff
			Vector3 angularVelocity;
			Vector3 torque;
			Vector3 inverseInertia;
			Matrix3 inverseInteriaTensor;

			std::deque<Vector3> positionDeltaQueue;
			bool isSleeping = inverseMass == 0.0f;

			bool isSnow;
		};
	}
}