#include "soldato.h"
#include "game.h"

void initSoldato(Entity * entity, Game * game) {
	entity->model = &game->assets.models[SOLDATO_ASSET];
	entity->collisionModel = entityCreateCollisionModel(*entity->model);
	entity->transformedCollisionModel = entityCreateCollisionModel(*entity->model);
	setEntityRadius(entity);

	// Acceleration.
	entity->useAcceleration = false;

	// Allocate data.
	entity->data = KF_MALLOC(sizeof(Soldato));

	if (entity->data == NULL) {
		ALLOCATION_ERROR;
		return;
	}

	Soldato * data = (Soldato*)entity->data;

	PIDConfig speedPID = (PIDConfig){
		.kP = 0.5,
		.kI = 0.0,
		.kD = 0.0,
		.angleMode = false,
		.doClamp = true,
		.min = 0.0,
		.max = 20.0
	};

	// Create fly to point.
	data->flyToPoint = (EntityFlyToPointInfo){
		.controller.bangbang.speed = 50.0,
		.controller.bangbang.stopAt = entity->radius * 2,
		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
		.rotationSpeed = 0.0
	};

	data->leaderId = ENTITY_NONE;
	data->followerId = ENTITY_NONE;
}

void closeSoldato(Entity * entity) {
	if (entity->data != NULL)
		KF_FREE(entity->data);

	entityFreeCollisionModel(entity->collisionModel);
	entityFreeCollisionModel(entity->transformedCollisionModel);
}

void soldatoFollowLeader(Game * game, Entity * entity) {
	Soldato * data = (Soldato*)entity->data;
	Entity * leader;

	// Get leader.
	leader = getEntityFromWorld(game->world, data->leaderId);

	// Bye bye leader.
	if (leader == NULL) {
		data->leaderId = ENTITY_NONE;
		return;
	}

	if (leader->fingerprint != data->leaderFingerprint) {
		data->leaderId = ENTITY_NONE;
		return;
	}

	// Fly to leader.
	entityFlyToPoint(
		entity,
		leader->position,
		&data->flyToPoint
	);
}

void handleFollower(Game * game, Entity * entity) {
	Soldato * data = (Soldato*)entity->data;
	Entity * follower;

	if (data->followerId == ENTITY_NONE)
		return;

	// Get follower.
	follower = getEntityFromWorld(game->world, data->followerId);

	// Bye bye follower.
	if (follower == NULL) {
		data->followerId = ENTITY_NONE;
		return;
	}

	if (follower->fingerprint != data->followerFingerprint) {
		data->followerId = ENTITY_NONE;
		return;
	}
}

void updateSoldato(Game * game, Entity * entity) {
	entityUpdateLastValues(entity);

	Entity * player = getEntityFromWorld(game->world, 0);
	Soldato * data = (Soldato*)entity->data;

	// Fly to player if no leader.
	if (data->leaderId == ENTITY_NONE) {
		entityFlyToPoint(
			entity,
			player->position,
			&data->flyToPoint
		);
	} else {
		soldatoFollowLeader(game, entity);
		handleFollower(game, entity);
	}

	entityCheckTransformedCollisionModel(entity);
}

void drawSoldato(Game * game, Entity * entity) {
	entityDraw(entity);

	Soldato * data = (Soldato*)entity->data;
	Entity * leader;

	// Debug line.
	//if (data->leaderId != ENTITY_NONE) {
	//	leader = getEntityFromWorld(game->world, data->leaderId);
	//	DrawLine3D(entity->position, leader->position, BLUE);
	//}
}

void setSoldatoLeader(Entity * entity1, Entity * entity2) {
	Soldato * data1;
	Soldato * data2;

	if (entity1->type != ENTITY_SOLDATO || entity2->type != ENTITY_SOLDATO)
		return;

	data1 = (Soldato*)entity1->data;
	data2 = (Soldato*)entity2->data;

	// Already has follower.
	if (data2->followerId != ENTITY_NONE)
		return;

	if (data1->leaderId == ENTITY_NONE) {
		data1->leaderId = entity2->id;
		data1->leaderFingerprint = entity2->fingerprint;

		data2->followerId = entity1->id;
		data2->followerFingerprint = entity1->fingerprint;
	}
}