#include "LedControl.h" //FOR DOT MATRIX 
#include <SPI.h> //FOR SD CARD
#include <SD.h>  //FOR SD CARD
#include <IRremote.h> //FOR IR-REMOTE
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

//FOR LCD SCRN
LiquidCrystal_I2C status(0x27,  16, 2);

//MAXIMUM NUMBER OF MOVEMENTS TRACKED
#define MOVELIMIT 250

//USED FOR MOVEMENT - 
#define leftstep  7
#define rightstep 6
#define leftdir   5
#define rightdir  4

#define vertpin A1
#define horpin A2
#define switchpin 8

//for ON/OFF of LED's
#define ON true
#define OFF false

//Here, 1 is up, 2 is right, 3 is down, 4 is left
int userinput = 0;
int facedir = 4;

//pre declaration of functions (movement)
void step(int direction);
int turn(int direction);

//pre declaration of functions (movement constraints)
int validateinput();

//declaration of function which clears botpath.txt (deletes and creates it again)
void clearbotpath();
//SDCARD PIN
#define SDPIN 10
#define SPI_SPEED SD_SCK_MHZ(4)

//BOT LOCATION
int botloc[2] = {0,0};
int botloctemp[2] = {0, 0};

//SDCARD FILE RELATED
File obstacles;
File botpath;
File checkpoint;
File toxin;

//variables for dot matrix (botpath)
int botpathcolumn, botpathrow;

//variables for dot matrix (OBSTACLES)
int obstacledat[10][2] = {{0,3}, {6,1}, {10,6}, {11,1}, {15,3},{15,6}, {21,4}, {23,1},{26,2},{28,6}};

//variables for dot matrix (checkpoints)
int checkpointdat[4][2] = {{7,7},{15,0},{23,7},{31,0}};
int8_t ischeckpoint = 0;
int chkpntbotpathcolumn, chkpntbotpathrow;
int lastpostemp;
File tempfilebotpath;

//variables for dot matrix (components)
int componentdat[10][2];
int checkpointid;

//USED FOR LED DOT MATRIX DISP
LedControl lc=LedControl(11,13,9,4); 

//IR RECIEVER PIN
#define irpin 3
IRrecv ir(irpin);
decode_results results;
int ircommand = -1;
int hexcodes;

//VARIABLES RELATED TO RECALL 
int rclbotpathcolumn, rclbotpathrow;

//FOR RECALL, predeclaration of few functions
int checkifcheckpoint();
void newbotpath(int id);

//FORWARD DECLARATION OF FUNCTION WHICH BLINKS COORDINATE BOTLOCTEMP n times
void blink(int times);
//FORWARD DECLARATION OF FUNCTION WHICH DISPLAYS BOTPATH
void botpathdisp();


void setup() {
  
  Serial.begin(115200 );

  if (!(SD.begin(SDPIN))){
    while (true){
      Serial.println("222");
    }
  }

  //for LCD
  status.init();
  status.backlight();

  //CREATING REQUIRED TEXT FILES obstacles, botpath, checkpoint and toxin
  obstacles = SD.open("obstacles.txt", FILE_WRITE);
  obstacles.close();
  botpath = SD.open("botpath.txt", FILE_WRITE);
  botpath.print("0 0@");
  botpath.close();
  checkpoint = SD.open("checkpoint.txt",FILE_WRITE);
  checkpoint.close();
  toxin = SD.open("toxin.txt",FILE_WRITE);
  toxin.close();


  pinMode(leftstep,OUTPUT);
  pinMode(rightstep,OUTPUT);
  pinMode(leftdir,OUTPUT);
  pinMode(rightdir,OUTPUT);

  pinMode(vertpin,INPUT);
  pinMode(horpin,INPUT);
  pinMode(switchpin,INPUT);

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

  ir.enableIRIn();

}


