#include <AccelStepper.h>
#include <LedControl.h>
#include <LiquidCrystal_I2C.h>

// Motors (A4988-style)
AccelStepper leftMotor(AccelStepper::DRIVER, 3, 2);
AccelStepper rightMotor(AccelStepper::DRIVER, 5, 4);

// LED Matrix
LedControl matrix = LedControl(11, 13, 10, 1);

// LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Joystick
const int joyX = A0;
const int joyY = A1;

// LED "dot" pin
const int LED_PIN = 12;

// Map and movement
const int matrixSize = 8;
int botX = 0, botY = 0;
int direction = 0; // 0=NORTH, 1=EAST, 2=SOUTH, 3=WEST

// Checkpoint, component, obstacle data
const int numCheckpoints = 3;
const int checkpoints[numCheckpoints][2] = {
  {2, 1}, {4, 3}, {6, 6}
};

const int numComponents = 5;
const int components[numComponents][2] = {
  {1, 1}, {2, 2}, {4, 2}, {5, 5}, {6, 4}
};

const int numObstacles = 4;
const int obstacles[numObstacles][2] = {
  {3, 1}, {3, 2}, {5, 3}, {6, 2}
};

int componentCount = 0;

// Utility
bool matches(int x, int y, const int arr[][2], int size) {
  for (int i = 0; i < size; i++)
    if (arr[i][0] == x && arr[i][1] == y)
      return true;
  return false;
}

// Movement vector from direction
void getDirectionVector(int dir, int &dx, int &dy) {
  dx = 0; dy = 0;
  if (dir == 0) dy = -1;      // NORTH
  else if (dir == 1) dx = 1;  // EAST
  else if (dir == 2) dy = 1;  // SOUTH
  else if (dir == 3) dx = -1; // WEST
}

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();

  pinMode(LED_PIN, OUTPUT);

  leftMotor.setMaxSpeed(300);
  rightMotor.setMaxSpeed(300);

  matrix.shutdown(0, false);
  matrix.setIntensity(0, 5);
  matrix.clearDisplay(0);

  lcd.setCursor(0, 0);
  lcd.print("Bot Start!");
  delay(1000);
  lcd.clear();
}

void loop() {
  int xVal = analogRead(joyX);
  int yVal = analogRead(joyY);
  bool isMoving = false;

  // Movement thresholding
  if (yVal > 600 && abs(xVal - 512) < 100) {
    // Forward
    int dx, dy;
    getDirectionVector(direction, dx, dy);
    int newX = botX + dx;
    int newY = botY + dy;

    if (newX >= 0 && newX < matrixSize && newY >= 0 && newY < matrixSize &&
        !matches(newX, newY, obstacles, numObstacles)) {
      botX = newX;
      botY = newY;

      leftMotor.moveTo(leftMotor.currentPosition() + 50);
      rightMotor.moveTo(rightMotor.currentPosition() + 50);
      isMoving = true;
      lcd.setCursor(0, 0);
      lcd.print("Moving Forward ");
    }
  }
  else if (yVal < 400 && abs(xVal - 512) < 100) {
    // Backward
    int dx, dy;
    getDirectionVector(direction, dx, dy);
    int newX = botX - dx;
    int newY = botY - dy;

    if (newX >= 0 && newX < matrixSize && newY >= 0 && newY < matrixSize &&
        !matches(newX, newY, obstacles, numObstacles)) {
      botX = newX;
      botY = newY;

      leftMotor.moveTo(leftMotor.currentPosition() - 50);
      rightMotor.moveTo(rightMotor.currentPosition() - 50);
      isMoving = true;
      lcd.setCursor(0, 0);
      lcd.print("Moving Backward");
    }
  }
  else if (xVal < 400 && abs(yVal - 512) < 100) {
    // Turn Left
    direction = (direction + 3) % 4;
    lcd.setCursor(0, 0);
    lcd.print("Turning Left   ");
    isMoving = true;
  }
  else if (xVal > 600 && abs(yVal - 512) < 100) {
    // Turn Right
    direction = (direction + 1) % 4;
    lcd.setCursor(0, 0);
    lcd.print("Turning Right  ");
    isMoving = true;
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Idle           ");
  }

  // Update matrix
  matrix.clearDisplay(0);

  for (int i = 0; i < numCheckpoints; i++)
    matrix.setLed(0, checkpoints[i][1], checkpoints[i][0], true);
  for (int i = 0; i < numComponents; i++)
    matrix.setLed(0, components[i][1], components[i][0], true);
  for (int i = 0; i < numObstacles; i++)
    matrix.setLed(0, obstacles[i][1], obstacles[i][0], true);

  // Show bot
  matrix.setLed(0, botY, botX, true);

  // Component collection
  if (matches(botX, botY, components, numComponents)) {
    componentCount++;
    delay(200);
  }

  // Checkpoint feedback
  if (matches(botX, botY, checkpoints, numCheckpoints)) {
    lcd.setCursor(0, 1);
    lcd.print("Components: ");
    lcd.print(componentCount);
    delay(1000);
  }

  // Run motors
  leftMotor.run();
  rightMotor.run();

  // Blink LED when moving
  if (isMoving) {
    digitalWrite(LED_PIN, HIGH);
    delay(100);
    digitalWrite(LED_PIN, LOW);
    delay(100);
  } else {
    digitalWrite(LED_PIN, LOW);
  }
}