/* #include <MD_MAX72xx.h>
#include <TimerOne.h>

#define	MAX_DEVICES	4

const int maxX = 31;
const int maxY = 7;

#define	CLK_PIN		13
#define	DATA_PIN	11
#define	CS_PIN		10

#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN  2

#define push_button A2

MD_MAX72XX mx = MD_MAX72XX(MD_MAX72XX::PAROLA_HW, CS_PIN, MAX_DEVICES);

int led=7;
int x = 0;
int y = 0;
int newX=0;
int newY=0;
int dt=500;
int point;
int flag=0;
int size;
int obs_[] = {16, 28, 18, 45, 56, 78, 90, 100, 103, 105, 202, 232, 245, 195, 186, 165,};
int obs[sizeof(obs_)/sizeof(obs_[0])][2]={};
int path[224]={};


void setup() {
  mx.begin();
  mx.control(MD_MAX72XX::INTENSITY, MAX_INTENSITY / 2);
  mx.clear();
  Serial.begin(9600);

  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);
  pinMode(push_button, INPUT);

  Timer1.initialize(100000);
  Timer1.attachInterrupt(blink_led);

  for(int k=0;k<224;k++){
    path[k]=-1;
  }
  on(219);
  on(220);
  on(0);

}

void loop(){
  Serial.println(analogRead(push_button));
  delay(100);
  joystick();
}

void joystick() {
  int horz = analogRead(HORZ_PIN);
  int vert = analogRead(VERT_PIN);
  if (!(vert == 512 && horz == 512)) {
    int newX=x; int newY=y;
    if (vert < 300) {
      y = min(y + 1, maxY);
    } if (vert > 700) {
      y = max(y - 1, 0);
    } if (horz > 700) {
      x = min(x + 1, maxX);
    } if (horz < 300) {
      x = max(x - 1, 0);
    } if (digitalRead(SEL_PIN) == LOW) {
      mx.clear();
    }
    int point = y+8*x;
    flag=checkObs(point, obs_);
    if(flag){
      Serial.println("Stop");
      delay(250);
      x=newX;
      y=newY;
    } else {
      Serial.println("Continue");
      on(point);
      delay(250);
    }
  }
}

void blink_led(){
  if(!analogRead(push_button)){
    mx.clear();
    showObs(obs_); }
  else {
    offObs(obs_);
    on(219); on(220);
    showPath(path);
  }
}

void blink(int point){
  int x = point/8;
  int y = point%8;
  mx.setPoint(y,x,true);
  mx.update();
  delay(500);
  mx.setPoint(y,x,false);
  mx.update();
  delay(500);
}

void on(int point){
  int x = point/8;
  int y = point%8;
  mx.setPoint(y, x, true);
  mx.update();
}
void off(int point){
  int x = point/8;
  int y = point%8;
  mx.setPoint(y, x, false);
  mx.update();
}

void showPath(int path[224]){
  for(int i=0;i<224;i++){
    if(path[i]>=0){
      on(path[i]);
      delay(100);
    }
  }
}

void revPath(int path[224]){
  for(int i=224-1;i>0;i--){
    if(path[i]>=0){
      off(path[i]);
      delay(100);
    }
  }
  mx.clear();
}

void showObs(int obs[10]){
  for(int i=0;i<10;i++){
    on(obs[i]);
  }
}
void offObs(int obs[10]){
  for(int i=0;i<10;i++){
    off(obs[i]);
  }
}

int checkObs(int point, int obs[10]){
  for(int k=0;k<10;k++){
    if(point == obs[k]){
      return 1;
    }
  }
  return 0;
} */

#include <MD_MAX72xx.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

//LCD Display object
LiquidCrystal_I2C lcd(0x27,16,4); 

//Dot Matrix LED Display pins
#define MAX_DEVICES 4
#define CLK_PIN 13
#define DATA_PIN 11
#define CS_PIN 10

//Joystick Pins
#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN 2

//Stepper Motor pins
const int STEP_L_PIN=9;
const int STEP_R_PIN=8;
const int STEP_H_PIN=7;
const int DIR_PIN=4;

//Servo Motor pins
const int SERVO1_PIN=3;
const int SERVO2_PIN=5;

