#include "Geo_Obj_Physics.h"
#include "Global_Properties.h"
#include "Collision_Manager.h"
#include "Time.h"

namespace Geo
{
	GeoObjPhysics::GeoObjPhysics(Geo::Data::GeoData& aModelData, Textures::Data::TextureData& aTextureData) : GeoObjTextured(aModelData, aTextureData),
				mVelocity(0), mMass(1.0f), mIsKinematic(false), mUseGravity(true), mIsSleeping(false), mAbleToSleep(false),
				mSleepFrameCounter(0), mRWAMotion(0)
	{
		SetPosition(glm::vec3(0)); // Important to do this here, makes sure the collider is correctly positioned
		CollisionManager::I().AddPhysicsObject(*this);
		mCollider.CreateBoundingBox(mModelData);

#ifdef USE_OCTREE
		mCollider.SetPhysicsObject(this);
#endif
	}

	void GeoObjPhysics::Update()
	{
		if (mIsSleeping)
			return;

		CheckMinMaxVelocity();
		CheckIfSleeping();
		Move();
	}

	void GeoObjPhysics::CheckIfSleeping()
	{
		float motion = glm::dot(GetPosition(), GetPosition());
		float bias = 0.94f;
		mRWAMotion = bias*mRWAMotion + (1-bias)*motion;

		if (abs(mRWAMotion - motion) < GlobalProperties::SLEEP_EPSILON) // You are slow enough to be sleeping
		{
			if (++mSleepFrameCounter >= GlobalProperties::SLEEP_FRAMES) // If you have been slow enough for SLEEP_FRAMES frames, time to go to sleep
				mAbleToSleep = true;
		}
		else // You are not slow enough to be sleeping, start over frame counter
		{
			mAbleToSleep = false;
			mSleepFrameCounter = 0;
		}
	}

	void GeoObjPhysics::Sleep()
	{
		mIsSleeping = true;
		mAbleToSleep = false;
		mUseGravity = false;
		mVelocity = glm::vec3(0);

		mCollider.SetHit(true); // Debug
	}

	void GeoObjPhysics::WakeUp()
	{
		mIsSleeping = false;
		mUseGravity = true;
		mSleepFrameCounter = 0;

		// Wake up all objects that were sleeping on this object
		std::list<GeoObjPhysics*>::iterator i = mObjectsSleepingOnThis.begin();
		while(i != mObjectsSleepingOnThis.end())
		{
			(**i).WakeUp();
			mObjectsSleepingOnThis.erase(i++);
		}

		mCollider.SetHit(false); // Debug
	}

	void GeoObjPhysics::CheckMinMaxVelocity()
	{
		// Check max
		float tv = GlobalProperties::TERMINAL_VELOCITY;

		if (mVelocity.x < -tv)
			mVelocity.x = -tv;
		if (mVelocity.y < -tv)
			mVelocity.y = -tv;
		if (mVelocity.z < -tv)
			mVelocity.z = -tv;

		if (mVelocity.x > tv)
			mVelocity.x = tv;
		if (mVelocity.y > tv)
			mVelocity.y = tv;
		if (mVelocity.z > tv)
			mVelocity.z = tv;

		// Check min
		float mv = GlobalProperties::VELOCITY_MIN;

		if (mVelocity.x != 0 && mVelocity.x > -mv && mVelocity.x < mv)
			mVelocity.x = 0;
		if (mVelocity.y != 0 && mVelocity.y > -mv && mVelocity.y < mv)
			mVelocity.y = 0;
		if (mVelocity.z != 0 && mVelocity.z > -mv && mVelocity.z < mv)
			mVelocity.z = 0;
	}

	void GeoObjPhysics::Move()
	{
		Translate(mVelocity * GLUtils::Time::GetDeltaTime());
	}

	float GeoObjPhysics::GetInverseMass()
	{
		if (mMass == 0.0f)
			return 0.0f;
		else
			return 1 / mMass;
	}

	void GeoObjPhysics::SetPosition(glm::vec3 aPos)
	{
		glm::vec3 oldPos = GetPosition();

		GetMatrices().SetPosition(aPos);
		mCollider.GetMatrices().SetPosition(aPos);

		#ifdef USE_OCTREE 
		CollisionManager::I().ObjectMoved(*this, oldPos);
		#endif
	}
	void GeoObjPhysics::Translate(glm::vec3 aVec)
	{
		glm::vec3 oldPos = GetPosition();

		GetMatrices().AppendLocation(aVec);
		mCollider.GetMatrices().AppendLocation(aVec);

		#ifdef USE_OCTREE 
		CollisionManager::I().ObjectMoved(*this, oldPos);
		#endif
	}

	void GeoObjPhysics::SetScale(glm::vec3 aScale)
	{
		glm::vec3 oldPos = GetPosition();

		GetMatrices().SetScale(aScale);
		mCollider.SetScale(aScale);

		#ifdef USE_OCTREE 
		CollisionManager::I().ObjectMoved(*this, oldPos);
		#endif
	}
	
	void GeoObjPhysics::Scale(glm::vec3 aScale)
	{
		glm::vec3 oldPos = GetPosition();

		GetMatrices().AppendScale(aScale);
		mCollider.Scale(aScale);

		#ifdef USE_OCTREE 
		CollisionManager::I().ObjectMoved(*this, oldPos);
		#endif
	}

	void GeoObjPhysics::UpdateBindRender()
	{
		GeoObjTextured::UpdateBindRender();

		// Draw the collider
		if (GlobalProperties::DRAW_COLLIDERS)
			mCollider.Draw();
	}
}