#include <LedControl.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <SD.h>
#include <SPI.h>

#define joyX A0
#define joyY A1
#define dirPin1 2
#define stepPin1 3
#define dirPin2 6
#define stepPin2 7
#define SD_CS 10

LedControl lc = LedControl(11, 13, 10, 4);
LiquidCrystal_I2C lcd(0x27, 16, 2);
File dataFile;

int xPos = 0, yPos = 0;
int prevX = 0, prevY = 0;
bool moved = false;
bool returning = false;

#define MAX_TRAIL 128  // Reduced from 512
byte trailX[MAX_TRAIL];
byte trailY[MAX_TRAIL];
int trailLen = 0;
byte path[8][32] = {0}; // Optimized from bool to byte

const int obstacleCount = 10;
const byte obstacles[obstacleCount][2] = {
  {6, 1}, {11, 1}, {23, 1}, {26, 2}, {0, 3},
  {15, 3}, {21, 4}, {10, 6}, {15, 6}, {28, 6}
};

const int componentCount = 8;
const byte components[componentCount][2] = {
  {2, 2}, {18, 1}, {29, 2}, {13, 4},
  {22, 4}, {5, 5}, {25, 5}, {30, 5}
};
bool collectedComponents[componentCount] = {false};

const int checkpointCount = 3;
const byte checkpoints[checkpointCount][2] = {
  {15, 0}, {7, 7}, {23, 7}
};

void setup() {
  pinMode(dirPin1, OUTPUT); pinMode(stepPin1, OUTPUT);
  pinMode(dirPin2, OUTPUT); pinMode(stepPin2, OUTPUT);
  pinMode(SD_CS, OUTPUT);

  lcd.init(); lcd.backlight();
  lcd.setCursor(0, 0); lcd.print(F("Bot Ready"));

  for (int i = 0; i < 4; i++) {
    lc.shutdown(i, false); lc.setIntensity(i, 8); lc.clearDisplay(i);
  }

  if (!SD.begin(SD_CS)) {
    lcd.setCursor(0, 1); lcd.print(F("SD init failed"));
  } else {
    lcd.setCursor(0, 1); lcd.print(F("SD Ready"));
    dataFile = SD.open("trail.txt", FILE_WRITE);
    if (dataFile) {
      dataFile.println(F("BOT STARTED"));
      dataFile.close();
    }
  }

  drawAt(xPos, yPos);
  path[yPos][xPos] = 1;
  logTrail(xPos, yPos);
}

void loop() {
  if (returning) return;

  int x = analogRead(joyX), y = analogRead(joyY);
  bool xCentered = (x > 450 && x < 570);
  bool yCentered = (y > 450 && y < 570);

  if (!moved && !(xCentered && yCentered)) {
    prevX = xPos; prevY = yPos;

    if (y > 600 && yPos > 0) { moveStepper(HIGH, HIGH); yPos--; moved = true; }
    else if (y < 400 && yPos < 7) { moveStepper(LOW, LOW); yPos++; moved = true; }
    else if (x > 600 && xPos < 31) { moveStepper(HIGH, LOW); xPos++; moved = true; }
    else if (x < 400 && xPos > 0) { moveStepper(LOW, HIGH); xPos--; moved = true; }

    if (moved) {
      if (isObstacle(xPos, yPos)) {
        blinkObstacle(xPos, yPos);
        xPos = prevX; yPos = prevY;
      } else {
        path[yPos][xPos] = 1;
        logTrail(xPos, yPos);

        if (isComponent(xPos, yPos)) blinkComponent(xPos, yPos);
        if (isCheckpoint(xPos, yPos)) showCheckpointStatus();
        drawPath();

        if (xPos == 31 && yPos == 0) {
          lcd.clear(); lcd.print(F("End Point Reached"));
          delay(1500);
          lcd.clear(); lcd.print(F("Returning..."));
          retraceTrail();
        }
      }
    }
  }

  if (xCentered && yCentered) moved = false;
}

void moveStepper(int d1, int d2) {
  digitalWrite(dirPin1, d1); digitalWrite(dirPin2, d2);
  digitalWrite(stepPin1, HIGH); digitalWrite(stepPin2, HIGH);
  delay(5);
  digitalWrite(stepPin1, LOW); digitalWrite(stepPin2, LOW);
  delay(5);
}

