#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 = true;
	entity->acceleration = (EntityAcceleration){
		.speedUp = 20.0,
		.speedDown = 15.0
	};

	// PID configs.
	PIDConfig speedPIDConfig = {
		.kP = 1,
		.kI = 0.0,
		.kD = 0.0,
		.angleMode = false,
		.doClamp = true,
		.min = 0.0,
		.max = 100.0
	};

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

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

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

	// Create fly to point.
	data->flyToPoint = (EntityFlyToPointInfo){
		//.controller.speedPID = createPID(speedPIDConfig),
		.controller.bangbang.speed = 20.0,
		.controller.bangbang.stopAt = 0.0,
		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
		.rotationSpeed = 0.0
	};
}

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

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

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

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

	entityFlyToPoint(
		entity,
		player->position,
		&data->flyToPoint
	);

	entityCheckTransformedCollisionModel(entity);
}

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