//created by JustAnElad 

//ROWS = Vertical
//COLs = Horizontal
int goalCol = 0;
int goalRow = 4;
const int ROWS = 8;  // Number of rows in the grid
const int COLS = 8;  // Number of columns in the grid
byte facing = 0;
byte carRow = 0;
byte carCol = 0;

// Define Node structure
struct Node {
  int row;
  int col;
  int gScore;  // Cost from start node
  int fScore;  // gScore + heuristic value
  Node* parent;  // Parent node

  Node(int r, int c, int g, int f, Node* p) : row(r), col(c), gScore(g), fScore(f), parent(p) {}
};

// Define the map grid 
int gridMap[ROWS][COLS] = {
  {0, 0, 0, 1, 0, 0, 0, 0}, // <-- ROWS
  {1, 0, 1, 0, 0, 1, 0, 0},
  {0, 0, 1, 0, 0, 1, 0, 0},
  {0, 0, 0, 0, 1, 0, 0, 0},
  {0, 0, 0, 0, 1, 0, 0, 0},
  {0, 1, 1, 1, 1, 1, 0, 0},
  {0, 0, 0, 0, 0, 0, 0, 0},
  {0, 0, 0, 0, 0, 0, 0, 0}
};  

// Function to check if a position is valid and not an obstacle
bool isValid(int row, int col) {
  // Check if the position is within the grid boundaries and not an obstacle
  return row >= 0 && row < ROWS && col >= 0 && col < COLS && gridMap[row][col] == 0;
}

// Function to calculate the heuristic (Manhattan distance)
int heuristic(int row, int col, int goalRow, int goalCol) {
  return abs(row - goalRow) + abs(col - goalCol);
}

// A* algorithm implementation
bool aStar(int startRow, int startCol, int goalRow, int goalCol, int path[][2], int& pathLength) {
  // Create open and closed lists
  Node* openList[ROWS * COLS];
  Node* closedList[ROWS * COLS];
  int openSize = 0;
  int closedSize = 0;

  // Create start node
  Node* startNode = new Node(startRow, startCol, 0, heuristic(startRow, startCol, goalRow, goalCol), nullptr);
  openList[openSize++] = startNode;

  while (openSize > 0) {
    // Find node with lowest fScore in the open list
    int currentIndex = 0;
    for (int i = 1; i < openSize; ++i) {
      if (openList[i]->fScore < openList[currentIndex]->fScore) {
        currentIndex = i;
      }
    }
    Node* current = openList[currentIndex];

    // Check if goal is reached
    if (current->row == goalRow && current->col == goalCol) {
      // Path found, trace back to construct path
      int index = 0;
      while (current != nullptr) {
        path[index][0] = current->row;
        path[index][1] = current->col;
        current = current->parent;
        index++;
      }
      pathLength = index;
      return true;  // Path found
    }

    // Move current node from open to closed list
    openList[currentIndex] = openList[--openSize];
    closedList[closedSize++] = current;

    // Explore neighbors
    const int dr[] = {1, 0, -1, 0};
    const int dc[] = {0, 1, 0, -1};
    for (int i = 0; i < 4; ++i) {
      int newRow = current->row + dr[i];
      int newCol = current->col + dc[i];
      if (isValid(newRow, newCol)) {
        // Calculate tentative gScore
        int tentativeGScore = current->gScore + 1;

        // Check if neighbor is already in closed list
        bool inClosedList = false;
        int c;
        for (c = 0; c < closedSize; ++c) {
          if (closedList[c]->row == newRow && closedList[c]->col == newCol) {
            inClosedList = true;
            break;
          }
        }

        if (!inClosedList || tentativeGScore < closedList[c]->gScore) {
          // Discover a new node or find a better path to an existing node
          int newFScore = tentativeGScore + heuristic(newRow, newCol, goalRow, goalCol);
          Node* neighbor = new Node(newRow, newCol, tentativeGScore, newFScore, current);
          bool inOpenList = false;
          for (int j = 0; j < openSize; ++j) {
            if (openList[j]->row == newRow && openList[j]->col == newCol) {
              inOpenList = true;
              break;
            }
          }
          if (!inOpenList) {
            openList[openSize++] = neighbor;
          }
        }
      }
    }
  }

  return false;  // No path found
}

// Function to check if there's an obstacle in the specified direction
bool CheckObs() {
  switch (facing) {
    case 0: // Up
      return carCol == 0 || gridMap[carCol - 1][carRow] != 0;
    case 1: // Right
      return carRow == COLS - 1 || gridMap[carCol][carRow + 1] != 0;
    case 2: // Down
      return carCol == ROWS - 1 || gridMap[carCol + 1][carRow] != 0;
    case 3: // Left
      return carRow == 0 || gridMap[carCol][carRow - 1] != 0;
    default:
      return false;
  }
}
void Autonomous(){
  while(carRow == goalCol && carCol == goalRow){
    
  }
  
  int path[ROWS * COLS][2];
  int pathLength = 0;

  // Call aStar function with appropriate start and goal positions
  bool pathFound = aStar(0, 0, goalRow, goalCol, path, pathLength);
  if (pathFound) {
        Serial.print("Car position: ");
        Serial.print(carRow);
        Serial.print(",");
        Serial.println(carCol);
    // Path found, take appropriate actions
    while(carRow != goalCol || carCol != goalRow){
    for (int i = pathLength - 2; i >= 0; i--) {
      Serial.print("Next step: ");
      Serial.print(path[i][1]);
      Serial.print(",");
      Serial.println(path[i][0]);

    // Update facing direction based on current and next position
      if (path[i][0] < carCol) //path[i][0]
        facing = 0;
      if (path[i][0] > carCol)
        facing = 2;        
      if (path[i][1] > carRow) //path[i][1]
        facing = 1;
      if (path[i][1] < carRow)
        facing = 3;
    // Move car if there's no obstacle
      if (!CheckObs()) {
        carRow = path[i][1];
        carCol = path[i][0];
        Serial.print("Car position: ");
        Serial.print(carRow);
        Serial.print(",");
        Serial.println(carCol);
      } else {
        Serial.println("Obstacle detected. Recalculating path...");
        break; // Break out of loop if obstacle detected
      }
    }
    Serial.println(); 
     } // Print a newline
  } else {
    // No path found, handle accordingly
    Serial.println("No path found.");
  }
}
void setup() {
  Serial.begin(9600); // Initialize serial communication with baud rate 9600
}

void loop() {
  Autonomous();
  // Add delay if needed to control the loop rate
}