#include "systems.h"


#include "game_state.h"
#include "input.h"
#include "pathfinding.h"

#include <math.h>
#include <raymath.h>

void entityClearTasks(const ecs_entity_t entity) {
    ecs_remove(ECS, entity, HarvestTask);
}
bool entitySetPath(const ecs_entity_t entity, const Vector2 target, Game *game) {
    const Vector2 *pPath = ecs_get(ECS, entity, Position);
    BZ_ASSERT(pPath);
    const Vector2 start = *pPath;

    Path path = {NULL, 0};
    const bool foundPath = pathfindAStar(&(PathfindingDesc) {
        .start = start,
        .target = target,
        .map = &game->map,
        .outPath = &path,
        .pool = game->pools.pathData,
        .alloc = &game->stackAlloc,
    });
    if (foundPath) {
        BZ_ASSERT(path.paths);
        ecs_set_ptr(ECS, entity, Path, &path);
        return true;
    }
    return false;
}


static Position getBottomLeftPos(Position pos, Size size) {
    return (Position) {pos.x - size.x * 0.5f, pos.y - size.y * 0.5f};
}

void entitySpatialRemove(ecs_iter_t *it) {
    Game *game = ecs_singleton_get_mut(ECS, Game);
    SpatialGridID *spatialID = ecs_field(it, SpatialGridID, 1);

    for (i32 i = 0; i < it->count; i++) {
        bzSpatialGridRemove(game->entityGrid, spatialID[i]);
    }

}

void entityPathRemove(ecs_iter_t *it) {
    Game *game = ecs_singleton_get_mut(ECS, Game);
    BzObjectPool *pool = game->pools.pathData;
    Path *path = ecs_field(it, Path, 1);

    for (i32 i = 0; i < it->count; i++) {
        ecs_entity_t entity = it->entities[i];

        ecs_remove(ECS, entity, TargetPosition);

        PathData *cur = path[i].paths;
        while (cur) {
            bzObjectPoolRelease(pool, cur);
            cur = cur->next;
        }
    }
}

void entitySetAnimationState(ecs_iter_t *it) {
    /*
    Animation     *anim     = ecs_field(it, Animation, 1);
    AnimationType *animType = ecs_field(it, AnimationType , 2);
    for (i32 i = 0; i < it->count; i++) {
        EntityType    entityType = anim[i].entityType;
        AnimationType type       = animType[i];
        BZ_ASSERT(entityHasAnimation(entityType, type));
        anim[i].animType = type;
        anim[i].sequence = getEntityAnimation(entityType, type);
    }
     */
}

void entityUpdateSpatialID(ecs_iter_t *it) {
    Game *game = ecs_singleton_get_mut(ECS, Game);
    Position *position = ecs_field(it, Position, 1);
    Position *size = ecs_field(it, Position, 2);
    //Velocity *velocity = ecs_field(it, Velocity, 3);
    SpatialGridID *id = ecs_field(it, SpatialGridID, 4);

    for (i32 i = 0; i < it->count; i++) {
        Position pos = getBottomLeftPos(position[i], size[i]);
        bzSpatialGridUpdate(game->entityGrid, id[i], pos.x, pos.y, size[i].x, size[i].y);
    }

}

void entityUpdateKinematic(ecs_iter_t *it) {
    Position *position = ecs_field(it, Position, 1);
    Rotation *rotation = ecs_field(it, Rotation, 2);
    Velocity *velocity = ecs_field(it, Velocity, 3);
    Steering *steering = ecs_field(it, Steering, 4);

    f32 dt = it->delta_time;

    for (i32 i = 0; i < it->count; i++) {
        // Update position and rotation
        // position += velocity * dt
        position[i] = Vector2Add(position[i], Vector2Scale(velocity[i], dt));

        // Update velocity and angular velocity
        // velocity += steering.liner * dt
        velocity[i] = Vector2Add(velocity[i], Vector2Scale(steering[i], dt * 10));

        if (Vector2LengthSqr(steering[i]) == 0) {
            // Decay velocity
            velocity[i] = Vector2Scale(velocity[i], 1 - (dt * 5.0f));
        }
        // Reset steering
        steering[i] = Vector2Zero();

        {
            const InputState *input = ecs_singleton_get(ECS, InputState);

            Vector2 mouse = input->mouseDownWorld;
            f32 rot = Vector2Angle(position[i], mouse) + 270 * DEG2RAD;
            rotation[i] = rot;

        }

        // Check for speeding and clip
        const f32 maxSpeed = 15.0f;
        if (Vector2Length(velocity[i]) > maxSpeed) {
            velocity[i] = Vector2Normalize(velocity[i]);
            velocity[i] = Vector2Scale(velocity[i], maxSpeed);
        }

        // Update flipX
        ecs_entity_t entity = it->entities[i];
        if (ecs_has(it->world, entity, TextureRegion)) {
            TextureRegion *text = ecs_get_mut(it->world, entity, TextureRegion);
            text->flipX = rotation[i] >= 0.0f * RAD2DEG && rotation[i] <= 180.0f * RAD2DEG;
        }
    }
}

