#include "GridNavigatorAutomatic.h"
#include <vector>
#include <set>
#include <limits>
GridNavigatorAutomatic::GridNavigatorAutomatic() : GridNavigator()
{
}


     //Function to find the shortest path in a grid, takes 2d array grid, start x and y coordinates
    void GridNavigatorAutomatic :: Shortest(int startX, int startY) {
        moveresults mr = moveresults::ok;
        // movements to traverse grid                left,      right,       up      ,  down   
        const vector<pair<int, int>> movements = { { -1, 0 }, { 1, 0 }, { 0, -1 }, { 0, 1 } };

        int** gridCopy = new int* [20];
        for (int i = 0; i < 20; i++) {
            gridCopy[i] = new int[20];
        }

        auto testGrid = grid->GetGrid();

        int rows = 20;
        int cols = 20;
 

        // Distance set to infinte
        vector<vector<int>> distance(rows, vector<int>(cols, numeric_limits<int>::max()));
        distance[startX][startY] = 0;

        // Set to store nodes as {distance, {x, y}}
        set<pair<int, pair<int, int>>> nodeSet;
        nodeSet.insert({ 0, {startX, startY} });

        while (!nodeSet.empty() ) {
            // Extract the node with the smallest distance
            auto it = nodeSet.begin();
            int currentDist = it->first;
            int x = it->second.first;
            int y = it->second.second;
            nodeSet.erase(it);

            // iterate neighnbours by using movements to traverse grid
            for (int i = 0; i < movements.size(); i++) {


                int newX = x + movements[i].first;
                int newY = y + movements[i].second;

                // check wheter next position is wall
                if (newX >= 0 && newX < rows && newY >= 0 && newY < cols) {
                    int weight = gridCopy[newX][newY]; // Weight of the cell being visited
                    
                    if (grid->GetGridSquare(newX,newY) == griditem::wall) continue;   // enum movr result



                    if (distance[newX][newY] > currentDist + weight) {
                        // If already in the set, erase the old entry
                        if (distance[newX][newY] != numeric_limits<int>::max()) {
                            nodeSet.erase({ distance[newX][newY], {newX, newY} });
                        }
                        // Update distance and insert the new entry
                        distance[newX][newY] = currentDist + weight;
                        nodeSet.insert({ distance[newX][newY], {newX, newY} });
                        grid->MovePlayer(static_cast<movedirection>(i));
                    }
                }
            }
        }
        if (mr == moveresults::complete) {
            cout << "Maze Solved";
        }
    }

  void GridNavigatorAutomatic::Navigate() {
      //vector<griditem> grid1 = grid->GetGrid();
      
      int playerStartx = 1;
      int playerStarty = 18;
      auto testGrid = grid->GetGrid();

      int** gridCopy = new int* [20];
      for (int i = 0; i < 20; i++) {
          gridCopy[i] = new int[20];
      }
      

     

        // Convert the grid to a std::vector<std::vector<griditem>>
        
        //TODO: your code here!

        GridNavigatorAutomatic::Shortest(playerStartx, playerStarty);
        delete gridCopy;
   }