#include <MD_MAX72xx.h>
#include <SPI.h>
#include <IRremote.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#define CSPIN A3
LiquidCrystal_I2C lcd(0x27,16,2);
volatile bool hightoxin=false;
#define MAXDEVICES 4
#define HARDWARE_TYPE MD_MAX72XX::PAROLA_HW
#define CLKPIN 12
#define DATAPIN 10
#define CSPIN 11
#define maxX (MAXDEVICES *8-1)
#define maxY 7
#define PINRECEIVER 7

#define JOYX A1
#define JOYY A0
#define MQ2APIN A2
#define MQ2DPIN 3

#define DIRL 2
#define STEPL 6
#define DIRR 4
#define STEPR 5

IRrecv receiver(PINRECEIVER);
MD_MAX72XX mx=MD_MAX72XX(HARDWARE_TYPE, DATAPIN, CLKPIN, CSPIN, MAXDEVICES);

const int comp[][2]={{16,1},{29,2},{2,2},{22,4},{13,4},{30,5},{25,5},{5,5}};
int obs[][2]={{23,1},{6,1},{11,1},{26,2},{0, 3},{15,3},{21,4},{6,1},{10,6},{28,6}};
int obsindex=8;
const int checkpoints[][2]={{8,2},{16,4},{24,6},{31,0}}; 
bool comp_NOTfound=true;
int checkpointscount=0;

int x=0,y=0;
int prevX=0,prevY=0;
int componentcount=0;
int path[75][2];
int pathindex=0;
int foundcom[8][2];
int foundcheck[4][2];
int obsfound[15][2];
void setup() {
  pinMode(JOYX,INPUT);
  pinMode(JOYY,INPUT);
  pinMode(DIRL,OUTPUT);
  pinMode(STEPL,OUTPUT);
  pinMode(DIRR,OUTPUT);
  pinMode(STEPR,OUTPUT);
  pinMode(MQ2DPIN,INPUT_PULLUP);
  pinMode(MQ2APIN,INPUT);
  attachInterrupt(digitalPinToInterrupt(MQ2DPIN),checkSensor,FALLING);
  Serial.begin(9600);
  Serial.println("hi");
  IrReceiver.begin(PINRECEIVER);
  mx.begin();
  mx.control(MD_MAX72XX::INTENSITY, MAX_INTENSITY / 2);
  mx.clear();
  mx.setPoint(y, x, true);
  mx.update();
  lcd.init();        
  lcd.setCursor(0,0);  
  lcd.print("Bot Started");
  path[pathindex][0]=x;
  path[pathindex][1]=y;
  pathindex++;

}

void loop() {
  int xVal=analogRead(JOYX);
  int yVal=analogRead(JOYY);
  prevX=x; prevY=y;
  if (yVal>600) {
    forleft(1,300);
    y=max(y-1,0);
  } else if (yVal<400) {
    backright(1,300);
    y=min(y+1,maxY);
  } else if(xVal>600) {
    forleft(1,300);
    x=min(x+1,maxX);
  } else if(xVal<400) {
    backright(1,300);
    x=max(x-1,0);
  }
  for(int i=0; i<sizeof(obs) / sizeof(obs[0]); i++){
    if((x ==obs[i][0] && y==obs[i][1])||hightoxin){
      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();
      obsfound[i][0]=x;
      obsfound[i][1]=y;
      x=prevX;
      y=prevY;
      delay(500);
      if (hightoxin) {
        obs[obsindex][0]=x;
        obs[obsindex][1]=y;
        obsindex++;
        x=path[pathindex-2][0];
        y=path[pathindex-2][1];
        mx.setPoint(path[pathindex-2][1],path[pathindex-2][0],true);
        mx.update();
        hightoxin=false;
    }
      lcd.clear();
      break;
    }
  }
 
  for (int i=0;i<sizeof(comp)/sizeof(comp[0]);i++) {
  if (x==comp[i][0]&&y==comp[i][1]) {
    bool alreadyFound=false; 
    for (int j=0;j<componentcount;j++) {
      if (foundcom[j][0]==x && foundcom[j][1]==y) {
        alreadyFound=true;
        break;
      }
    }
    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;  
  }
}
  for (int i = 0;i<sizeof(checkpoints)/sizeof(checkpoints[0]);i++) {
     if (x ==checkpoints[i][0]&&y==checkpoints[i][1]) {
    bool alreadyFound1=false;   
    for (int j=0;j<checkpointscount;j++) {
      if (foundcheck[j][0]==x && foundcheck[j][1]==y) {
        alreadyFound1=true;
        break;
      }
    }
    if(!alreadyFound1){
      foundcheck[checkpointscount][0]=x;
      foundcheck[checkpointscount][1]=y;
      checkpointscount++;
     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);
    }

    break;  
  }
    
  }
  
  if (x!=path[pathindex-1][0]||y!=path[pathindex-1][1]) {
    path[pathindex][0]=x;
    path[pathindex][1]=y;
    pathindex++;
  }
      mx.setPoint(y, x, true);
      mx.update();
  if (receiver.decode()) {
    IRsetup();
    receiver.resume(); 
  }
}

void forleft(int steps,int stepDelay) {
  digitalWrite(DIRL,HIGH);
  digitalWrite(DIRR,HIGH);
  for (int i = 0;i<steps;i++) {
    digitalWrite(STEPL,HIGH);
    digitalWrite(STEPR,HIGH);
    delay(stepDelay);
    digitalWrite(STEPL,LOW);
    digitalWrite(STEPR,LOW);
    delay(stepDelay);
  }
}

void backright(int steps,int stepDelay) {
  digitalWrite(DIRL,LOW);
  digitalWrite(DIRR,LOW);
  for (int i=0;i<steps;i++) {
    digitalWrite(STEPL,HIGH);
    digitalWrite(STEPR,HIGH);
    delay(stepDelay);
    digitalWrite(STEPL,LOW);
    digitalWrite(STEPR,LOW);
    delay(stepDelay);
  }
}
void checkSensor(){
	if (digitalRead(MQ2DPIN)==LOW ) {
    hightoxin=true;
   
  }
}

void IRsetup(){
  switch(receiver.decodedIRData.command){
    case 104:
      IRsetup0();
      break;
    case 48:
      IRsetup1();
      break;

  }

}
void IRsetup0(){
  for(int i=pathindex-1;i>= 0; i-- ){
    Serial.println(path[i][1]);
    Serial.println(path[i][0]);
    mx.setPoint(path[i][1],path[i][0],false);
    mx.update();
    delay(500);
  }
  x=0;
  y=0;
}
void IRsetup1(){
    mx.setPoint(foundcheck[checkpointscount-1][1],foundcheck[checkpointscount-1][0],false);
    mx.update();
    x=foundcheck[checkpointscount-2][0];
    y=foundcheck[checkpointscount-2][1];
  }