#include "utils/maths.h"
#include "physics_object.h"
#include "physics_world.h"
#include "physics_render.h"
#include "game/game_object.h"

#include <ode/collision_trimesh.h>

#include <vector>

PhysicsObject::PhysicsObject() :
	m_pCollisionShape(nullptr), m_pRigidBody(nullptr), m_pTriMesh(nullptr),
	m_pEntity(nullptr),
	m_Friction(0.5f), m_Gravity(0.0f), m_LinearVelocity(0.0f)
{
}

PhysicsObject::~PhysicsObject()
{
}

void PhysicsObject::CreateTriMesh(const Vec3* vertices, const int verticesCount, const uint32_t* indices, const int indicesCount)
{
	m_pRigidBody = dBodyCreate(g_PhysicsWorld->GetWorld());
	dBodySetPosition(m_pRigidBody, 0, 0, 0);

	dMass m;
	dMassSetBox(&m, 1, 10.0f, 10.0f, 10.0f);
	dBodySetMass(m_pRigidBody, &m);

	// create triangle mesh
	m_pTriMesh = dGeomTriMeshDataCreate();

	// collect data
	std::vector<dReal> odevertices;
	int j = 0;
	for (int i = 0; i < verticesCount; i++)
	{
		if (j > 2)
			j = 0;

		if (j == 0)
			odevertices.push_back(vertices[i].x);
		if (j == 1)
			odevertices.push_back(vertices[i].x);
		if (j == 2)
			odevertices.push_back(vertices[i].x);

		j++;
	}

	dGeomTriMeshDataBuildSimple(m_pTriMesh, odevertices.data(), odevertices.size(), (const dTriIndex*)indices, indicesCount);
	m_pCollisionShape = dCreateTriMesh(g_PhysicsWorld->GetSpace(), m_pTriMesh, NULL, NULL, NULL);
	dGeomSetBody(m_pCollisionShape, m_pRigidBody);
}

void PhysicsObject::CreatePlayer()
{
	m_pRigidBody = dBodyCreate(g_PhysicsWorld->GetWorld());
	dBodySetPosition(m_pRigidBody, 0, 0, 0);
	dBodySetKinematic(m_pRigidBody);
	dBodySetAutoDisableFlag(m_pRigidBody, 0); // disable body sleeping

	dMass m;
	dMassSetBox(&m, 1, 10.0f, 10.0f, 10.0f);
	dBodySetMass(m_pRigidBody, &m);

	m_pCollisionShape = dCreateBox(g_PhysicsWorld->GetSpace(), 0.5f, 0.5f, 0.5f);
	dGeomSetBody(m_pCollisionShape, m_pRigidBody);
}

void PhysicsObject::CreateSimpleBox()
{
	m_pRigidBody = dBodyCreate(g_PhysicsWorld->GetWorld());
	dBodySetPosition(m_pRigidBody, 0, 0, 0);

	dMass m;
	dMassSetBox(&m, 1, 10.0f, 10.0f, 10.0f);
	dBodySetMass(m_pRigidBody, &m);

	m_pCollisionShape = dCreateBox(g_PhysicsWorld->GetSpace(), 0.5f, 0.5f, 0.5f);
	dGeomSetBody(m_pCollisionShape, m_pRigidBody);
}

void PhysicsObject::CreateSimpleBox_Kinematic()
{
	m_pRigidBody = dBodyCreate(g_PhysicsWorld->GetWorld());
	dBodySetPosition(m_pRigidBody, 0, 0, 0);
	dBodySetKinematic(m_pRigidBody);

	dMass m;
	dMassSetBox(&m, 1, 10.0f, 10.0f, 10.0f);
	dBodySetMass(m_pRigidBody, &m);

	m_pCollisionShape = dCreateBox(g_PhysicsWorld->GetSpace(), 0.5f, 0.5f, 0.5f);
	dGeomSetBody(m_pCollisionShape, m_pRigidBody);
}

void PhysicsObject::Destroy()
{
	g_PhysicsWorld->RemoveObject(this);

	dGeomTriMeshDataDestroy(m_pTriMesh);
	dGeomDestroy(m_pCollisionShape);
	dBodyDestroy(m_pRigidBody);
}

void PhysicsObject::SyncWithEntity()
{
	if (!m_pEntity)
		return;

	Vec3 position = m_pEntity->m_position;
	dBodySetPosition(m_pRigidBody, position.x, position.y, position.z);
}

void PhysicsObject::SyncEntity()
{
	if (!m_pEntity)
		return;

	const dReal* pPos = dBodyGetPosition(m_pRigidBody);
	m_pEntity->m_position.x = pPos[0];
	m_pEntity->m_position.y = pPos[1];
	m_pEntity->m_position.z = pPos[2];
}

float PhysicsObject::GetFriction() const
{
	return m_Friction;
}

void PhysicsObject::SetFriction(float friction)
{
	m_Friction = friction;
}

void PhysicsObject::SetLinearVelocity(const Vec3& vel)
{
	m_LinearVelocity = vel;
	dBodySetLinearVel(m_pRigidBody, vel.x, vel.y, vel.z);
}

Vec3 PhysicsObject::GetLinearVelocity() const
{
	const dReal* vel = dBodyGetLinearVel(m_pRigidBody);
	return Vec3( vel[0], vel[1], vel[2] );
}

void PhysicsObject::SetGravity(const Vec3& gravityVector)
{
	m_Gravity = gravityVector;
}

Vec3 PhysicsObject::GetGravity()
{
	return m_Gravity;
}