Servo servo1; //Catcher tilt servo object
Servo servo2; //Water servo object

//RGB LED pins
const int blueP = 6;

//LED pins for Laser transmission
const int LED_PIN = 12;

//Buzzer pin
const int buzzer=0;

//Push button pin
#define push_button A2

MD_MAX72XX mx = MD_MAX72XX(MD_MAX72XX::PAROLA_HW, CS_PIN, MAX_DEVICES);

int maxX = 31;
int maxY = 7;
int x = 0;
int y = 0;
int capX = 27;
int capY1 = 3, capY2 = 4;
int f = 0;
int j = 0;
int size = 224;
int path[224] = {};
// int obs[] = {16, 26, 19, 45, 56, 78, 90, 102, 103, 105, 202, 195, 186, 165};
int obs[] = {16, 28, 18, 45, 56, 78, 90, 100, 103, 105, 202, 232, 245, 195, 186, 165,};
int len=sizeof(obs)/sizeof(obs[0]);
int obs_count[sizeof(obs)/sizeof(obs[0])]={};

void setup() {
  mx.begin();
  mx.control(MD_MAX72XX::INTENSITY, MAX_INTENSITY / 2);
  mx.clear();

  lcd.init();
  lcd.backlight();

  Serial.begin(9600);
  
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);
  pinMode(push_button, INPUT);
  pinMode(DIR_PIN,OUTPUT);
  pinMode(STEP_L_PIN, OUTPUT);
  pinMode(STEP_R_PIN, OUTPUT);
  pinMode(STEP_H_PIN, OUTPUT);
  pinMode(blueP, INPUT);
  pinMode(LED_PIN, OUTPUT);
  // pinMode(buzzer, OUTPUT);

  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);

  on(219); on(220); on(0);

  path[0] = 0;
  for (int i = 1; i < size; i++) {
    path[i] = -1;
  }
  for(int k=0; k<len; k++){
    obs_count[k]=0;
  }

  //Getting input from user
  //get_input();
}

void loop() {
  int horz = analogRead(HORZ_PIN);
  int vert = analogRead(VERT_PIN);

  if (x == capX && (y == capY1 || y == capY2)) {
    if (f == 0) {
      f = 1;
      int point = y + 8 * x;
      path[j] = point;
      Serial.println("Reached capture spot!!");
      delay(1000);
      Serial.println("Completed the Informal Path finding run!");
      delay(1000);
      Serial.println("Tracing back to lander for recharging");
      revPath(path, size);
      delay(1000);
      Serial.println("Start Autonomous Run? [Y/N]: ");
      while (Serial.available() == 0) {}
      String choice = Serial.readString();
      choice.toLowerCase();
      choice.trim();
      if (choice == "y") {
        Serial.println("Starting Autonomous Run");
        showPath(path, size);
        delay(1000);
        Serial.println("Formal Autonomous Run completed. Reached Capture spot");
      } else if (choice == "n") {
        // Does nothing, remains in current state
      } else {
        Serial.println("Invalid input");
      }
    }
  } else {
    if(trigger()){
      lcd.clear();
      lcd.print("Detected obstacles");
      mx.clear();
      showObs(obs,len);
      delay(1000);
      offObs(obs,len);
      on(219); 
      on(220);
      displayPath(path,size);
    } else {
      int selP = digitalRead(SEL_PIN);
      if (selP == LOW) {
        mx.clear();
        delay(1000);
      } else if (!(vert == 512 && horz == 512)) {
        int newX = x; int newY = y;
        if (vert < 300) {
          y = min(y + 1, maxY);
        }
        if (vert > 700) {
          y = max(y - 1, 0);
        }
        if (horz > 700) {
          if (x < capX) {
            x = min(x + 1, maxX);
          } else {
            Serial.println("Not safe zone!!");
          }
        }
        if (horz < 300) {
          x = max(x - 1, 0);
        }
        int point = y + 8 * x;
        int flag = checkObs(point, obs, len);
        if (flag) {
          int key;
          for(int k=0;k<len;k++){
            if(obs[k]==point){
              key=k;
            }
          }
          if(obs_count[key]==0){
            Serial.println("Obstacle detected");
            obs_count[key]++;
            delay(250);
            mx.clear();
            delay(1000);
            blink1(point);
            start();
            x = newX;
            y = newY;
        } else {
            Serial.println("Crashed on an detected obstacle");
            Serial.println("Motor stops");
            delay(250);
            mx.clear();
            delay(1000);
            blink2(point);
            start();
            x = newX;
            y = newY;
          }
        } else {
          path[++j] = point;
          on(point);
          delay(250);
        }
      }
    }
  }
}

