#include <MD_MAX72xx.h>
#include <SPI.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);
// LED Dot Matrix (MAX7219) configuration
volatile bool highToxin = false;
#define MAX_DEVICES 4
#define HARDWARE_TYPE MD_MAX72XX::PAROLA_HW
#define CLK_PIN 12
#define DATA_PIN 10
#define CS_PIN 11
#define maxX (MAX_DEVICES * 8 - 1)
#define maxY 7

// Joystick and Gas Sensor
#define JOY_X A1
#define JOY_Y A0
#define MQ2_APIN A2
#define MQ2_DPIN 6

// Stepper Motor Pins
#define DIR_L 2
#define STEP_L 3
#define DIR_R 4
#define STEP_R 5





// I2C Slave Address


MD_MAX72XX mx = MD_MAX72XX(HARDWARE_TYPE, DATA_PIN, CLK_PIN, CS_PIN, MAX_DEVICES);

// Coordinates
int components[][2] = {{16, 1}, {29,2}, {2, 2}, {22,4}, {13, 4}, {30,5}, {25,5}, {5, 5}};
int hinderance[][2] = {{23, 1}, {6, 1}, {11,1}, {26,2}, {0, 3}, {15,3}, {21,4}, {6, 1}, {10,6}, {28,6}};
int checkpoints[][2] = {{8, 2}, {16, 4}, {24, 6}}; 
bool comp_NOTfound=true;
//int gasLevel;
int x = 0, y = 0;
int prevX = 0, prevY = 0;
int componentCount = 0;
int path[100][2]; // Store path for return journey
int pathIndex = 0;
//int gaslevel= digitalRead(MQ2_DPIN);
int foundcom[8][2];

void setup() {
  // Initialize pins
  pinMode(JOY_X, INPUT);
  pinMode(JOY_Y, INPUT);
  pinMode(DIR_L, OUTPUT);
  pinMode(STEP_L, OUTPUT);
  pinMode(DIR_R, OUTPUT);
  pinMode(STEP_R, OUTPUT);
  pinMode(MQ2_DPIN, INPUT_PULLUP);
  pinMode(MQ2_APIN, INPUT);
  PCICR|=B00000010;
  PCMSK1|=B00000100;
  Serial.begin(9600);
  Serial.println("hi");
  // Initialize LED Matrix
  mx.begin();
  mx.control(MD_MAX72XX::INTENSITY, MAX_INTENSITY / 2);
  mx.clear();
  mx.setPoint(y, x, true);
  mx.update();

  // Initialize LCD
  //lcd.begin(16, 2);
  lcd.init();        
  lcd.setCursor(0, 0);  // 
  lcd.print("Bot Started");

  
  // Store initial position
  path[pathIndex][0] = x;
  path[pathIndex][1] = y;
  pathIndex++;
}

void loop() {
  int xVal = analogRead(JOY_X);
  int yVal = analogRead(JOY_Y);
  prevX = x; prevY = y;

  

  // Joystick movement
  if (yVal > 600) {
    forward(1);
    y = max(y - 1, 0);
  } else if (yVal < 400) {
    backward(1);
    y = min(y + 1, maxY);
  } else if (xVal > 600) {
    turnLeft(1);
    x = min(x + 1, maxX);
  } else if (xVal < 400) {
    turnRight(1);
    x = max(x - 1, 0);
  }

  // Check for obstacles
  for (int i = 0; i < sizeof(hinderance) / sizeof(hinderance[0]); i++) {
    if (x == hinderance[i][0] && y == hinderance[i][1] ) {
      lcd.clear();
      lcd.print("Obstacle Hit");
      mx.setPoint(y, x, true);
      mx.update();
      delay(500);
      mx.setPoint(y, x, false);
      mx.update();
      delay(500);
      mx.setPoint(y, x, true);
      mx.update();
      delay(500);
      mx.setPoint(y, x, false);
      mx.update();
      x = prevX;
      y = prevY;
      //gasInterrupt = false;
      break;
    }
  }

  // Check for components
  for (int i = 0; i < sizeof(components) / sizeof(components[0]); i++) {
  if (x == components[i][0] && y == components[i][1]) {
    bool alreadyFound = false;

    // Check if this component was already found
    for (int j = 0; j < componentCount; j++) {
      if (foundcom[j][0] == x && foundcom[j][1] == y) {
        alreadyFound = true;
        break;
      }
    }

    // If it's a new component, store and notify
    if (!alreadyFound) {
      foundcom[componentCount][0] = x;
      foundcom[componentCount][1] = y;
      componentCount++;

      lcd.clear();
      lcd.print("Component Found");
      mx.setPoint(y, x, true);
      mx.update();
      delay(1500);
      mx.setPoint(y, x, false);
      mx.update();
      mx.setPoint(prevY, prevX, true);
      mx.update();
      delay(1500);
      mx.setPoint(y, x, true);
      mx.update();
      lcd.clear();
      delay(1000);
    }

    break;  
  }
}

    
  


  // Check for checkpoints
  for (int i = 0; i < sizeof(checkpoints) / sizeof(checkpoints[0]); i++) {
    if (x == checkpoints[i][0] && y == checkpoints[i][1]) {
      lcd.clear();
      lcd.print("Checkpoint ");
      lcd.print(i + 1);
      lcd.setCursor(0, 1);
      lcd.print("Components: ");
      lcd.print(componentCount);
      delay(1000);
      lcd.clear();
      delay(1000);
     // handleIRControl();
      break;
    }
  }

  // Update LED Matrix
  mx.setPoint(y, x, true);
  mx.update();

  // Store path
  if (x != path[pathIndex - 1][0] || y != path[pathIndex - 1][1]) {
    path[pathIndex][0] = x;
    path[pathIndex][1] = y;
    pathIndex++;
  }
  if (highToxin) {
    mx.setPoint(y, x, true);
    mx.update();
    delay(1000);
    mx.setPoint(y, x, false);
    mx.update();
    delay(1000);
    x = prevX;
    y = prevY;
    highToxin = false;
  Serial.println("Gas detected!");
  int sensorValue = analogRead(MQ2_APIN);
  Serial.println(sensorValue);  // Read raw value
  delay(500);
  }
 
  
}

// Stepper motor control
void stepMotor(int steps, bool dirA, bool dirB) {
  digitalWrite(DIR_L, dirA ? HIGH : LOW);
  digitalWrite(DIR_R, dirB ? HIGH : LOW);
  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP_L, HIGH);
    digitalWrite(STEP_R, HIGH);
    delay(300);
    digitalWrite(STEP_L, LOW);
    digitalWrite(STEP_R, LOW);
    delay(300);
  }
}

void forward(int steps) {
  stepMotor(steps, true, true);
}

void backward(int steps) {
  stepMotor(steps, false, false);
}

void turnLeft(int steps) {
  stepMotor(steps, false, true);
}

void turnRight(int steps) {
  stepMotor(steps, true, false);
}

ISR(PCINT1_vect){
	if (analogRead(MQ2_APIN) > 360) {
    highToxin = true;
   
  }
}