//
// Created by bruno on 7.6.2025.
//

#include "pathfinding.h"

uint32_t globalPathfindingVersion = 1;

Node openList[MAX_OPEN_NODES];
int openCount = 0;


void clear_pathfind_data() {
    for (int y = 0; y < MAP_HEIGHT; ++y)
        for (int x = 0; x < MAP_WIDTH; ++x)
            tileMap[y][x].pathFind = (PathFindDat) {0};
}

int heuristic(MiniRect a, MiniRect b) {
    return abs(a.x - b.x) + abs(a.y - b.y);
}


void add_to_open(MiniRect pos, int fCost) {
    if (openCount < MAX_OPEN_NODES)
        openList[openCount++] = (Node) {pos.x, pos.y, fCost};
}

Node pop_best_node() {
    int bestIdx = 0;
    for (int i = 1; i < openCount; ++i) {
        if (openList[i].fCost < openList[bestIdx].fCost)
            bestIdx = i;
    }

    Node best = openList[bestIdx];
    openList[bestIdx] = openList[--openCount];
    return best;
}

bool find_path(MiniRect start, MiniRect end) {
    clear_pathfind_data();
    openCount = 0;

    Tile *startT = &tileMap[start.y][start.x];
    startT->pathFind.gCost = 0;
    startT->pathFind.hCost = heuristic(start, end);
    startT->pathFind.open = true;

    add_to_open(start, startT->pathFind.gCost + startT->pathFind.hCost);

    const int dirs[4][2] = {
            {0,  -1},
            {1,  0},
            {0,  1},
            {-1, 0}
    };

    while (openCount > 0) {
        Node current = pop_best_node();
        PathFindDat *currentPath = &tileMap[current.pos.y][current.pos.x].pathFind;
        currentPath->open = false;
        currentPath->closed = true;

        if (memcmp(&current.pos, &end, sizeof(MiniRect)) == 0) {
            return true; // Path found!
        }

        for (int i = 0; i < 4; ++i) {
            MiniRect n = {current.pos.x + dirs[i][0], current.pos.y + dirs[i][1]};

            if (!isWalkable(n)) continue;

            PathFindDat *neighbor = &tileMap[n.y][n.x].pathFind;

            if (neighbor->closed) continue;

            int tentativeG = currentPath->gCost + 1;

            if (!neighbor->open || tentativeG < neighbor->gCost) {
                neighbor->gCost = tentativeG;
                neighbor->hCost = heuristic(n, end);
                neighbor->parent = currentPath;

                if (!neighbor->open) {
                    neighbor->open = true;
                    add_to_open(n, neighbor->gCost + neighbor->hCost);
                }
            }
        }
    }

    return false; // No path found
}

Path reconstruct_path(MiniRect end) {
    Path path = { .length = 0 };

    PathFindDat* current = &tileMap[end.y][end.x].pathFind;

    // Walk back through the parent pointers
    while (current != NULL) {
        // Find the tile coordinates for the current node
        for (int y = 0; y < MAP_HEIGHT; ++y) {
            for (int x = 0; x < MAP_WIDTH; ++x) {
                if (&tileMap[y][x].pathFind == current) {
                    if (path.length < MAX_OPEN_NODES) {
                        path.steps[path.length++] = (MiniRect){ x, y };
                    }
                    goto found;
                }
            }
        }
        found:
        current = current->parent;
    }

    // Reverse the path to go from start to end
    for (int i = 0; i < path.length / 2; ++i) {
        MiniRect tmp = path.steps[i];
        path.steps[i] = path.steps[path.length - i - 1];
        path.steps[path.length - i - 1] = tmp;
    }
    path.stepIndex = 0;

    return path;
}