void loop() {
  
  while (true){
    
    //JOYSTICK INPUT
    if(analogRead(vertpin) > 700) userinput = 1;
    if(analogRead(horpin) < 400) userinput = 2;
    if(analogRead(vertpin) < 400) userinput = 3;
    if(analogRead(horpin) > 700) userinput = 4;
    if((userinput != 0 && validateinput()) ) break;

    }
    
  //CLEARING LCD AS BOT SHOULD'VE MOVED BY NOW
  //lcd.clear();
  Serial.println("CHECKPNT1");
  //resetting ischeckpoint status as now its either that bot is no longer
  //in a checkpoint, or is tasked one of the checkpoint movements

  
  //IR REMOTE CHECKS
  

  //DETERMINE LOCATION OF BOT AFTER MOVEMENT 
  if (userinput == 1){
    botloctemp[0] = botloc[0];
    botloctemp[1] = botloc[1] - 1;
  }

  else if (userinput == 3){
    botloctemp[0] = botloc[0];
    botloctemp[1] = botloc[1] + 1;
  }

  else if (userinput == 2){
    botloctemp[0] = botloc[0] - 1;
    botloctemp[1] = botloc[1];
  }

  else if (userinput == 4){
    botloctemp[0] = botloc[0] + 1;
    botloctemp[1] = botloc[1];
  }
  Serial.println("CHECKING OBSTACLES SECTION");
  //CHECK IF OBSTACLE:
  obstacles = SD.open("obstacle.txt",FILE_WRITE);
  for(int i = 0; i < 10; i++){
    if(obstacledat[i][0] == botloctemp[0] && obstacledat[i][1] == botloctemp[1]){
      
      obstacles.print(String(botloctemp[0]) + " " + String(botloctemp[1]) + "@");
      status.setCursor(0,0);
      status.print("OBSTACLE HIT!");
      blink(2);

      obstacles.close();
      goto endofloop;
    }
  }
  obstacles.close();
  //CHECKPOINT CHECKS:
  Serial.println("STARTING OF MOVEMENT SECTION");
 ;
  //MOVEMENT:
  if ((userinput - facedir )% 2 == 0){
    step(userinput - facedir == 0 ? 1 : 0);
  }
  else{
    if ( userinput - facedir > 0 || userinput - facedir == -3){
      facedir = turn(1);
    }
    else facedir = turn(0);
    step(1);
  }
  //WRITING BOT PATH TO SD
  botpath = SD.open("botpath.txt",FILE_WRITE);
  botpath.print(String(botloc[0]) + " " + String(botloc[1]) + "@");
  botpath.close();

  
  
  
  //LED MATRIX PRINTER
  endofloop:
  botpathdisp();
  
  userinput = 0;
  ircommand = -1;
  delay(300);

}


void step(int direction){

  if (direction){
    digitalWrite(rightdir,HIGH);
    digitalWrite(leftdir,LOW);
    digitalWrite(rightstep,HIGH);
    digitalWrite(leftstep,HIGH);
    delay(10);
    digitalWrite(rightstep,LOW);
    digitalWrite(leftstep,LOW);

  }
  else{
    digitalWrite(rightdir,LOW);
    digitalWrite(leftdir,HIGH);
    digitalWrite(rightstep,HIGH);
    digitalWrite(leftstep,HIGH);
    delay(10);
    digitalWrite(rightstep,LOW);
    digitalWrite(leftstep,LOW);
  }

  if (facedir == 1){
    if(direction){
      botloc[1] = botloc[1] - 1;
    }
    else{
      botloc[1] = botloc[1] + 1;
    }
  }

  else if(facedir == 3){
    if(direction){
      botloc[1] = botloc[1] + 1;
    }
    else{
      botloc[1] = botloc[1] - 1;
    }
  }
  else if (facedir == 2){
    if(direction){
      botloc[0] = botloc[0] - 1;
    }
    else{
      botloc[0] = botloc[0] + 1;
    }
  }
  else if (facedir == 4){
    if(direction){
      botloc[0] = botloc[0] + 1;
    }
    else{
      botloc[0] = botloc[0] - 1;
    }
  }
 //testing of map corners
 /*Serial.println(botloc[0]);
 Serial.println(botloc[1]);
 Serial.println();*/
 
}
int turn(int direction){

  if(direction){
    
    digitalWrite(leftdir,LOW);
    digitalWrite(leftstep,HIGH);
    delay(10);
    digitalWrite(leftstep,LOW);
    
  }

  else{
    digitalWrite(rightdir,LOW);
    digitalWrite(rightstep,HIGH);
    delay(10);
    digitalWrite(rightstep,LOW);
    
  }
  delay(100);
  

return(userinput);
  
}


