#include "GridNavigatorAutomatic.h"
#include <queue>
#include <vector>
#include <utility>

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

void GridNavigatorAutomatic::Navigate()
{
    const int SIZE = 20;
    auto raw = grid->GetGrid();


    auto trySolve = [&](bool isYX) -> bool
    {
        auto get = [&](int x, int y) -> griditem
        {
            return isYX ? raw[y][x] : raw[x][y];
        };

        // Find start + target
        std::pair<int,int> start{-1,-1}, target{-1,-1};
        for (int y = 0; y < SIZE; y++)
        {
            for (int x = 0; x < SIZE; x++)
            {
                if (get(x,y) == griditem::player) start = {x,y};
                if (get(x,y) == griditem::target) target = {x,y};
            }
        }

        // If couldn't find both, this orientatin is wrong
        if (start.first == -1 || target.first == -1) return false;

        // BFS
        std::queue<std::pair<int,int>> q;
        std::vector<std::vector<bool>> visited(SIZE, std::vector<bool>(SIZE, false));
        std::vector<std::vector<std::pair<int,int>>> parent(
            SIZE, std::vector<std::pair<int,int>>(SIZE, {-1,-1})
        );

        visited[start.second][start.first] = true;
        q.push(start);

        auto tryPush = [&](int fromX, int fromY, int toX, int toY)
        {
            if (toX < 0 || toX >= SIZE || toY < 0 || toY >= SIZE) return;
            if (visited[toY][toX]) return;
            if (get(toX,toY) == griditem::wall) return;

            visited[toY][toX] = true;
            parent[toY][toX] = {fromX, fromY};
            q.push({toX, toY});
        };

        while (!q.empty())
        {
            auto cur = q.front();
            q.pop();

            if (cur == target) break;

            int x = cur.first;
            int y = cur.second;

            tryPush(x, y, x, y - 1); // up
            tryPush(x, y, x, y + 1); // down
            tryPush(x, y, x - 1, y); // left
            tryPush(x, y, x + 1, y); // right
        }

        // If target wasn't reached, fail
        if (target != start && parent[target.second][target.first].first == -1) return false;

        // Reconstruct path
        std::vector<std::pair<int,int>> path;
        for (auto p = target; p != start; p = parent[p.second][p.first])
            path.push_back(p);

        // Follow path
        std::pair<int,int> cur = start;
        for (auto it = path.rbegin(); it != path.rend(); ++it)
        {
            int nx = it->first;
            int ny = it->second;

            if (nx > cur.first)       grid->MovePlayer(movedirection::right);
            else if (nx < cur.first)  grid->MovePlayer(movedirection::left);
            else if (ny > cur.second) grid->MovePlayer(movedirection::down);
            else if (ny < cur.second) grid->MovePlayer(movedirection::up);

            cur = *it;
        }

        return grid->CheckComplete();
    };

    // Try both orientations (one of these will be correct)
    if (trySolve(true)) return;
    if (trySolve(false)) return;


    }