#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;

	data->timeSinceLastShot = GetTime();

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

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

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

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;
 
	// Needs more time.
	if (t - data->timeSinceLastShot < SOLDATO_COOLDOWN)
		return;

	Bullet bullet = createBulletFromEntity(*entity, SOLDATO_BULLET_DAMAGE);
	shootBulletAtEntity(player, bullet);
	data->timeSinceLastShot = t;

	/*
	// See if ray hits player radius.
	Ray ray = (Ray){
		entity->position,
		Vector3RotateByQuaternion((Vector3){0.0, 0.0, 1.0}, entity->rotation)
	};

	RayCollision collision = traceRayToEntityRadius(*player, ray, SOLDATO_AIM_RADIOUS);

	// Shoots bullet is ray hits.
	if (collision.hit) {
		shootBulletAtEntity(player, createBulletFromEntity(*entity, SOLDATO_BULLET_DAMAGE));
		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);

	/*
	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;

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

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