void drawAt(int x, int y) {
  lc.setLed(x / 8, y, x % 8, true);
}

void clearAt(int x, int y) {
  lc.setLed(x / 8, y, x % 8, false);
}

void drawPath() {
  for (int i = 0; i < 4; i++) lc.clearDisplay(i);
  for (int y = 0; y < 8; y++) {
    for (int x = 0; x < 32; x++) {
      if (path[y][x]) drawAt(x, y);
    }
  }
}

void logTrail(int x, int y) {
  if (trailLen < MAX_TRAIL) {
    trailX[trailLen] = x;
    trailY[trailLen] = y;
    trailLen++;
  }
}

void retraceTrail() {
  returning = true;
  for (int i = trailLen - 2; i >= 0; i--) {
    int tx = trailX[i], ty = trailY[i];
    int dx = tx - xPos, dy = ty - yPos;

    if (dx == 1) moveStepper(HIGH, LOW);
    else if (dx == -1) moveStepper(LOW, HIGH);
    else if (dy == 1) moveStepper(LOW, LOW);
    else if (dy == -1) moveStepper(HIGH, HIGH);

    clearAt(xPos, yPos);
    xPos = tx; yPos = ty;
    drawAt(xPos, yPos);
    delay(100);
  }

  clearAt(xPos, yPos);
  xPos = 0; yPos = 0;
  trailLen = 0;
  returning = false;

  lcd.clear(); lcd.print(F("Returned Home"));

  dataFile = SD.open("trail.txt", FILE_WRITE);
  if (dataFile) {
    dataFile.println(F("Retraced to (0,0)"));
    dataFile.println(F("Trail:"));
    for (int i = 0; i < trailLen; i++) {
      dataFile.print("("); dataFile.print(trailX[i]);
      dataFile.print(","); dataFile.print(trailY[i]); dataFile.println(")");
    }
    dataFile.close();
  }
}

bool isObstacle(int x, int y) {
  for (int i = 0; i < obstacleCount; i++) {
    if (obstacles[i][0] == x && obstacles[i][1] == y) return true;
  }
  return false;
}

void blinkObstacle(int x, int y) {
  int m = x / 8, c = x % 8;
  for (int i = 0; i < 4; i++) lc.clearDisplay(i);
  for (int i = 0; i < 2; i++) {
    lc.setLed(m, y, c, true); delay(250);
    lc.setLed(m, y, c, false); delay(250);
  }
  lcd.clear(); lcd.print(F("Obstacle Hit!"));
  lcd.setCursor(0,1); lcd.print("("); lcd.print(x); lcd.print(","); lcd.print(y); lcd.print(")");
  delay(500); drawPath();
}

bool isComponent(int x, int y) {
  for (int i = 0; i < componentCount; i++) {
    if (components[i][0] == x && components[i][1] == y && !collectedComponents[i]) {
      collectedComponents[i] = true;
      lcd.clear(); lcd.setCursor(0, 0);
      lcd.print(F("Component Found:"));
      lcd.setCursor(0, 1);
      lcd.print("("); lcd.print(x); lcd.print(","); lcd.print(y); lcd.print(")");
      delay(1000);
      return true;
    }
  }
  return false;
}

void blinkComponent(int x, int y) {
  int m = x / 8, c = x % 8;
  lc.setLed(m, y, c, false); delay(200);
  lc.setLed(m, y, c, true);
}

bool isCheckpoint(int x, int y) {
  for (int i = 0; i < checkpointCount; i++) {
    if (checkpoints[i][0] == x && checkpoints[i][1] == y) return true;
  }
  return false;
}

void showCheckpointStatus() {
  lcd.clear(); lcd.setCursor(0,0); lcd.print(F("Checkpoint Hit"));
  delay(1000);
  for (int i = 0; i < componentCount; i++) {
    if (collectedComponents[i]) {
      lcd.clear(); lcd.setCursor(0, 0);
      lcd.print(F("Collected:"));
      lcd.setCursor(0, 1);
      lcd.print("("); lcd.print(components[i][0]);
      lcd.print(","); lcd.print(components[i][1]); lcd.print(")");
      delay(1200);
    }
  }
  lcd.clear(); lcd.print(F("Resuming..."));
  delay(800);
}