From d8c7bb70740246d3d7b5e9d11e73ccda7e498f59 Mon Sep 17 00:00:00 2001
From: Klemen Plestenjak <plestenjak.klemen@gmail.com>
Date: Tue, 13 Feb 2024 14:40:44 +0100
Subject: [PATCH] Improve harvest target assigment (asign closest first)

---
 game/systems/s_input.c | 69 ++++++++++++++++++++++++++++++------------
 1 file changed, 49 insertions(+), 20 deletions(-)

diff --git a/game/systems/s_input.c b/game/systems/s_input.c
index a164dae..5d6050c 100644
--- a/game/systems/s_input.c
+++ b/game/systems/s_input.c
@@ -96,6 +96,19 @@ static int entityPosPairCmpZero(const void *lhsData, const void *rhsData) {
 
     return vec2CmpZero(&lhs->pos, &rhs->pos);
 }
+typedef struct EntityFloatPair {
+    ecs_entity_t entity;
+    f32 value;
+} EntityFloatPair;
+static int entityFloatPairCmp(const void *lhsData, const void *rhsData) {
+    const EntityFloatPair *lhs = lhsData;
+    const EntityFloatPair *rhs = rhsData;
+
+    if (lhs->value < rhs->value) return -1;
+    if (lhs->value > rhs->value) return 1;
+    return 0;
+}
+
 void inputUnitAction(Game *game, InputState *input) {
     ecs_query_t *query = input->queries.selected;
     BzTileMap   *map   = &game->map;
@@ -127,35 +140,51 @@ void inputUnitAction(Game *game, InputState *input) {
             BzSpatialGridIter gridIt = bzSpatialGridIter(game->entityGrid,
                                                          mPos.x - hRadius, mPos.y - hRadius,
                                                          mPos.x + hRadius, mPos.y + hRadius);
+            EntityFloatPair *harvestables = bzStackAlloc(&game->stackAlloc, 0);
+            i32 numHarvestables = 0;
+            while (bzSpatialGridQueryNext(&gridIt)) {
+                ecs_entity_t harvestEntity = *(ecs_entity_t *) gridIt.data;
+                if (!ecs_has(ECS, harvestEntity, Harvestable))
+                    continue;
+                if (!ecs_has(ECS, harvestEntity, Resource))
+                    continue;
+                if (!ecs_has(ECS, harvestEntity, Position))
+                    continue;
+                if (!ecs_has(ECS, harvestEntity, HitBox))
+                    continue;
+                Resource res = *ecs_get(ECS, harvestEntity, Resource);
+                if (res.type != resource.type)
+                    continue;
+                Position harvestPos = *ecs_get(ECS, harvestEntity, Position);
+                HitBox harvestHB = *ecs_get(ECS, harvestEntity, HitBox);
+                harvestPos = entityGetCenter(harvestPos, harvestHB);
+                f32 dst = Vector2Distance(harvestPos, mPos);
+                harvestables[numHarvestables++] = (EntityFloatPair) {
+                    harvestEntity, dst
+                };
+            }
+            qsort(harvestables, numHarvestables, sizeof(*harvestables), entityFloatPairCmp);
+            i32 idxHarvestable = 0;
+
             ecs_defer_begin(ECS);
             ecs_iter_t it = ecs_query_iter(ECS, query);
             while (ecs_query_next(&it)) {
                 for (i32 i = 0; i < it.count; i++) {
+                    if (idxHarvestable >= numHarvestables)
+                        break;
                     ecs_entity_t entity = it.entities[i];
 
-                    bool hasNext = false;
-                    ecs_entity_t harvestEntity = 0;
-                    do {
-                        hasNext = bzSpatialGridQueryNext(&gridIt);
-                        if (!hasNext) break;
-                        harvestEntity = *(ecs_entity_t *) gridIt.data;
-                        if (!ecs_has(ECS, harvestEntity, Resource))
-                            continue;
-                        const Resource *res = ecs_get(ECS, harvestEntity, Resource);
-                        if (res->type != resource.type)
-                            continue;
-                        if (!ecs_has(ECS, harvestEntity, Harvestable))
-                            continue;
-                        Harvestable *harvestable = ecs_get_mut(ECS, harvestEntity, Harvestable);
+                    EntityFloatPair harvestEntity = {0, 0};
+                    while (idxHarvestable < numHarvestables) {
+                        harvestEntity = harvestables[idxHarvestable++];
+                        Harvestable *harvestable = ecs_get_mut(ECS, harvestEntity.entity, Harvestable);
                         if (harvestable->harvestCount >= harvestable->harvestLimit)
                             continue;
                         harvestable->harvestCount++;
                         break;
-                    } while (hasNext);
-
-                    if (!hasNext) break;
-                    Position target = *ecs_get(ECS, harvestEntity, Position);
-                    HitBox targetHB = *ecs_get(ECS, harvestEntity, HitBox);
+                    }
+                    Position target = *ecs_get(ECS, harvestEntity.entity, Position);
+                    HitBox targetHB = *ecs_get(ECS, harvestEntity.entity, HitBox);
                     target = entityGetCenter(target, targetHB);
 
                     f32 proximity = 4.0f;
@@ -166,7 +195,7 @@ void inputUnitAction(Game *game, InputState *input) {
                     setAIBehaviour(entity, game->BTs.workerHarvest, &(AIBlackboard) {
                             .as.worker = {
                                     .harvestType = resource.type,
-                                    .harvestTarget = harvestEntity,
+                                    .harvestTarget = harvestEntity.entity,
                                     .harvestPos = target,
                             },
                             .proximity = proximity,