#include <UnitTest++.h>

#include "Logging.h"
#include "EntityBase.h"

using namespace std;
using namespace Engine;

struct EntityFixture {
	EntityFixture () {
		LoggingModule.Init (0, NULL);
		LoggingModule.SetLogPrintLevel (LogLevelWarning);
	}

	~EntityFixture () {
		LoggingModule.Destroy ();
	}

	Logging LoggingModule;
};

TEST_FIXTURE ( EntityFixture, EntityPhysicStateGlobalizeTranslation ) {
	EntityPhysicState phys_entity;

	vector3d position (1, 0, -2);
	vector3d orientation (0, 0.25 * M_PI, 0);

	phys_entity.SetPosition (position);
	phys_entity.SetOrientation (orientation);

	vector3d origin (0, 0, 0);
	phys_entity.Globalize (origin);

	CHECK_EQUAL (origin[0], 1);
	CHECK_EQUAL (origin[1], 0);
	CHECK_EQUAL (origin[2], -2);
}

TEST_FIXTURE ( EntityFixture, EntityPhysicStateGlobalizeFull ) {
	EntityPhysicState phys_entity;

	vector3d position (3, 0, 1);
	vector3d orientation (0, 45, 0);

	phys_entity.SetPosition (position);
	phys_entity.SetOrientation (orientation);

	vector3d front (5, 0, 0);
	phys_entity.Globalize (front);

	CHECK_CLOSE (3 + cos (45 * M_PI / 180) * 5., front[0], 1.0e-5);
	CHECK_CLOSE (0, front[1], 1.0e-5);
	CHECK_CLOSE (1 - sin (45 * M_PI / 180) * 5., front[2], 1.0e-5);
}

TEST_FIXTURE ( EntityFixture, EntityPhysicStateLocalizeTranslation ) {
	EntityPhysicState phys_entity;

	vector3d position (1, 0, -2);
	vector3d orientation (0, 45, 0);

	phys_entity.SetPosition (position);
	phys_entity.SetOrientation (orientation);

	vector3d origin (1., 0, -2.);
	phys_entity.Localize (origin);

	CHECK_CLOSE (0, origin[0], 1.0e-5);
	CHECK_CLOSE (0, origin[1], 1.0e-5);
	CHECK_CLOSE (0, origin[2], 1.0e-5);
}

TEST_FIXTURE ( EntityFixture, EntityPhysicStateLocalizeFull ) {
	EntityPhysicState phys_entity;

	vector3d position (1, 0, -1);
	vector3d orientation (0, 45, 0);

	phys_entity.SetPosition (position);
	phys_entity.SetOrientation (orientation);

	vector3d origin (0, 0, 0);
	phys_entity.Localize (origin);

	CHECK_CLOSE (-sqrt (2), origin[0], 1.0e-5);
	CHECK_CLOSE (0, origin[1], 1.0e-5);
	CHECK_CLOSE (0, origin[2], 1.0e-5);
}