#include "pathfinding.h"

static i32 dst(TilePosition a, TilePosition b) {
    //i32 dX = a.x - b.x;
    //i32 dY = a.y - b.y;
    //return dX * dX + dY * dY;
    int dstX = a.x - b.x;
    int dstY = a.y - b.y;
    if (dstX < 0) dstX = -dstX;
    if (dstY < 0) dstY = -dstY;

    if (dstX > dstY)
        return 14 * dstY + 10 * (dstX - dstY);
    return 14 * dstX + 10 * (dstY - dstX);
}

bool findPath(const PathfindingDesc *desc) {
    BZ_ASSERT(desc->map);
    BzTileMap *map = desc->map;
    typedef struct Visited {
        bool visited : 1;
        i8 x : 3;
        i8 y : 3;
    } Visited;
    Visited visited[map->width * map->height] = {};
    bzMemSet(visited, 0, sizeof(visited));

    PathNode *heap = desc->heap;
    if (!heap) heap = bzHeapNew(PathNode, map->width * map->height);
    else bzHeapClear(heap);

    i32 toTargetCost = dst(desc->start, desc->target);
    bzHeapPush(heap, (PathNode) {toTargetCost, 0, toTargetCost, desc->start});

    bool foundPath = false;

    while (!bzHeapIsEmpty(heap)) {
        PathNode node = bzHeapPop(heap);
        if (node.pos.x == desc->target.x &&
            node.pos.y == desc->target.y) {
            // Found path
            foundPath = true;
            break;
        }

        visited[node.pos.y * map->width + node.pos.x].visited = true;

        for (int y = node.pos.y - 1; y <= node.pos.y + 1; y++) {
            for (int x = node.pos.x - 1; x <= node.pos.x + 1; x++) {
                if (x == node.pos.x && y == node.pos.y)
                    continue;
                if (y < 0 || y >= map->height ||
                    x < 0 || x >= map->width)
                    continue;
                if (bzTileMapGetCollider(map, x, y).shapes[0].type != BZ_TILE_SHAPE_NONE)
                    continue;
                Visited *curVisited = &visited[y * map->width + x];
                if (curVisited->visited)
                    continue;
                curVisited->visited = true;

                TilePosition curPos = {x, y};
                i32 gCost = node.gCost + dst(node.pos, curPos);
                //if (gCost >= node.gCost) continue;
                toTargetCost = dst(curPos, desc->target);

                curVisited->x = (i8) (curPos.x - node.pos.x);
                curVisited->y = (i8) (curPos.y - node.pos.y);

                bzHeapPush(heap, (PathNode) {
                    .weight = gCost + toTargetCost,
                    .gCost = gCost,
                    .hCost = toTargetCost,
                    .pos = (TilePosition) {x, y}
                });

            }
        }
    }

    i32 pathLen = 0;
    if (foundPath && desc->outPath) {
        TilePosition pos = desc->target;
        while (pos.x != desc->start.x || pos.y != desc->start.y) {
            Visited *visit = &visited[pos.y * map->width + pos.x];
            BZ_ASSERT(visit->x != 0 && visit->y != 0);
            pos.x -= visit->x;
            pos.y -= visit->y;
            pathLen++;
        }
        Path *out = desc->outPath;
        out->curWaypoint = 0;
        pos = desc->target;
        i32 len = pathLen;
        // Skip positions
        while (len >= out->maxWaypoints) {
            Visited visit = visited[pos.y * map->width + pos.x];
            pos.x -= visit.x;
            pos.y -= visit.y;
            len--;
        }
        // Write path
        for (i32 i = 0; i < len; i++) {
            out->waypoints[len - i - 1] = (Position){
                pos.x * map->tileWidth + map->tileWidth * 0.5f,
                pos.y * map->tileHeight + map->tileHeight * 0.5f
            };
            out->numWaypoints++;
            Visited visit = visited[pos.y * map->width + pos.x];
            pos.x -= visit.x;
            pos.y -= visit.y;
        }
        BZ_ASSERT(len == out->maxWaypoints);
        out->numWaypoints = len;
    }

    if (!desc->heap) {
        bzHeapFree(heap);
        heap = NULL;
    }

    return foundPath ? pathLen : -1;
}