#include <MD_MAX72xx.h>
#include <SPI.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
Servo S1;
Servo S2;

// Define the number of devices (matrices) of dot led matrix
#define MAX_DEVICES 4
#define stepPIN1 12
#define dirPIN1 2
#define stepPIN2 8
#define dirPIN2 A2
#define stepPIN 7
#define dirPIN A3

LiquidCrystal_I2C lcd(0x27, 16, 2);

// Define the data pin, clock pin, and load pin
#define DATA_PIN 11
#define CLK_PIN 13
#define CS_PIN 10
int xpin = A1;
int ypin = A0;
int buzzPin= A4;
int redPIN = 5;
int greenPIN = 4;
int bluePIN = 3;
int xval;
int yval;
//START POINT (0,0,7)
int matrixIndex = 0;
int row = 0 ;
int col = 7;
int actualCol;
int dt = 50;
int rowarray[100];
int acarray[100];
String ans;
int r, a, i = 0;
int x, y, z;
int obstacleRows[] = {1, 2, 3, 4, 5};
int obstacleCols[] = {10, 15, 20, 25, 30};
int numObstacles = 5;
int q=0;
int h=0;
int ro[20], co[20];
//Volcano (3,3,0) and (3,4,0)
//Spot (3,3,4) and (3,4,4)

// Initialize the LED display
MD_MAX72XX ledDisplay = MD_MAX72XX(MD_MAX72XX::FC16_HW, DATA_PIN, CLK_PIN, CS_PIN, MAX_DEVICES);


void setup() {
  lcd.begin(16,2);
  lcd.print("hello world");
  // Initialize the display
  ledDisplay.begin();
  // Clear the display
  ledDisplay.clear();
  ledDisplay.setPoint(3, 28, true);
  ledDisplay.setPoint(4, 28, true);
  Serial.begin(9600);
  pinMode(xpin, INPUT);
  pinMode(ypin, INPUT);
  pinMode(stepPIN1, OUTPUT);
  pinMode(dirPIN1, OUTPUT);
  pinMode(stepPIN2, OUTPUT);
  pinMode(dirPIN2, OUTPUT);
  S1.attach(6);
  S1.attach(9);
}

void loop() {
  actualCol = matrixIndex * 8 + col;
  ledDisplay.setPoint(row, actualCol, true);
  memorise();
  xval = analogRead(xpin);
  yval = analogRead(ypin);
  delay(250);

  if (yval < 500) {
    left();
  }
  if (yval > 520) {
    right();
  }
  if (xval < 500) {
    bkwd();
  }
  if (xval > 520) {
    fwd();
  }

  if ((row == 3 || row == 4) && actualCol == 28) {
    if (q == 0) {
      Serial.println("Reached capture spot");
      Serial.println("Completed the informal path finding run");
      traceBack();
      Serial.println("Want autonomous run? Y/N");
      while (Serial.available() == 0)
      {

      }
      ans = Serial.readString();
      ans.trim();
      if (ans.equals("Y")) {
        display();
      }
    }
    else if (q==1) {
      Serial.println("Reached capture spot");
      Serial.println("Autonomous run completed");
      q++;
    }
  }
}

void left() {
  //Motor
  if (!isObstacle(row + 1, actualCol)) {
    digitalWrite(dirPIN1, HIGH);
    digitalWrite(stepPIN1, HIGH);
    delay(dt);
    digitalWrite(stepPIN1, LOW);
    delay(dt);
    digitalWrite(dirPIN2, HIGH);
    digitalWrite(stepPIN2, HIGH);
    delay(dt);
    digitalWrite(stepPIN2, LOW);
    delay(dt);
    //LED
    row++;
  }
  else {
    safety(row + 1, actualCol);
    blinkmem(row + 1, actualCol);
  }
}
void right() {
  //Motor
  if (!isObstacle(row - 1, actualCol)) {
    digitalWrite(dirPIN1, HIGH);
    digitalWrite(stepPIN1, HIGH);
    delay(dt);
    digitalWrite(stepPIN1, LOW);
    delay(dt);
    digitalWrite(dirPIN2, LOW);
    digitalWrite(stepPIN2, HIGH);
    delay(dt);
    digitalWrite(stepPIN2, LOW);
    delay(dt);
    //LED
    row--;
  }
  else {
    safety(row - 1, actualCol);
    blinkmem(row - 1, actualCol);
    //buzzer
  }
}
void fwd() {
  //LED & Motor
  if (col > 0) {

    if (!isObstacle(row, actualCol - 1)) {
      if (actualCol == 28) {
        Serial.println("Cant go forward");
      }
      else {
        col--;
        digitalWrite(dirPIN1, HIGH);
        digitalWrite(stepPIN1, HIGH);
        delay(dt);
        digitalWrite(stepPIN1, LOW);
        delay(dt);
      }
    }
    else {
      safety(row, actualCol - 1);
      blinkmem(row, actualCol - 1);
    }
  }
  else {
    if (!isObstacle(row, actualCol + 15)) {
      matrixIndex++;
      col = 7;
      digitalWrite(dirPIN1, HIGH);
      digitalWrite(stepPIN1, HIGH);
      delay(dt);
      digitalWrite(stepPIN1, LOW);
      delay(dt);
    }
    else {
      safety(row, actualCol + 15);
      blinkmem(row, actualCol + 15);
    }
  }
}
void bkwd() {
  //Motor
  //LED
  if (col < 7 ) {
    if ( !isObstacle(row, actualCol + 1)) {
      digitalWrite(dirPIN1, LOW);
      digitalWrite(stepPIN1, HIGH);
      delay(dt);
      digitalWrite(stepPIN1, LOW);
      delay(dt);
      actualCol++;
    }
    else {
      safety(row, actualCol + 1);
      blinkmem(row, actualCol + 1);
    }
  }
  else {
    if (!isObstacle(row, actualCol - 15)) {
      digitalWrite(dirPIN1, LOW);
      digitalWrite(stepPIN1, HIGH);
      delay(dt);
      digitalWrite(stepPIN1, LOW);
      delay(dt);
      matrixIndex--;
      col = 0;
    }
    else {
      safety(row, actualCol - 15);
      blinkmem(row, actualCol - 15);
    }
  }
}
void memorise() {
  rowarray[i] = row;
  acarray[i] = actualCol;
  i++;
}
void display() {
  Serial.println("Displaying recorded path...");
  for (int j = 0; j < i; j++) {
    r = rowarray[j];
    a = acarray[j];
    ledDisplay.setPoint(r, a, true);
    delay(100);
  }
  q++;
  i = 0;
}
bool isObstacle(int r, int c) {
  for (int i = 0; i < numObstacles; i++) {
    if (r == obstacleRows[i] && c == obstacleCols[i]) {
      Serial.println("Obstacle detected");
      return true;
    }
  }
  return false;
}
void blinkmem(int r, int c) {
  ledDisplay.setPoint(r, c, true);
  delay(250);
  ledDisplay.setPoint(r, c, false);
  delay(250);
  ledDisplay.setPoint(r, c, true);
  delay(250);
  ledDisplay.setPoint(r, c, false);
  delay(250);
  ro[h]= r;
  co[h]= c;
  h++;
  //gcghc
}
void safety(int r,int c){
  for(int i=0;i<h;i++){
    if (r==ro[i] && c==co[i]){
      Serial.println("Crashed on an detected obstacle");
      Serial.println("Motor stopped");
    }
  }
}

void traceBack() {
  for (int j = i - 2; j >= 0; j--) {
    r = rowarray[j];
    a = acarray[j];
    ledDisplay.setPoint(r, a, false);
    delay(100);
  }
}