int validateinput(){

  if (userinput == 1){
    return botloc[1] == 0 ? 0 : 1;
  }

  if (userinput == 3){
    return botloc[1] == 7 ? 0 : 1;
  }
if (userinput == 2){
    return botloc[0] == 0 ? 0 : 1;
  }

  if (userinput == 4){
    return botloc[0] == 31 ? 0 : 1;
  }
}

void blink(int times){
  int DELAY;
  if (times == 1) DELAY = 300;
  else DELAY = 100;
  for(int i = 0; i<4; i++){
    lc.clearDisplay(i);
    //turns all leds off in the dot matrix disp
  }
  for(int i = 0; i< times; i++){
    lc.setLed(botloctemp[0]/8, botloctemp[1], botloctemp[0] % 8,HIGH);
    delay(DELAY);
    lc.setLed(botloctemp[0]/8, botloctemp[1], botloctemp[0] % 8,LOW);
    delay(DELAY);
  }
  botpathdisp();

}

void botpathdisp(){
  botpath = SD.open("botpath.txt",FILE_READ);
  while(botpath.available()){
    while (botpath.peek() >= '0' && botpath.peek() <= '9') {
      botpathcolumn = botpathcolumn * 10 + (botpath.read() - '0');
    }

    if (botpath.peek() == ' ') {
      botpath.read();
    }
    if (botpath.peek() >= '0' && botpath.peek() <= '9') {
      botpathrow = botpath.read() - '0';
    }
    if (botpath.peek() == '@') {
      botpath.read();
    }
    lc.setLed(botpathcolumn / 8, botpathrow, botpathcolumn % 8, true);
    botpathcolumn = 0;
    botpathrow = 0;
  }
  botpath.close();
}


/*void clearbotpath(){
  Serial.println(SD.remove("filename.txt") ? "Success" : "Failed");
  delay(500);
  botpath = SD.open("botpath.txt",FILE_WRITE);
  botpath.print("0 0@");
  botpath.close();
  botloc[0] = 0;
  botloc[1] = 0;

}*/

/*int checkifcheckpoint(){
  checkpoint = SD.open("checkpoint.txt",FILE_READ);
  int tempcheckpoint[10][2];
  int i = 0;

  while (checkpoint.available()){
    tempcheckpoint[i][0] = 0;
    tempcheckpoint[i][1] = 0;
    int checkpointid = 0;
    while(checkpoint.peek() >= '0' && checkpoint.peek() <= '9'){
      tempcheckpoint[i][0] = tempcheckpoint[i][0] * 10 + checkpoint.read() - 48;
    }
    checkpoint.read();
    tempcheckpoint[i][1] = checkpoint.read() - 48;
    checkpoint.read();
    for(int j = 0; j<4; j++){
      if(tempcheckpoint[j][0] == checkpointdat[j][0] && tempcheckpoint[j][1] == checkpointdat[j][1]){
        checkpointid = j;
      }
    }
  }
  return checkpointid;
}*/
/*void newbotpath(int chckpntid){
  int chkcol = checkpointdat[chckpntid][0];
  int chkrow = checkpointdat[chckpntid][1];
  int tempvarcol = 0, tempvarrow = 0;
  int currpos,apos;
  
  
  botpath = SD.open("botpath.txt",FILE_READ);
  while (botpath.available()){
    while(botpath.peek()>= '0' && botpath.peek()<='9'){
      tempvarcol = tempvarcol * 10 + botpath.read() - 48;
    }
    if (botpath.peek() == ' ') botpath.read();
    if(botpath.peek()>= '0' && botpath.peek()<='9'){
      tempvarrow = botpath.read() - 48;
    }
    currpos = botpath.position();
    botpath.read();
    if(tempvarcol == chkcol && tempvarrow == chkrow) apos = currpos;
  }
  botpath.close();
  botpath = SD.open("botpath.txt",FILE_READ);
  while(botpath.available()){
    while(botpath.peek()>= '0' && botpath.peek()<='9'){
      tempvarcol = tempvarcol * 10 + botpath.read() - 48;
    }
    if (botpath.peek() == ' ') botpath.read();
    if(botpath.peek()>= '0' && botpath.peek()<='9'){
      tempvarrow = botpath.read() - 48;
    }
    if(botpath.position() <= apos){
      lc.setLed(tempvarcol/8, tempvarrow, tempvarcol % 8, false);
    }
    else break;
    tempvarcol = 0;
    tempvarrow = 0;
  }
  botpath.close();
  botpath = SD.open("botpath.txt",FILE_READ);
  tempfilebotpath = SD.open("botpathtemp.txt",FILE_WRITE);
  for(int i=0; i<=apos;i++){
    
    tempfilebotpath.print(botpath.read());

  }
  botpath.close();
  tempfilebotpath.close();
  SD.remove("botpath.txt");
  botpath = SD.open("botpath.txt",FILE_WRITE);
  tempfilebotpath = SD.open("botpathtemp.txt",FILE_READ);
  while(tempfilebotpath.available()){
    botpath.print(tempfilebotpath.read());
  }

}*/