void entityMoveToTarget(ecs_iter_t *it) {
    Position *position = ecs_field(it, Position, 1);
    Rotation *rotation = ecs_field(it, Rotation, 2);
    Velocity *velocity = ecs_field(it, Velocity, 3);

    TargetPosition *targetPos = ecs_field(it, TargetPosition, 4);
    Steering       *steering  = ecs_field(it, Steering, 5);

    for (i32 i = 0; i < it->count; i++) {
        Position target = targetPos[i];
        steering[i] = Vector2Subtract(target, position[i]);
        f32 dst = Vector2LengthSqr(steering[i]);
        f32 maxAccel = 10.0f;
        steering[i] = Vector2Normalize(steering[i]);
        steering[i] = Vector2Scale(steering[i], maxAccel);

        if (Vector2Length(velocity[i]) > 10.0f) {
            f32 rot = Vector2Angle(position[i], target);
            rotation[i] = rot;
        }

        if (dst < 8.0f) {
            // Arrived
            ecs_remove(ECS, it->entities[i], TargetPosition);
        }
    }
}

void entityFollowPath(ecs_iter_t *it) {
    const Game *game = ecs_singleton_get(ECS, Game);

    Path *path = ecs_field(it, Path, 1);

    for (i32 i = 0; i < it->count; i++) {
        const ecs_entity_t entity = it->entities[i];

        if (!ecs_has(ECS, entity, TargetPosition)) {
            if (path[i].curWaypoint >= path[i].paths->numWaypoints) {
                path[i].curWaypoint = 0;
                PathData *oldPath = path[i].paths;
                bzObjectPoolRelease(game->pools.pathData, oldPath);
                path[i].paths = path[i].paths->next;
                if (!path[i].paths) ecs_remove(ECS, it->entities[i], Path);
            }
            if (path[i].paths) {
                TargetPosition target = path[i].paths->waypoints[path[i].curWaypoint];
                path[i].curWaypoint++;
                ecs_set_ptr(ECS, entity, TargetPosition, &target);
            }

        }
    }
}

static ecs_entity_t findNearestStorage(ResourceType type) {
    ecs_filter_t *storageFilter = ecs_filter(ECS, {
        .terms = {{ecs_id(Storage)} }
    });
    ecs_iter_t it = ecs_filter_iter(ECS, storageFilter);

    ecs_entity_t closest = 0;
    while (ecs_filter_next(&it)) {
        Storage *storage = ecs_field(&it, Storage, 1);

        for (i32 i = 0; i < it.count; i++) {
            if (true || storage[i].capacity[type]) {
                closest = it.entities[i];
            }
        }
    }
    ecs_filter_fini(storageFilter);
    return closest;
}

void entityHarvestTaskSystem(ecs_iter_t *it) {
    Game *game = ecs_singleton_get_mut(ECS, Game);

    const Position *position = ecs_field(it, Position, 1);
    const Rotation *rotation = ecs_field(it, Rotation, 2);

    const HarvestTask *harvestTask = ecs_field(it, HarvestTask, 3);

    for (i32 i = 0; i < it->count; i++) {
        const ecs_entity_t entity       = it->entities[i];
        const ecs_entity_t targetEntity = harvestTask[i].entity;

        const Position *pTarget = ecs_get(ECS, targetEntity, Position);
        BZ_ASSERT(pTarget);
        const Position target = *pTarget;

        const f32 DST_LIMIT = 10.0f;

        Resource *resource = ecs_get_mut(ECS, targetEntity, Resource);

        if (resource->amount <= 0) {
            ecs_delete(ECS, targetEntity);
            ecs_remove(ECS, entity, HarvestTask);
            continue;
        }

        const f32 dst = Vector2Distance(position[i], target);
        if (!ecs_has(ECS, entity, Path) && dst > DST_LIMIT) {
            bzLogInfo("%.2f", dst);
            // Pathfind to target
            entitySetPath(entity, target, game);
            continue;
        } else if (dst < DST_LIMIT && !ecs_has(ECS, entity, Path)) {
            if (!ecs_has(ECS, entity, Path)) {
                bzLogInfo("Mine");
                resource->amount -= 5;
            }
            ecs_remove(ECS, entity, Path);
            // MINE
            // find nearest warehouse for wood
            ecs_entity_t storage = findNearestStorage(RES_WOOD);
            if (storage) {
                const Position *storagePos = ecs_get(ECS, storage, Position);
                BZ_ASSERT(storagePos);
                entitySetPath(entity, *storagePos, game);
            }
        }

        // Harvest
        const i32 carryCapacity = 5;

    }
}

