#include "soldato.h"
#include "game.h"
#include "entity.h"
#include "world.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);

	entity->health = 0.125;

	// Acceleration.
	entity->useAcceleration = false;

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

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

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

	data->timeSinceLastShot = GetTime();

	PIDConfig followingPID = (PIDConfig){
		.kP = 1.0,
		.kI = 0.0,
		.kD = 0.0,
		.angleMode = false,
		.doClamp = true,
		.min = 0.0,
		.max = 80.0
	};

	// Create fly to point.
	data->flyToPointLeading = (EntityFlyToPointInfo){
		.controller.bangbang.speed = 80.0,
		.controller.bangbang.stopAt = 0.0,
		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
		.rotationSpeed = SOLDATO_ROTATION_SPEED,
		.applyRotation = true
	};

	data->flyToPointFollowing = (EntityFlyToPointInfo){
		.controller.speedPID = createPID(followingPID),
		.controlType = ENTITY_FLY_TO_POINT_PID,
		.rotationSpeed = SOLDATO_ROTATION_SPEED,
		.applyRotation = true
	};

	data->gunTarget = Vector3One();
}

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, entity->follow.leaderId);

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

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

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

void handleFollower(Game * game, Entity * entity) {
	Entity * follower;

	if (entity->follow.followerId == ENTITY_NONE)
		return;

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

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

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

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

	// Gun target shit.
	Vector3 target = Vector3Normalize(Vector3Subtract(player->position, entity->position));
	data->gunTarget = Vector3Lerp(data->gunTarget, target, GetFrameTime() * SOLDATO_GUN_TARGETING_SPEED);
 
	// Needs more time.
	if (t - data->timeSinceLastShot < SOLDATO_COOLDOWN)
		return;

	// Shoot this fucker.
	Bullet bullet = createBulletFromDirection(*entity, data->gunTarget, SOLDATO_BULLET_DAMAGE);
	BulletHitInfo hit = shootBulletAtEntity(player, bullet);

	// if (hit.hit)
	// 	printf("This fucker hit %lf\n", t);

	data->timeSinceLastShot = t;
}

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 (entity->follow.leaderId == ENTITY_NONE) {
		entityFlyToPoint(entity, player->position, &data->flyToPointLeading);
	} else {
		soldatoFollowLeader(game, entity);
		handleFollower(game, entity);
	}

	updateSoldatoGuns(game, entity);

	entityCheckTransformedCollisionModel(entity);
}

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

	// Debug gun.
	/*
	Soldato * data = (Soldato*)entity->data;
	DrawLine3D(
		entity->position,
		Vector3Add(entity->position, Vector3Scale(data->gunTarget, SOLDATO_GUN_MAX_RANGE)),
		BLUE
	);
	*/

	/*
	Entity * leader;

	// Debug line.
	if (entity->follow.leaderId != ENTITY_NONE) {
		leader = getEntityFromWorld(game->world, entity->follow.leaderId);

		if (leader == NULL)
			return;

		DrawLine3D(entity->position, leader->position, BLUE);
	}
	*/
}

void setSoldatoLeader(Entity * entity1, Entity * entity2) {
	Entity * follower;
	Entity * leader;

	// Who is follower and leader.
	if (entity1->type == ENTITY_SOLDATO && entity2->type == ENTITY_SOLDATO) {
		if (entity1->follow.leaderId == ENTITY_NONE) {
			follower = entity1;
			leader = entity2;
		} else {
			follower = entity2;
			leader = entity1;
		}
	} else if (entity1->type == ENTITY_SOLDATO) {
		follower = entity1;
		leader = entity2;
	} else if (entity2->type == ENTITY_SOLDATO) {
		follower = entity2;
		leader = entity1;
	} else {
		return;
	}

	// Already has follower.
	if (leader->follow.followerId != ENTITY_NONE)
		return;
	// Already has leader.
	if (follower->follow.leaderId != ENTITY_NONE)
		return;
	// Stops funny thingy.
	if (follower->follow.followerFingerprint == leader->fingerprint
		|| leader->follow.leaderFingerprint == follower->fingerprint)
		return;

	follower->follow.leaderId = leader->id;
	follower->follow.leaderFingerprint = leader->fingerprint;

	leader->follow.followerId = follower->id;
	leader->follow.followerFingerprint = follower->fingerprint;
}