#include "maresciallo.h"
#include "assets.h"
#include "game.h"
#include "PID.h"

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

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

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

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

	PIDConfig flyAwayPID = (PIDConfig){
		.kP = 1.1,
		.kI = 0.0,
		.kD = 0.0,
		.angleMode = false,
		.doClamp = true,
		.min = 0.0,
		.max = 210.0
	};

	data->flyAway = (EntityFlyToPointInfo){
		.controller.speedPID = createPID(flyAwayPID),
		.controlType = ENTITY_FLY_TO_POINT_PID,
		.rotationSpeed = 0.0,
		.applyRotation = true
	};

}

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

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

void getAwayMaresciallo(Game * game, Entity * entity) {
	Maresciallo * data = (Maresciallo*)entity->data;
	Entity * player = getEntityFromWorld(game->world, 0);

	// Get target.
	Vector3 target = Vector3Subtract(entity->position, player->position);
	target = Vector3Normalize(target);
	target = Vector3Scale(target, MARESCIALLO_CIRCLE_AT_DIS);
	target = Vector3Add(player->position, target);

	// Fucking get away from the smelly player.
	entityFlyToPoint(entity, target, &data->flyAway);
}

void circlePlayerMaresciallo(Game * game, Entity * entity) {
	Maresciallo * data = (Maresciallo*)entity->data;
	Entity * player = getEntityFromWorld(game->world, 0);
}

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

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

	// Fly away if to close to player.
	if (Vector3Distance(entity->position, player->position) < MARESCIALLO_CIRCLE_AT_DIS) {
		getAwayMaresciallo(game, entity);
	} else {
		circlePlayerMaresciallo(game, entity);
	}

	entityCheckTransformedCollisionModel(entity);
}

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