void entityUpdateAnimationState(ecs_iter_t *it) {
    Velocity      *velocity = ecs_field(it, Velocity, 1);
    //AnimationType *animType = ecs_field(it, AnimationType , 2);
    for (i32 i = 0; i < it->count; i++) {
        f32 len = Vector2Length(velocity[i]);

        /*
        ecs_entity_t  entity = it->entities[i];
        AnimationType type   = ANIM_IDLE;
        if (len > 1.0f)
            type = ANIM_WALK;

        ecs_set(it->world, entity, AnimationType, {type});
         */
    }
}
void entityUpdateAnimation(ecs_iter_t *it) {
    Game          *game    = ecs_singleton_get_mut(ECS, Game);
    Animation     *anim    = ecs_field(it, Animation, 1);
    TextureRegion *texture = ecs_field(it, TextureRegion, 2);

    float dt = GetFrameTime();

    for (i32 i = 0; i < it->count; i++) {
        AnimationFrame    frame = anim[i].frame;
        AnimationSequence seq   = anim[i].sequence;

        anim[i].elapsed += dt;
        if (anim[i].elapsed < frame.duration) continue;

        i32 nextFrame    = (anim[i].curFrame + 1) % seq.frameCount;
        anim[i].curFrame = nextFrame;
        anim[i].frame    = entityGetAnimationFrame(anim[i].entityType, anim[i].animType, nextFrame);
        anim[i].elapsed  = 0.0f;

        texture[i].rec = bzTilesetGetTileRegion(anim[i].tileset, anim[i].frame.frame);
    }
}

static void render(ecs_iter_t *it) {
    Position *p = ecs_field(it, Position, 1);
    Size *s = ecs_field(it, Size, 2);
    Rotation *r = ecs_field(it, Rotation, 3);
    TextureRegion *t = ecs_field(it, TextureRegion, 4);

    for (i32 i = 0; i < it->count; i++) {
        Rectangle dst = {p[i].x, p[i].y, s[i].x, s[i].y};
        Vector2 origin = {dst.width * 0.5f, dst.height * 0.5f};
        Rectangle src = t[i].rec;
        if (t[i].flipX) src.width *= -1.0f;
        if (t[i].flipY) src.height *= -1.0f;
        DrawTexturePro(t[i].texture, src, dst, origin, t[i].rotation, WHITE);
    }
}

void renderTerrain(ecs_iter_t *it) {
    render(it);
}

void renderBuildings(ecs_iter_t *it) {
    render(it);
}

void renderEntities(ecs_iter_t *it) {
    render(it);
}

void renderColliders(ecs_iter_t *it) {
    Position *pos = ecs_field(it, Position, 1);
    Size *size = ecs_field(it, Size, 2);

    for (i32 i = 0; i < it->count; i++) {
        f32 posX = pos[i].x - size[i].x * 0.5f;
        f32 posY = pos[i].y - size[i].y * 0.5f;
        DrawRectangleLines(posX, posY, size[i].x, size[i].y, RED);
    }
}

void renderRotationDirection(ecs_iter_t *it) {
    Position *pos = ecs_field(it, Position, 1);
    Rotation *rot = ecs_field(it, Rotation, 2);

    for (i32 i = 0; i < it->count; i++) {
        Vector2 v = {10.0f, 0.0f};
        v = Vector2Rotate(v, rot[i]);
        v = Vector2Add(v, pos[i]);
        DrawCircle(v.x, v.y, 1.0f, RED);
    }

}


void renderDebugPath(ecs_iter_t *it) {
    Path *path = ecs_field(it, Path, 1);

    for (i32 i = 0; i < it->count; i++) {
        PathData *pathData = path[i].paths;
        bool first = true;
        while (pathData) {
            for (i32 iPath = 0; iPath < pathData->numWaypoints; iPath++) {
                Color color = RED;
                if (first && iPath < path[i].curWaypoint)
                    color = GREEN;
                else if (first && iPath == path[i].curWaypoint)
                    color = ORANGE;
                color.a = 180;

                Position pos = pathData->waypoints[iPath];
                DrawCircle(pos.x, pos.y, 3, color);
            }
            first = false;
            pathData = pathData->next;
        }
    }
}