#include "EntityBase.h"

#include "coll2d.h"

namespace Engine {

/** \brief Copy constructor */
inline EntityPhysicState::EntityPhysicState (const EntityPhysicState& state):
	mId (state.mId),
	mBaseType (state.mBaseType),
	mPosition (state.mPosition),
	mOrientation (state.mOrientation),
	mVelocity (state.mVelocity),
	mAngleVelocity (state.mAngleVelocity),
	mStatic (state.mStatic),
	mAlive (state.mAlive),
	mContactNormals(state.mContactNormals)
{
	mShape = state.mShape->getCopy();
};

/** \brief Assignment operator */
inline EntityPhysicState& EntityPhysicState::operator= (const EntityPhysicState& state) {
	if (this != &state) {
		mId = state.mId;
		mBaseType = state.mBaseType;
		mPosition = state.mPosition;
		mOrientation = state.mOrientation;
		mVelocity = state.mVelocity;
		mAngleVelocity = state.mAngleVelocity;
		mShape = state.mShape->getCopy();
		mStatic = state.mStatic;
		mAlive = state.mAlive;
		mContactNormals = state.mContactNormals;
	}

	return *this;
}

vector3d& EntityPhysicState::GetPosition () {
	return mPosition;
}

vector3d& EntityPhysicState::GetVelocity () {
	return mVelocity;
}

vector3d& EntityPhysicState::GetOrientation () {
	return mOrientation;
}

float& EntityPhysicState::GetAngleVelocity () {
	return mAngleVelocity;
}

void EntityPhysicState::SetPosition (const vector3d &position) {
	mPosition = position;
}

void EntityPhysicState::SetVelocity (const vector3d &velocity) {
	mVelocity = velocity;
}

void EntityPhysicState::SetOrientation (const vector3d &orientation) {
	mOrientation = orientation;
}

void EntityPhysicState::SetAngleVelocity (const float &angle_velocity) {
	mAngleVelocity = angle_velocity;
}

void EntityPhysicState::Globalize (vector3d &vec) {
	// make a copy of the local coordinates
	vector3d local (vec);

	// calculate sin and cos for the current rotation
	float sintheta, costheta;
	sintheta = sin (- mOrientation[1] * M_PI / 180);
	costheta = cos (- mOrientation[1] * M_PI / 180);

	// rotation
	vec[0] = - sintheta * local[2] + costheta * local[0];
	vec[1] = local[1];
	vec[2] = costheta * local[2] + sintheta * local[0] ;

	// and translation
	vec += mPosition;
}

void EntityPhysicState::Localize (vector3d &vec) {
	// translation
	vec -= mPosition;

	vector3d global (vec);

	// calculate sin and cos for the current rotation
	float sintheta, costheta;
	sintheta = sin (mOrientation[1] * M_PI / 180);
	costheta = cos (mOrientation[1] * M_PI / 180);

	// rotation
	vec[0] = - sintheta * global[2] + costheta * global[0];
	vec[1] = global[1];
	vec[2] = costheta * global[2] + sintheta * global[0] ;
}

void EntityPhysicState::GlobalizeRotation (vector3d &vec) {
	// make a copy of the local coordinates
	vector3d local (vec);

	// calculate sin and cos for the current rotation
	float sintheta, costheta;
	sintheta = sin (- mOrientation[1] * M_PI / 180);
	costheta = cos (- mOrientation[1] * M_PI / 180);

	// rotation
	vec[0] = - sintheta * local[2] + costheta * local[0];
	vec[1] = local[1];
	vec[2] = costheta * local[2] + sintheta * local[0] ;
}

void EntityPhysicState::LocalizeRotation (vector3d &vec) {
	vector3d global (vec);

	// calculate sin and cos for the current rotation
	float sintheta, costheta;
	sintheta = sin (mOrientation[1] * M_PI / 180);
	costheta = cos (mOrientation[1] * M_PI / 180);

	// rotation
	vec[0] = - sintheta * global[2] + costheta * global[0];
	vec[1] = global[1];
	vec[2] = costheta * global[2] + sintheta * global[0] ;
}


void EntityPhysicState::UpdateShape () {
	assert (mShape);

	mShape->setPosition (mPosition);
	mShape->setAngle (mOrientation[1] * M_PI / 180);

	mShape->setVelocity (mVelocity);
	mShape->setAngleVelocity (mAngleVelocity);
}


}