/* void get_input() {
  float ang,vt;       //variables for projection angle and velocity
  float ht,recv_ang;  //variables for recieving height and angle 
  while(1){
    Serial.println("Enter angle: ");
    while(Serial.available()==0){
      //Waits for input
    }
    ang = Serial.parseFloat();
    while(Serial.available() > 0) {
      Serial.read();
    }   //Clears Serial buffer
    if(ang >= 0 && ang <=90){
      break;
    } else {
      Serial.println("Invalid angle!!");
    }
  }
  Serial.print("Angle: ");
  Serial.println(ang);

  Serial.println("Enter Velocity: ");
  while(Serial.available() == 0) {
    //Waits for input
  }
  vt=Serial.parseFloat();
  while(Serial.available() > 0) {
      Serial.read();
    }   //Clears Serial buffer
  Serial.print("Velocity: ");
  Serial.println(vt);

  calculation(&ht,&recv_ang,vt,ang);

  if((ht>=0 && ht<=4) && (recv_ang >=0 && recv_ang<=90)){
    v = vt;
    theta = ang;
    Serial.println("Valid inputs");
    lcd.clear();
    lcd.print("Move Rover");
  } else {
    Serial.println("The sample can't be caught. Please enter valid inputs.");
  }
} */

void displayPath(int path[], int size) {
  for (int i = 0; i < size; i++) {
    if (path[i] >= 0) {
      on(path[i]);
    }
  }
}

void offObs(int obs[], int obsSize) {
  for(int k=0; k<obsSize; k++){
    if(obs_count[k]>0){
      off(obs[k]); 
    }
  }
}

bool trigger(){
  if(!analogRead(push_button) && f==0){
    return true; 
  } else {
    return false;
  }
}

void on(int point) {
  int x = point / 8;
  int y = point % 8;
  mx.setPoint(y, x, true);
  mx.update();
}

void off(int point) {
  int x = point / 8;
  int y = point % 8;
  mx.setPoint(y, x, false);
  mx.update();
}

void blink1(int point) {
  int x = point / 8;
  int y = point % 8;
  mx.setPoint(y, x, true);
  mx.update();
  mx.setPoint(y, x, false);
  mx.update();
  delay(1000);
  mx.setPoint(y, x, true);
  mx.update();
  delay(1000);
  mx.setPoint(y, x, false);
  mx.update();
  delay(1000);
}

void blink2(int point) {
  int x = point / 8;
  int y = point % 8;
  mx.setPoint(y, x, true);
  mx.update();
  delay(1000);
  mx.setPoint(y, x, false);
  mx.update();
  delay(1000);
  mx.setPoint(y, x, true);
  mx.update();
  delay(1000);
  mx.setPoint(y, x, false);
  mx.update();
  delay(1000);
}

void start() {
  mx.begin();
  on(219);
  on(220);
  for(int l=0;l<=j;l++){
    on(path[l]);
  }
}

void showPath(int path[], int size) {
  for (int i = 0; i < size; i++) {
    if (path[i] >= 0) {
      on(path[i]);
      delay(150);
    }
  }
}

void revPath(int path[], int size) {
  for (int i = size - 1; i >= 0; i--) {
    if (path[i] >= 0) {
      if (path[i] == 219 || path[i] == 220) {
        on(path[i]);
      } else {
        off(path[i]);
        delay(150);
      }
    }
  }
}

int checkObs(int point, int obs[], int obsSize) {
  for (int k = 0; k < obsSize; k++) {
    if (point == obs[k]) {
      return 1;
    }
  }
  return 0;
}

void showObs(int obs[], int obsSize) {
  for(int k=0; k<obsSize; k++){
    on(obs[k]);
  }
}