void newbotpath(int chckpntid) {
  int chkcol = checkpointdat[chckpntid][0];
  int chkrow = checkpointdat[chckpntid][1];
  int tempvarcol = 0, tempvarrow = 0;
  int currpos = 0, apos = -1;

  // 1st pass: Find position where checkpoint appears
  botpath = SD.open("botpath.txt", FILE_READ);
  while (botpath.available()) {
    tempvarcol = 0;
    tempvarrow = 0;

    // Read column
    while (botpath.peek() >= '0' && botpath.peek() <= '9') {
      tempvarcol = tempvarcol * 10 + botpath.read() - '0';
    }

    if (botpath.peek() == ' ') botpath.read(); // Skip space

    // Read row
    if (botpath.peek() >= '0' && botpath.peek() <= '9') {
      tempvarrow = botpath.read() - '0';
    }

    currpos = botpath.position();  // Save position AFTER full coordinate
    botpath.read(); // Skip '@'

    // If match, store position
    if (tempvarcol == chkcol && tempvarrow == chkrow) {
      apos = currpos;
    }
  }
  botpath.close();

  if (apos == -1) return; // Checkpoint not found, exit early

  // 2nd pass: Turn off LEDs after checkpoint
  botpath = SD.open("botpath.txt", FILE_READ);
  while (botpath.available()) {
    tempvarcol = 0;
    tempvarrow = 0;

    while (botpath.peek() >= '0' && botpath.peek() <= '9') {
      tempvarcol = tempvarcol * 10 + botpath.read() - '0';
    }

    if (botpath.peek() == ' ') botpath.read();

    if (botpath.peek() >= '0' && botpath.peek() <= '9') {
      tempvarrow = botpath.read() - '0';
    }

    if (botpath.position() <= apos) {
      lc.setLed(tempvarcol / 8, tempvarrow, tempvarcol % 8, false);
    } else {
      break;
    }

    botpath.read(); // skip '@'
  }
  botpath.close();

  // 3rd pass: Copy up to apos into new file
  botpath = SD.open("botpath.txt", FILE_READ);
  tempfilebotpath = SD.open("botpathtemp.txt", FILE_WRITE);
  for (int i = 0; i < apos; i++) {
    tempfilebotpath.write(botpath.read());
  }
  botpath.close();
  tempfilebotpath.close();

  // Replace original file
  SD.remove("botpath.txt");
  tempfilebotpath = SD.open("botpathtemp.txt", FILE_READ);
  botpath = SD.open("botpath.txt", FILE_WRITE);
  while (tempfilebotpath.available()) {
    botpath.write(tempfilebotpath.read());
  }
  tempfilebotpath.close();
  botpath.close();
}