#include "caporale.h"
#include "assets.h"
#include "game.h"
#include <raylib.h>
#include <raymath.h>

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

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

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

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

	data->flyToPlayer = (EntityFlyToPointInfo){
		.controller.bangbang.speed = 80,
		.controller.bangbang.stopAt = 0.0,
		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
		.rotationSpeed = 0.5,
		.applyRotation = true
	};

	data->timeSinceLastShot = 0.0;

	PIDConfig aimPIDConfig = (PIDConfig){
		.kP = 2.0,
		.kI = 0.0,
		.kD = 0.0,
		.angleMode = false,
		.doClamp = false,
		.min = 0.0,
		.max = 0.0
	};

	data->aimXPID = createPID(aimPIDConfig);
	data->aimYPID = createPID(aimPIDConfig);
	data->aimZPID = createPID(aimPIDConfig);
	
	data->initTarget = true;
}

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

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

void updateGunsCaporale(Game * game, Entity * entity) {
	double t = GetTime();
	Caporale * data = (Caporale*)entity->data;
	Entity * player = getEntityFromWorld(game->world, 0);

	DrawRay(data->ray, BLUE);

	// Get target.
	Vector3 target = Vector3Subtract(player->position, entity->position);
	target = Vector3Normalize(target);
	target = Vector3Scale(target, CAPORALE_TARGET_POINTER_DIS);
	data->targetSetpoint = target;

	// Cool down.
	if (t - data->timeSinceLastShot < CAPORALE_COOLDOWN)
		return;

	// Get direction.	
	target = Vector3Add(entity->position, data->target); // Transform to entity.
	Vector3 direction = Vector3Subtract(target, entity->position);
	direction = Vector3Normalize(direction);

	// Create bullet and shoot.
	Bullet bullet = createBulletFromDirection(*entity, direction, CAPORALE_BULLET_DAMAGE);
	BulletHitInfo info =  shootBulletAtEntity(player, bullet);

	printf("%d\n", info.hit);

	data->ray = bullet.ray;
	data->timeSinceLastShot = t;
}

void updateAimCaporale(Game * game, Entity * entity) {
	float t = GetFrameTime();
	Caporale * data = (Caporale*)entity->data;
	Entity * player = getEntityFromWorld(game->world, 0);

	// Init.
	if (data->initTarget) {
		data->initTarget = false;
		data->target = Vector3Zero();
		data->targetVelocity = Vector3Zero();
		return;
	}

	// Move target closer to player.
	data->targetVelocity = (Vector3){
		runPID(data->targetSetpoint.x, data->target.x, &data->aimXPID),
		runPID(data->targetSetpoint.y, data->target.y, &data->aimYPID),
		runPID(data->targetSetpoint.z, data->target.z, &data->aimZPID)
	};

	data->target = Vector3Add(data->target, Vector3Scale(data->targetVelocity, t));

	// debug.
	Vector3 target = Vector3Add(entity->position, data->target);
	DrawCubeV(target, Vector3One(), BLUE);
	DrawLine3D(entity->position, target, BLUE);
}

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

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

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

	entityCheckTransformedCollisionModel(entity);
}

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

	updateAimCaporale(game, entity);
	updateGunsCaporale(game, entity);
}