#include "SdFat.h"

#define SPI_SPEED SD_SCK_MHZ(4)
#define CS_PIN 5
SdFat sd;
File file;

String directionPath = ""; // To store the accumulated string
String compressedDirectionPath = "";

// Define maximum rows and columns
#define MAX_ROWS 5
#define MAX_COLS 5

// Store CSV Data
String csvData[MAX_ROWS][MAX_COLS];

char line[100];

void setup() {
Serial.begin(9600);

if (!sd.begin(CS_PIN, SPI_SPEED)) {
  Serial.println("SD initialization failed.");
  return;
}

file = sd.open("database.csv", FILE_READ);
if (!file) {
  Serial.println("Failed to open CSV file.");
  return;
}

int row = 0;
while (file.available() && row < MAX_ROWS) {
  int n = file.fgets(line, sizeof(line));
  if (n <= 0) break; // Stop if fgets fails

  // Remove newline character
  if (line[n - 1] == '\n') line[n - 1] = '\0';

  // Parse CSV
  char* token = strtok(line, ","); // Splitting by comma
  int col = 0;
  while (token != NULL && col < MAX_COLS) {
  csvData[row][col] = String(token);
  token = strtok(NULL, ",");
  col++;
  }
  row++;
}

file.close();
Serial.println("CSV File Read Successfully!\n");

// Print the CSV file in the terminal
printCSV();
entirePath("Lobby", "Compf");
Serial.println(directionPath);
compressedDirectionPath = compressDirections (directionPath);
Serial.println(compressedDirectionPath);
}

// Function to print the CSV file with spaces instead of commas
void printCSV() {
Serial.println("CSV File Contents:");
for (int i = 0; i < MAX_ROWS; i++) {
  for (int j = 0; j < MAX_COLS; j++) {
  Serial.print(csvData[i][j]);
  Serial.print(" "); // Space instead of comma
  }
  Serial.println(); // New line after each row
}
}
String compressDirections(String input) {
  input += " ";  // Append space to process the last word
  String result = "";
  String currentWord = "";
  String lastWord = "";
  int count = 0;
  
  for (int i = 0; i < input.length(); i++) {
      if (input[i] == ' ') {
          if (currentWord != "") {
              if (currentWord == lastWord) {
                  count++;
              } else {
                  if (count > 0) {
                      result += " " + String(count);
                  }
                  if (result.length() > 0) {
                      result += " ";
                  }
                  result += currentWord;
                  count = 1;
              }
              lastWord = currentWord;
              currentWord = "";
          }
      } else {
          currentWord += input[i];
      }
  }
  
  if (count > 0) {
      result += " " + String(count);
  }
  
  return result;
}

// Function to find the position (row, col) of a component in the grid
void findPosition(const char* target, int &row, int &col) {
  for (int i = 0; i < MAX_ROWS; i++) {
      for (int j = 0; j < MAX_COLS; j++) {
          if (csvData[i][j] == target) { // Compare with csvData instead of grid
              row = i;
              col = j;
              return;
          }
      }
  }
  row = -1; col = -1;  // If not found
}
// Function to print the path from start to destination
void findPath(const char* start, const char* destination) {
  int startRow, startCol, destRow, destCol;
  findPosition(start, startRow, startCol);
  findPosition(destination, destRow, destCol);

  if (startRow == -1 || destRow == -1) {
      Serial.println("Invalid positions!");
      return;
  }

  int row = startRow, col = startCol;

  Serial.println(startRow);
  Serial.println(destRow);
  Serial.println(startCol);
  Serial.println(destCol);

  if (startRow <= destRow && startCol >= destCol){
    Serial.print("Path: ");
    // Serial.print(grid[row][col]);  // Print start position
    // Move right until reaching the last column
    while (col > 0) {
        col--;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("left ");
        directionPath += "left ";
    }
    // Move down to the dest row
    while (row < destRow) {
        row++;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("down ");
        directionPath += "down ";
    }
    // Move left in the dest col
    while (col < destCol) {
        col++;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("right ");
        directionPath += "right "; 
    } 
    // Serial.println(" -> END");
  }

  if(startRow <= destRow && startCol < destCol){
    Serial.print("Path: ");
    // Serial.print(grid[row][col]);  // Print start position

    // Move right until reaching the last column
    while (col > 0) {
        col--;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("left ");
        directionPath += "left ";  
    }
    // Move down to the dest row
    while (row < destRow) {
        row++;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("down ");
        directionPath += "down ";
    }
    // Move left in the dest col
    while (col < destCol) {
        col++;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("right ");
        directionPath += "right "; 
    }
    // Serial.println(" -> END");
  }
  
  if (startRow > destRow && startCol <=destCol){
    Serial.print("Path: ");
    // Serial.print(grid[row][col]);  // Print start position

    // Move right until reaching the last column
    while (col < 4) {
        col++;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("right ");
        directionPath += "right "; 
    }
    // Move down to the dest row
    while (row > destRow) {
        row--;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("up ");
        directionPath += "up ";
    }
    // Move left in the dest col
    while (col > destCol) {
        col--;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("left ");
        directionPath += "left "; 
    }
    Serial.println(" -> END");
  }

  if(startRow > destRow && startCol > destCol){

    Serial.print("Path: ");
    // Serial.print(grid[row][col]);  // Print start position
    
    // Move right until reaching the last column
    while (col > 0) {
        col--;
        // Serial.print(" -> ");
        // Serial.print(grid[row][col]);
        Serial.print("left ");
        directionPath += "left "; 
    }
    // Move down to the dest row
    while (row > destRow) {
      row--;
      // Serial.print(" -> ");
      // Serial.print(grid[row][col]);
      Serial.print("up");
      directionPath += "up "; 
    }
    // Move left in the dest col
    while (col < destCol) {
      col++;
      // Serial.print(" -> ");
      // Serial.print(grid[row][col]);   
      Serial.print("right ");
      directionPath += "right "; 
    }
  // Serial.println(" -> END"); 
  }
  
}

void findPathLobby(String startArea) {
    if(startArea == "Lobby"){
            Serial.print("right ");
            directionPath += "right ";  
        }
    if(startArea == "Comp6"){
        Serial.print("left ");
        directionPath += "left ";  
    }
  }
  
void entirePath(const char* startPoint, const char* endPoint){
    if (startPoint == "Lobby"){
        findPathLobby("Lobby");
        findPath("Comp6", endPoint);  // Call the function
        findPath(endPoint, "Comp6");  // Call the function
        findPathLobby("Comp6");
    }
    else{
        findPath(startPoint, endPoint);  // Call the function
        findPath(endPoint, "Comp6");  // Call the function
        findPathLobby("Comp6");
    }

}
void loop() {}