5 files changed, 78 insertions, 5 deletions

diff --git a/src/entities/caporale.c b/src/entities/caporale.c
index bfdc610..32f0f20 100644
--- a/src/entities/caporale.c
+++ b/src/entities/caporale.c
@@ -23,7 +23,7 @@ void initCaporale(Entity * entity, Game * game) {
 		.controller.bangbang.stopAt = 0.0,
 		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
 		.rotationSpeed = 0.5,
-		.applyRotation = false
+		.applyRotation = true
 	};
 }
 
diff --git a/src/entities/maresciallo.c b/src/entities/maresciallo.c
index 6127786..f447819 100644
--- a/src/entities/maresciallo.c
+++ b/src/entities/maresciallo.c
@@ -1,18 +1,84 @@
 #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);
-	entity->velocity.angularVelocity = (AxisAngle){(Vector3){1.0, 1.0, 1.0}, 1.0};
+
+	// 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) {
-	entityUpdateRotation(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) {
diff --git a/src/entities/maresciallo.h b/src/entities/maresciallo.h
index 51714df..7492751 100644
--- a/src/entities/maresciallo.h
+++ b/src/entities/maresciallo.h
@@ -4,6 +4,12 @@
 #ifndef MARESCIALLO_H
 #define MARESCIALLO_H
 
+#define MARESCIALLO_CIRCLE_AT_DIS 200.0
+
+typedef struct Maresciallo {
+	EntityFlyToPointInfo flyAway;
+} Maresciallo;
+
 void initMaresciallo(Entity * entity, Game * game);
 void closeMaresciallo(Entity * entity);
 void updateMaresciallo(Game * game, Entity * entity);
diff --git a/src/entities/sergente.c b/src/entities/sergente.c
index 4830e1c..0a50956 100644
--- a/src/entities/sergente.c
+++ b/src/entities/sergente.c
@@ -22,7 +22,8 @@ void initSergente(Entity * entity, Game * game) {
 		.controller.bangbang.speed = 85.0,
 		.controller.bangbang.stopAt = 0.0,
 		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
-		.rotationSpeed = 0.0
+		.rotationSpeed = 0.0,
+		.applyRotation = false
 	};
 
 	createSergenteTarget(game, entity);
diff --git a/src/entities/sergente.h b/src/entities/sergente.h
index 9ef4e0f..cc79c9a 100644
--- a/src/entities/sergente.h
+++ b/src/entities/sergente.h
@@ -9,7 +9,7 @@
 #define SERGENTE_NEXT_POINT_THRESHOLD 0.1
 #define SERGENTE_COME_BACK_DIS 200.0
 #define SERGENTE_COME_BACK_PERCENT 0.5
-#define SERGENTE_ROTATION_SPEED 40.0
+#define SERGENTE_ROTATION_SPEED 20.0
 
 typedef struct Sergente {
 	EntityFlyToPointInfo flyToPoint;