#include "Salamander.h"
#include "Body.h"

#include "Shape.h"
#include "World.h"

namespace Salamander
{

Body::Body() : Entity(),
	friction_(0.5f),
	mass_(0.0f),
	inverseMass_(DBL_MAX),
	shape_(NULL),
	world_(NULL)
{
	resetPhysics();
}

Body::~Body()
{
	if(shape_)
	{
		delete shape_;
		shape_ = NULL;
	}
}

const UniqueId& Body::shapeId()
{
	return shape_->typeId();
}

void Body::resetPhysics()
{
	force_ = Vec2(0.0f, 0.0f);
	velocity_ = Vec2(0.0f, 0.0f);
	biasedVelocity_ = Vec2(0.0f, 0.0f);

	torque_ = 0.0f;
	angularVelocity_ = 0.0f;
	biasedAngularVelocity_ = 0.0f;

	massMomentOfInertia_ = DBL_MAX;
	inverseMassMomentOfInertia_ = 0.0f;
}

void Body::setShape(Shape* shape)
{
	shape_ = shape;
	//shape_->postCalculateProperties
	//TODO PHYS : update shape & body data? (COM)
}

Shape* Body::getShape()
{
	return shape_;
}

	//rotationlocal_ = rotationworld_ - composite_->rotationworld_;
	//translationlocal_ = Mat22(composite_->translationworld_, composite_->rotationworld_).Inverse() * translationworld_;

void Body::setFriction(double friction)
{
	friction_ = friction;
}

double Body::getFriction()
{
	return friction_;
}

void Body::setMass(double mass)
{
	mass_ = mass;
	updateMass();
}

double Body::getMass()
{
	return mass_;
}

void Body::updateMass()
{
	// update the mass figures for easier calculations
	if (mass_ < DBL_MAX && mass_ != 0.0f)
	{
		inverseMass_ = 1.0f / mass_;
		if(shape_)
		{
			massMomentOfInertia_ = mass_ * shape_->momentOfInertia();
			inverseMassMomentOfInertia_ = 1.0f / massMomentOfInertia_;
		}
	}
	else
	{
		// non-moving object
		inverseMass_ = 0.0f;
		massMomentOfInertia_ = DBL_MAX;
		inverseMassMomentOfInertia_ = 0.0f;
	}
}

World* Body::world()
{
	return world_;
}

void Body::applyForce(Vec2 force)
{
	force_ += force;
}

void Body::applyTorque(double torque)
{
	torque_ += torque;
}

// TODO PHYS: this should reflect reference type
void Body::applyLocalForce(Vec2 location, Vec2 force)
{
	//force_ += Mat22(rotationworld_) * force;
	//torque_ += location ^ force;
}

void Body::applyWorldForce(Vec2 location, Vec2 force)
{
	//force_ += force;
	//torque_ += (Mat22(this->rotationworld_).Inverse() * (location - translationworld_)) ^ force;
}

void Body::updateMatrix()
{
	// update the vertices/etc in the shape
	shape_->applyMatrix(getTransform());
}

void Body::integrateForces(double ms)
{
	// don't do anything if it's got no weight
	if(inverseMass_ == 0.0f)
		return;
	
	velocity_ += (World::gravity_ + force_ * inverseMass_) * ms;
	angularVelocity_ += inverseMassMomentOfInertia_ * torque_ * ms;

	// Bias velocities are reset to zero each step.
	biasedVelocity_ = Vec2(0.0f, 0.0f);
	biasedAngularVelocity_ = 0.0f;
}

void Body::integrateVelocities(double ms)
{
	setTranslation(getTranslation() + (velocity_ + biasedVelocity_) * ms);
	setRotation(getRotation() + (angularVelocity_ + biasedAngularVelocity_) * ms);

	force_ = Vec2(0.0f, 0.0f);
	torque_ = 0.0f;
}

// TODO PHYS: when does this get used?
void Body::applyMatrix(Mat2 matrix)
{
	shape_->applyMatrix(matrix);
}

void Body::clearMatrix()
{
	shape_->clearMatrix();
}

double Body::radius()
{
	return shape_->radius();
}

double Body::radiusSquared()
{
	return shape_->radiusSquared();
}

bool Body::isPointInside(Vec2 point)
{
	return shape_->isPointInside(point);
}

Pick Body::getRayIntersection(Vec2 location, Vec2 direction)
{
	Pick pick = shape_->getRayIntersection(location, direction);
	if(pick.distance > 0.0f) pick.body = this;

	return pick;
}

}