#include <assert.h>
#include "utils/maths.h"
#include "render/debugrender.h"
#include "physics_world.h"
#include "physics_render.h"
#include "physics_object.h"
#include "game/game_object.h"

#include <ode/collision_trimesh.h>

static void odeNearCollisionCallback(void* _this, dGeomID id1, dGeomID id2)
{
	PhysicsWorld* physWorld = (PhysicsWorld*)_this;
	return physWorld->NearCollisionCallback(id1, id2);
}

PhysicsWorld* g_PhysicsWorld = nullptr;

PhysicsWorld::PhysicsWorld()
{
	m_dynamicsWorld = nullptr;
	m_space = nullptr;
}

PhysicsWorld::~PhysicsWorld()
{
}

void PhysicsWorld::Create()
{
	m_dynamicsWorld = dWorldCreate();
	m_space = dHashSpaceCreate(0);
}

void PhysicsWorld::Destroy()
{
	dSpaceDestroy(m_space);
	dWorldDestroy(m_dynamicsWorld);
}

void PhysicsWorld::Update(float deltaTime)
{
	dSpaceCollide(m_space, this, &odeNearCollisionCallback);
	dWorldQuickStep(m_dynamicsWorld, 1);


	//dWorldStep(m_dynamicsWorld, 0.05);
	//dWorldQuickStep(m_dynamicsWorld, stepSize);
}

void PhysicsWorld::DebugDraw()
{
	for (int i = 0; i < m_objects.size(); i++)
	{
		PhysicsObject* obj = m_objects[i];
		if (dGeomID geom = obj->GetShape())
		{
			int type = dGeomGetClass(geom);
			switch(type) 
			{ 

				//case dSphereClass: 
				//case dBoxClass: 
				//case dTriMeshClass: 
				//{
				//	//float rad = dGeomSphereGetRadius(geom);
				//	//GLUquadric* q = gluNewQuadric(); 
				//	//gluSphere(q, rad, 8, 8); break; 
				//} 
			case dBoxClass:
			{

			}
			break;

			case dTriMeshClass:
			{
					// not supported yet
			}
			break;
			case dSphereClass:
			{
				float rad = dGeomSphereGetRadius(geom);
				
				glm::mat4 m = obj->GetEntity()->GetModelMatrix();
				m = glm::scale(m, glm::vec3(RadtoDeg(rad)));
		
				g_debugRender->DrawEllipse(m, Vec3(0.5f, 0.5f, 1.0f));
			}
			break;
	
			default: 
				printf(" UNKNOWN OBJECT CLASS!\n"); 
				break;
			}
		}
	}
}

dWorldID PhysicsWorld::GetWorld()
{
	return m_dynamicsWorld;
}

dSpaceID PhysicsWorld::GetSpace()
{
	return m_space;
}

void PhysicsWorld::AddObject(PhysicsObject* object)
{
	std::vector<PhysicsObject*>::iterator it = std::find(
		m_objects.begin(),
		m_objects.end(),
		object
	);

	assert(it == m_objects.end());

	m_objects.push_back(object);
}

void PhysicsWorld::RemoveObject(PhysicsObject* object)
{
	std::vector<PhysicsObject*>::iterator it = std::find(
		m_objects.begin(),
		m_objects.end(),
		object
	);

	assert(it != m_objects.end());

	m_objects.erase(it);
}

void PhysicsWorld::SetGravity(const Vec3& gravityVector)
{
	dWorldSetGravity(m_dynamicsWorld, gravityVector.x, gravityVector.y, gravityVector.z);
}

Vec3 PhysicsWorld::GetGravity()
{
	dVector3 g;
	dWorldGetGravity(m_dynamicsWorld, g);
	return Vec3(g[0], g[1], g[2]);
}

void PhysicsWorld::NearCollisionCallback(dGeomID id1, dGeomID id2)
{
}

void PhysicsInit()
{
	dInitODE2(0);

	g_PhysicsWorld = new PhysicsWorld();
	g_PhysicsWorld->Create();
}

void PhysicsShutdown()
{
	if (g_PhysicsWorld)
	{
		g_PhysicsWorld->Destroy();

		delete g_PhysicsWorld;
		g_PhysicsWorld = nullptr;
	}

	dCloseODE();
}
