#include "GridNavigatorAutomatic.h"
#include <queue>
#include <map>
#include <vector>
#include <algorithm>
#include <cmath>

struct Position {
    int x, y;
    bool operator<(const Position& other) const {
        return std::tie(x, y) < std::tie(other.x, other.y);
    }
    bool operator==(const Position& other) const {
        return x == other.x && y == other.y;
    }
};

// -- Manhattan distance heuristic
int Heuristic(const Position& a, const Position& b) {
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

struct Node {
    Position pos;
    int priority;
};

struct Compare {
    bool operator()(const Node& a, const Node& b) {
        return a.priority > b.priority;
    }
};

GridNavigatorAutomatic::GridNavigatorAutomatic() : GridNavigator() {}

void GridNavigatorAutomatic::Navigate()
{
    const int gridSize = 20;
    Position start{ 1, gridSize - 2 };
    Position target{ gridSize - 2, 1 };

    std::priority_queue<Node, std::vector<Node>, Compare> openSet;
    std::map<Position, Position> cameFrom;
    std::map<Position, int> gScore;

    openSet.push({ start, 0 });
    gScore[start] = 0;
    cameFrom[start] = start;

    struct Dir { int dx, dy; movedirection dir; };
    Dir directions[] = {
        {0,-1,movedirection::up},
        {0,1,movedirection::down},
        {-1,0,movedirection::left},
        {1,0,movedirection::right}
    };

    while (!openSet.empty()) {
        Position current = openSet.top().pos;
        openSet.pop();

        if (current == target)
            break;

        for (auto d : directions) {
            Position next{ current.x + d.dx, current.y + d.dy };

            if (next.x < 0 || next.x >= gridSize || next.y < 0 || next.y >= gridSize)
                continue;

            if (grid->GetGridSquare(next.x, next.y) == griditem::wall)
                continue;

            int estimatedG = gScore[current] + 1;

            if (!gScore.count(next) || estimatedG < gScore[next]) {
                gScore[next] = estimatedG;
                int fScore = estimatedG + Heuristic(next, target);
                openSet.push({ next, fScore });
                cameFrom[next] = current;
            }
        }
    }

    // -- Reconstruct path
    std::vector<movedirection> path;
    Position current = target;

    while (!(current == start)) {
        Position prev = cameFrom[current];

        if (current.x == prev.x && current.y == prev.y - 1) path.push_back(movedirection::up);
        else if (current.x == prev.x && current.y == prev.y + 1) path.push_back(movedirection::down);
        else if (current.x == prev.x - 1 && current.y == prev.y) path.push_back(movedirection::left);
        else if (current.x == prev.x + 1 && current.y == prev.y) path.push_back(movedirection::right);

        current = prev;
    }

    std::reverse(path.begin(), path.end());

    for (auto move : path) {
        moveresults result = grid->MovePlayer(move);
        if (result == moveresults::complete)
            break;
    }
}