#include "entity.h"

// Entities.
#include "entities/antifaShip.h"
#include "entities/soldato.h"
#include "entities/caporale.h"
#include "entities/sergente.h"
#include "entities/maresciallo.h"
#include "entities/generale.h"
#include "entities/mussolini.h"

// This fucker is used for creating entities.
const EntityTypeInfo entityTypeInfo[ENTITY_TYPE_COUNT] = {
	(EntityTypeInfo){initAntifaShip, closeAntifaShip, updateAntifaShip, drawAntifaShip},
	(EntityTypeInfo){initSoldato, closeSoldato, updateSoldato, drawSoldato},
	(EntityTypeInfo){initCaporale, closeCaporale, updateCaporale, drawCaporale},
	(EntityTypeInfo){initSergente, closeSergente, updateSergente, drawSergente},
	(EntityTypeInfo){initMaresciallo, closeMaresciallo, updateMaresciallo, drawMaresciallo},
	(EntityTypeInfo){initGenerale, closeGenerale, updateGenerale, drawGenerale},
	(EntityTypeInfo){initMussolini, closeMussolini, updateMussolini, drawMussolini}
};

EntityVelocity entityVelocityIdentity() {
	return (EntityVelocity){
		.velocity = Vector3Zero(),
		.angularVelocity = AxisAngleIdentity(),
		.stick = Vector3Zero(),
		.speed = 0
	};
}

float accelerateValue(float value, float lastValue, float up, float down) {
	if (value - lastValue >= up)
		return lastValue + up;
	if (lastValue - value >= down)
		return lastValue - down;

	return value;
}

Vector3 accelerateVector3(Vector3 value, Vector3 lastValue, Vector3 up, Vector3 down) {
	return (Vector3){
		accelerateValue(value.x, lastValue.x, up.x, down.x),
		accelerateValue(value.y, lastValue.y, up.y, down.y),
		accelerateValue(value.z, lastValue.z, up.z, down.z)
	};
}

Entity createEntity(EntityType type, Game * game) {
	EntityTypeInfo info = entityTypeInfo[type];

	// Set defaults.
	Entity entity = (Entity){
		.type = type,
		.model = NULL,
		.radius = 0.0,
		.position = Vector3Zero(),
		.rotation = QuaternionIdentity(),
		.velocity = entityVelocityIdentity(),
		.lastVelocity = entityVelocityIdentity(),
		.useAcceleration = false,
		.updateCb = info.updateCb,
		.drawCb = info.drawCb,
		.health = ENTITY_MAX_HEALTH,
		.collision.hit = false,
		.data = NULL
	};

	// Init.
	info.initCb(&entity, game);

	return entity;
}

void closeEntity(Entity * entity) {
	entityTypeInfo[entity->type].closeCb(entity);
}

void setEntityRadius(Entity * entity) {
	int i, j;
	Mesh mesh;
	Vector3 v;
	float farthest = 0.0;

	// Loop through meshes.
	for (i = 0; i < entity->model->meshCount; ++i) {
		mesh = entity->model->meshes[i];

		// Loop though vertices.
		for (j = 0; j < mesh.vertexCount; ++j) {
			v = (Vector3){
				mesh.vertices[j * 3],
				mesh.vertices[j * 3 + 1],
				mesh.vertices[j * 3 + 2]
			};

			farthest = fmaxf(farthest, Vector3Length(v));
		}
	}

	entity->radius = farthest;
}

// Little triangle helper for checkEntityMeshCollision.
void getTriangleFromMeshAndTransform(int num, Mesh mesh, Vector3 * triangle, Quaternion rotation, Vector3 position) {
	int i;
	int triangleLocation = num * 9;
	int vertexLocation;

	for (i = 0; i < 3; ++i) {
		vertexLocation = (i * 3) + triangleLocation;

		// Get vertex.
		triangle[i] = (Vector3){
			mesh.vertices[vertexLocation],
			mesh.vertices[vertexLocation + 1],
			mesh.vertices[vertexLocation + 2],
		};

		// Transform vertex.
		triangle[i] = Vector3RotateByQuaternion(triangle[i], rotation);
		triangle[i] = Vector3Add(triangle[i], position);
	}
}

// Big mesh helper for checkEntityCollision.
bool checkEntityMeshCollision(Entity entity1, Entity entity2, int entity1MeshNum, int entity2MeshNum) {
	int triangle1Num;
	int triangle2Num;
	bool collided;

	Vector3 triangle1[3];
	Vector3 triangle2[3];

	Mesh mesh1 = entity1.model->meshes[entity1MeshNum];
	Mesh mesh2 = entity2.model->meshes[entity2MeshNum];

	Vector3 mesh2Triangles[mesh2.triangleCount][3];

	// Get mesh2Triangles.
	for (triangle2Num = 0; triangle2Num < mesh2.triangleCount; ++triangle2Num) {
		getTriangleFromMeshAndTransform(
			triangle2Num,
			mesh2,
			mesh2Triangles[triangle2Num],
			entity2.rotation,
			entity2.position
		);
	}

	// Test every triangle for collision.
	for (triangle1Num = 0; triangle1Num < mesh1.triangleCount; ++triangle1Num) {
		getTriangleFromMeshAndTransform(
			triangle1Num,
			mesh1,
			triangle1,
			entity1.rotation,
			entity1.position
		);

		for (triangle2Num = 0; triangle2Num < mesh2.triangleCount; ++triangle2Num) {
			triangle2[0] = mesh2Triangles[triangle2Num][0];
			triangle2[1] = mesh2Triangles[triangle2Num][1];
			triangle2[2] = mesh2Triangles[triangle2Num][2];

			// Get normals.
			Vector3 normal1 = (Vector3){
				mesh1.normals[triangle1Num * 3],
				mesh1.normals[triangle1Num * 3 + 1],
				mesh1.normals[triangle1Num * 3 + 2]
			};
			
			Vector3 normal2 = (Vector3){
				mesh2.normals[triangle2Num * 3],
				mesh2.normals[triangle2Num * 3 + 1],
				mesh2.normals[triangle2Num * 3 + 2]
			};

			// Check collision.
			collided = checkTriangleCollision3D(
				triangle1,
				triangle2,
				normal1,
				normal2
			);

			if (collided)
				return true;
		}
	}

	return false;
}

