#include "systems.h"


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

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

void entitySpatialRemoved(ecs_iter_t *it) {
    Game *game = ecs_singleton_get_mut(ECS, Game);
    Position *pos = ecs_field(it, Position, 1);
    SpatialGridID *spatialID = ecs_field(it, SpatialGridID , 2);

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

}

void pathRemoved(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++) {
        PathData *cur = path[i].paths;
        while (cur) {
            bzObjectPoolRelease(pool, cur);
            cur = cur->next;
        }
    }
}

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);
    AngularVelocity *angularVelocity = ecs_field(it, Rotation, 4);
    SteeringOutput *steeringOutput = ecs_field(it, SteeringOutput, 5);

    f32 dt = it->delta_time;

    for (i32 i = 0; i < it->count; i++) {

        // and velocity and angular velocity
        velocity[i] = Vector2Scale(velocity[i], 0.9f);
        velocity[i].x += steeringOutput[i].linear.x * dt;
        velocity[i].y += steeringOutput[i].linear.y * dt;
        f32 maxSpeed = 15.0f;
        velocity[i] = Vector2Clamp(velocity[i],
                                   (Velocity) {-maxSpeed, -maxSpeed},
                                   (Velocity){maxSpeed, maxSpeed});
        angularVelocity[i] += steeringOutput[i].angular * dt;
        steeringOutput[i] = (SteeringOutput){};

        // Update position and rotation
        position[i].x += velocity[i].x * dt * 10;
        position[i].y += velocity[i].y * dt * 10;
        rotation[i] += angularVelocity[i] * dt * 10;
    }
}

void entityFollowPath(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);
    AngularVelocity *angularVelocity = ecs_field(it, Rotation, 4);
    SteeringOutput *steeringOutput = ecs_field(it, SteeringOutput, 5);
    Path *path = ecs_field(it, Path, 6);

    for (i32 i = 0; i < it->count; i++) {
        Position target = path[i].paths->waypoints[path[i].curWaypoint];
        steeringOutput[i].linear.x = target.x - position[i].x;
        steeringOutput[i].linear.y = target.y - position[i].y;

        f32 dst = Vector2LengthSqr(steeringOutput[i].linear);
        if (dst < 8.0f) {
            path[i].curWaypoint++;
            if (path[i].curWaypoint >= path[i].paths->numWaypoints) {
                path[i].curWaypoint = 0;
                path[i].paths = path[i].paths->next;
                if (!path[i].paths) ecs_remove(ECS, it->entities[i], Path);
            }
        }

        steeringOutput[i].linear = Vector2Normalize(steeringOutput[i].linear);
        steeringOutput[i].linear = Vector2Scale(steeringOutput[i].linear, 10.0f);

    }
}

void updateAnimations(ecs_iter_t *it) {
    Game *game = ecs_singleton_get_mut(ECS, Game);
    Animation *anim = ecs_field(it, Animation, 1);
    TextureRegion *t = ecs_field(it, TextureRegion, 2);

    float dt = GetFrameTime();

    for (i32 i = 0; i < it->count; i++) {
        anim[i].frameDuration = game->frameDuration;
        anim[i].elapsed += dt;
        if (anim[i].elapsed < anim[i].frameDuration) continue;

        anim[i].currFrame = (anim[i].currFrame + 1) % anim[i].frameCount;
        anim[i].elapsed = 0.0f;
        t[i].rec.x = anim[i].firstFrame.rec.x + anim[i].currFrame * t[i].rec.width;
    }
}

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, r[i], 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 renderDebugPath(ecs_iter_t *it) {
    Path *path = ecs_field(it, Path, 1);

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

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