bool checkEntityCollision(Entity entity1, Entity entity2) {
	int i, j;

	// Failed quick check.
	if (Vector3Distance(entity1.position, entity2.position) > entity1.radius + entity2.radius)
		return false;

	// Loop through every mesh and check.
	for (i = 0; i < entity1.model->meshCount; ++i)
		for (j = 0; j < entity2.model->meshCount; ++j)
			if (checkEntityMeshCollision(entity1, entity2, i, j))
				return true;

	return false;
}

// Basic wireframe drawing.
void entityDraw(Entity * entity) {
	entity->model->transform = QuaternionToMatrix(entity->rotation);

	DrawModelWires(
		*entity->model,
		entity->position,
		1.0,
		GREEN
	);
}

void entityUpdatePosition(Entity * entity) {
	float t = GetFrameTime();

	Vector3 velocity = (Vector3){
		entity->velocity.velocity.x * t,
		entity->velocity.velocity.y * t,
		entity->velocity.velocity.z * t
	};

	entity->position = Vector3Add(entity->position, velocity);
}

void entityUpdateRotation(Entity * entity) {
	float t = GetFrameTime();

    Quaternion angularRotation = QuaternionFromAxisAngle(
		entity->velocity.angularVelocity.axis, 
		entity->velocity.angularVelocity.angle * t
	);

    entity->rotation = QuaternionMultiply(entity->rotation, angularRotation);
	entity->rotation = QuaternionNormalize(entity->rotation);
}

void entityJoystickControl(Entity * entity, Vector3 stick, float speed) {
	float s = speed;
	Vector3 st = stick;
	float t = GetFrameTime();

	// Handle acceleration.
	if (entity->useAcceleration) {
		s = accelerateValue(
			speed,
			entity->lastVelocity.speed,
			entity->acceleration.speedUp * t,
			entity->acceleration.speedDown * t
		);

		st = accelerateVector3(
			stick,
			entity->lastVelocity.stick,
			Vector3Scale(entity->acceleration.rotation, t),
			Vector3Scale(entity->acceleration.rotation, t)
		);
	}

	entity->velocity.stick = st;
	entity->velocity.speed = s;
	entity->lastVelocity = entity->velocity;

	// Set angular velocity.
	Vector3 angularVelocity = Vector3Scale(st, PI);
	entity->velocity.angularVelocity.angle = Vector3Length(angularVelocity);
	entity->velocity.angularVelocity.axis = st;

	entityUpdateRotation(entity);

	// Set position.
	Matrix m = QuaternionToMatrix(QuaternionInvert(entity->rotation));

	entity->velocity.velocity = (Vector3){
		m.m2 * s,
		m.m6 * s,
		m.m10 * s
	};

	entityUpdatePosition(entity);
}

void entityFlyToPoint(Entity * entity, Vector3 point, EntityFlyToPointInfo * info) {
	float t = GetFrameTime();

	// Get distance and direction.
	Vector3 dis = Vector3Subtract(entity->position, point);
	Vector3 direction = Vector3Normalize(dis);

	// Get look at and rotation.
	Matrix matrix = MatrixLookAt(Vector3Zero(), direction, (Vector3){0.0, 1.0, 0.0});
	Quaternion rotation = QuaternionInvert(QuaternionFromMatrix(matrix));

	// Rotate this fucker.
	if (info->rotationSpeed == 0.0)
		entity->rotation = rotation;
	else
		entity->rotation = QuaternionSlerp(entity->rotation, rotation, t * info->rotationSpeed);

	// Velocity control.
	float speed = 0.0;

	float distance = Vector3Length(dis);

	switch (info->controlType) {
		case ENTITY_FLY_TO_POINT_PID:
			speed = runPID(0.0, -distance, &info->controller.speedPID);
			break;
		case ENTITY_FLY_TO_POINT_BANG_BANG:
			speed = info->controller.bangbang.speed;

			if (distance <= info->controller.bangbang.stopAt)
				speed = 0.0;

			break;
		default: // Something is fucked up.
			break;
	}

	Matrix m = QuaternionToMatrix(QuaternionInvert(entity->rotation));

	// Accelerate.
	if (entity->useAcceleration)
		speed = accelerateValue(
			speed,
			entity->lastVelocity.speed,
			entity->acceleration.speedUp * t,
			entity->acceleration.speedDown * t
		);

	// Velocity.
	entity->velocity.velocity = (Vector3){
		m.m2 * speed,
		m.m6 * speed,
		m.m10 * speed
	};

	entityUpdatePosition(entity);

	entity->velocity.speed = speed;
	entity->lastVelocity = entity->velocity;
}