int CSpin = 10, CLKpin =13, DINpin=11, devices= 4;                                       // initialising pins for LED matrix

int ver_pin = A1, hor_pin = A0;                                   // initialising pins for joystick and counter variable for Matrix  
int ver_val, hor_val, x=0,y=0;

int dataPIN = 9, latchPIN = 8, clockPIN = 7;        // pins for 74HC595 shift register I
// 0- , 1-verdir, 2-verstep, 3-hordir, 4-horstep, 5-LED , 6-Buzzer, 7-sonicTrig;
int sonicEcho = A2;

int arm_servo = 5;
int water_servo = 6;                                             // initialising analog pins for two servos

int datapin = 2, latchpin = 4, clockpin = 12;                    // pins for 74HC595 shift register II
// 0-LED, 1-armdir, 2-armstep, 3-red, 4-green, 5-, 6- , 7- ;
int blue = 3;                                                    //blue pin of RGB 

#include <MD_MAX72xx.h>
MD_MAX72XX screen = MD_MAX72XX(MD_MAX72XX::PAROLA_HW,CSpin,devices);

#include <Servo.h>
Servo arm, water;

#include <LiquidCrystal_I2C.h>
#include <Wire.h>
LiquidCrystal_I2C display(0x27,16,2);

int obstacle[10][2] = {{7,0},{6,5},{0,5},{3,10},{4,15},{2,20},{1,10},{5,21},{6,18},{7,24}};               //setting obstacles
int path[100][2], detected_obstacle[10][2],J=0,K=0;

void setup() {
  // put your setup code here, to run once:
  screen.begin();
  screen.clear();
  screen.setPoint(3,27,true);
  screen.setPoint(4,27,true);                //LED matrix
  screen.setPoint(0,0,true);
  screen.update();

  display.init();                            //LCD display
  display.backlight();                       

  pinMode(ver_pin, INPUT);                   //Joystick
  pinMode(hor_pin, INPUT);
  
  pinMode(dataPIN, OUTPUT);
  pinMode(latchPIN, OUTPUT);                 //Shift register I
  pinMode(clockPIN, OUTPUT);
  pinMode(sonicEcho, INPUT);

  pinMode(datapin, OUTPUT);
  pinMode(latchpin, OUTPUT);                 //Shift register II
  pinMode(clockpin, OUTPUT);
  pinMode(blue, OUTPUT);

  arm.attach(arm_servo);
  water.attach(water_servo);                //Servos
}

void loop() {
  // put your main code here, to run repeatedly:
  ver_val = analogRead(ver_pin);
  hor_val = analogRead(hor_pin);                      //reads input from joystick
  
  while (ver_val>200 && ver_val<900 && hor_val>200 && hor_val<900) {
    ver_val = analogRead(ver_pin);
    hor_val = analogRead(hor_pin);                    //keeps reading till a movement
  }

  if (ver_val > 900 ) 
    y = max(0,y-1);
  if (ver_val < 200 ) 
    y = min(7,y+1);
  if (hor_val > 900 )                                //updates position according to joystick movement
    x = min(31,x+1);
  if (hor_val < 200 ) 
    x = max(0,x-1);
      
  int temp_3 = 0;
  for(int i=0; i<J; i++){
    if (y == detected_obstacle[i][0] && x == detected_obstacle[i][1]){   //checks if rover hit known obstacle
      display.clear();
      display.setCursor(0,0);
      display.print("CRASHED DETECTED ");                                //displays message on LCD
      display.setCursor(1,4);
      display.print("OBSTACLE");
      screen.clear();
      screen.setPoint(y,x,true);                                         //updates LED matrix
      screen.update();
      
      digitalWrite(latchPIN, LOW);
      shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000110);
      digitalWrite(latchPIN, HIGH);
      delay(250);
      digitalWrite(latchPIN, LOW);
      shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000010);                 //makes LED blink and Buzzer sound
      digitalWrite(latchPIN, HIGH);
      delay(250);
      digitalWrite(latchPIN, LOW);
      shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000110);
      digitalWrite(latchPIN, HIGH);
      delay(250);
      digitalWrite(latchPIN, LOW);
      shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000010);
      digitalWrite(latchPIN, HIGH);
      delay(250);
      temp_3++;
    }
  }

  if (temp_3==0){
    int temp_1 = 0;
    for(int i=0; i<10; i++){
      if (y == obstacle[i][0] && x == obstacle[i][1]){          //checks if rover hit obstacle
        display.clear();
        display.setCursor(2,0);
        display.print("Obstacle");                              //displays message on LCD 
        display.setCursor(2,1);
        display.print("Detected");
        detected_obstacle[J][0] = y;                            //stores the obstacle as detected obstacle
        detected_obstacle[J][1] = x;
        J++;
        screen.clear();
        screen.setPoint(y,x,true);                              //updates the LED matrix with obstacle
        screen.update();

        digitalWrite(latchPIN, LOW);
        shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000110);
        digitalWrite(latchPIN, HIGH);
        delay(250);
        digitalWrite(latchPIN, LOW);
        shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000010);        //LED blinks and BUZZER sounds 
        digitalWrite(latchPIN, HIGH);
        delay(250);
        digitalWrite(latchPIN, LOW);
        shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000110);
        digitalWrite(latchPIN, HIGH);
        delay(250);
        digitalWrite(latchPIN, LOW);
        shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000010);
        digitalWrite(latchPIN, HIGH);
        delay(250);
        temp_1++;
      }
    }
    if(temp_1==0){
      display.clear();
      int temp_2 = 0;
      for(int i=0; i<K; i++)
        if(path[i][0]==y && path[i][1]==x){temp_2++;}            
      if(temp_2==0){
        path[K][0] = y;
        path[K][1] = x;                                        //stores the path used
        K++;
        screen.setPoint(3,27,true);
        screen.setPoint(4,27,true);
        for(int i=0; i<K; i++){
          screen.setPoint(path[i][0],path[i][1],true);        //updates path to LED
          screen.update();
        }
      }
    }
  }
  delay(50);                                      

  if(x==27 && (y==3 || y==4)) {                //destination reached
    display.clear();
    display.setCursor(1,0);
    display.print("INFORMAL PATH");
    display.setCursor(1,1);                     //displays message on LCD
    display.print("IS DETECTED");
    delay(3000);
    screen.clear();
    display.clear();
    display.setCursor(1,0);
    display.print("Back to Lander");
    display.setCursor(1,1);
    display.print("for Recharging");

    for(int i=0; i<K; i++){
      screen.setPoint(3,27,true);
      screen.setPoint(4,27,true);                          //goes back to starting point
      screen.setPoint(0,0,true); 
      for(int j=0; j<K-i-1; j++){
          screen.setPoint(path[j][0],path[j][1],true);        
          screen.update();
        }
      delay(200);
      screen.clear();
    }

    Serial.begin(115200);
    Serial.flush();
    Serial.println("Start autonomous run?? (press 1 for yes)");      //asks input from user through serial monitor
    while(Serial.available()==0) {}
    int yes = Serial.parseInt();
    
    if(yes==1){
      display.clear();
      display.setCursor(1,0);
      display.print("Autonomous Run");                   //display message in LCD

      screen.setPoint(3,27,true);
      screen.setPoint(4,27,true); 
      screen.setPoint(0,0,true); 

      for(int i=1; i<=K; i++){
        for(int j=0; j<i; j++){
          screen.setPoint(path[j][0],path[j][1],true);          //updates the path to LED matrix
          screen.update();
        }
        if(path[i][0] > path[i-1][0]){                          //motion in +y
          digitalWrite(latchPIN, LOW);
          shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00100000);
          digitalWrite(latchPIN, HIGH);
          digitalWrite(latchPIN, LOW);
          shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000000);
          digitalWrite(latchPIN, HIGH);   
        }
        if(path[i][0] < path[i-1][0]) {                         //motion in -y
          digitalWrite(latchPIN, LOW);
          shiftOut(dataPIN, clockPIN, LSBFIRST, 0b01100000);
          digitalWrite(latchPIN, HIGH);
          digitalWrite(latchPIN, LOW);
          shiftOut(dataPIN, clockPIN, LSBFIRST, 0b01000000);
          digitalWrite(latchPIN, HIGH); 
        }
        if(path[i][1] > path[i-1][1]) {                         //motion in +x
          digitalWrite(latchPIN, LOW);
          shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00001000);
          digitalWrite(latchPIN, HIGH);
          digitalWrite(latchPIN, LOW);
          shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000000);
          digitalWrite(latchPIN, HIGH);    
        }
        if(path[i][1] < path[i-1][1]) {                         //motion in -x
          digitalWrite(latchPIN, LOW);
          shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00011000);
          digitalWrite(latchPIN, HIGH);
          digitalWrite(latchPIN, LOW);
          shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00010000);
          digitalWrite(latchPIN, HIGH); 
        }
        delay(200);
      }
      
      int d1, d2, t1, t2;
      
      digitalWrite(latchPIN, LOW);
      shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000001);
      digitalWrite(latchPIN, HIGH);
      delayMicroseconds(10);
      digitalWrite(latchPIN, LOW);
      shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000000);
      digitalWrite(latchPIN, HIGH);
      int duration_1 = pulseIn(sonicEcho, HIGH);
      d1 = duration_1/58;
      t1 = millis();

      Serial.println("Drop the block to calculate gravity: ");
      delay(2000);

      digitalWrite(latchPIN, LOW);
      shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000001);
      digitalWrite(latchPIN, HIGH);
      delayMicroseconds(10);
      digitalWrite(latchPIN, LOW);
      shiftOut(dataPIN, clockPIN, LSBFIRST, 0b00000000);
      digitalWrite(latchPIN, HIGH);
      int duration_2 = pulseIn(sonicEcho, HIGH);
      d2 = duration_2/58;
      t2 = millis();

      int D = d1-d2; int T = (t2-t1)/1000; 
      int a = (2*D)/(T*T);
      Serial.print("Gravity calculated = "); Serial.println(a);

      int h = -1, thetha =0, BlueVAL =0;
      while(h<0 || h>4){                                                         //reading user inputs until entered input is valid
        Serial.parseInt();
        Serial.println("enter the velocity of projection ");
        while(Serial.available()==0 ) {}
        float v = Serial.parseFloat();

        Serial.parseInt();
        Serial.println("enter the angle of projection ");
        while(Serial.available()==0 ) {}
        float i = Serial.parseFloat();      

        Serial.parseInt();
        Serial.println("enter the blue value in RGB ");
        while(Serial.available()==0 ) {}
        BlueVAL = Serial.parseInt();      

        int H = 10, H_dash = 1, s = 50;
        h = H - H_dash + s*tan(i * 3.14/180) - ((s*s*a)/(2*v*v*cos(i *3.14/180)*cos(i*3.14/180)));           //calculating height and angle for arm
        thetha = atan((a*(s/v*cos(i*3.14/180)) - v*sin(i*3.14/180))/(v*cos(i*3.14/180))) * 180/3.14;
      }

      Serial.print("Arm height calculated = "); Serial.println(h);
      Serial.print("Arm angle calculated = "); Serial.println(thetha);

      for(int j=0; j<h; j++){                                    //raising arm till 'h' with stepper motor
        digitalWrite(latchpin, LOW);
        shiftOut(datapin, clockpin, LSBFIRST, 0b00100000);
        digitalWrite(latchpin, HIGH);
        delay(100);
        digitalWrite(latchpin, LOW);
        shiftOut(datapin, clockpin, LSBFIRST, 0b00000000);         
        digitalWrite(latchpin, HIGH);
        delay(100);
      }

      for(int j=0; j<thetha; j++){
        arm.write(j);                                            //tilting arm till 'thetha' with servo
        delay(100);
      }
    
      digitalWrite(latchpin, LOW);
      shiftOut(datapin, clockpin, LSBFIRST, 0b00010000);         //red in RGB to show hot rock
      digitalWrite(latchpin, HIGH);
      display.clear();
      display.setCursor(1,1);
      display.print("stone captured");

      delay(1000);
      
      digitalWrite(latchpin, LOW);
      shiftOut(datapin, clockpin, LSBFIRST, 0b00000000);         
      digitalWrite(latchpin, HIGH);
      analogWrite(blue,255);                                     //blue in RGB to show water being poured
      display.clear();
      display.setCursor(1,1);
      display.print("cooling stone");

      for(int j=0; j<= (100*(float(BlueVAL)/192.00)); j++){
        water.write(j);                                         //tilting water bucket based on RGB colour 
        delay(15);
      }

      digitalWrite(latchpin, LOW);
      shiftOut(datapin, clockpin, LSBFIRST, 0b00010000);          //hex colour in RGB to show cooled rock
      digitalWrite(latchpin, HIGH);
      analogWrite(blue,BlueVAL);
      display.clear();
      display.setCursor(1,1);
      display.print("stone cooled");
 
      int deltaT = 100, L = 2260;
      float Mr = 0.5, Mw= float(BlueVAL)/192.00;
      int S_HEAT = (Mw*L)/(Mr*deltaT);                                //calculating specific heat of rock

      Serial.print("Specific Heat calculated = "); Serial.println(S_HEAT);
      delay(1000);
      int CODE[28], temp = 0;
      while(S_HEAT > 0){
        int x = S_HEAT % 10;
        for(int z=0; z<4; z++){                                 //converting specific heat to serial code
          CODE[temp] = x%2;
          x = x/2;
          temp++;
        }
        S_HEAT = S_HEAT/10;
      }
      
      Serial.print("Specific Heat CODE calculated = "); for(int i=0;i<28;i++) {Serial.print(CODE[i]);};

      int delay_time = 1000;
      unsigned long prev_time = millis(), curr_time = millis();
      for(int y=0; y<28; y++){
        if(CODE[y]==1){
          digitalWrite(latchpin, LOW);
          shiftOut(datapin, clockpin, LSBFIRST, 0b10010100);                         //blinking LED with serial code
          digitalWrite(latchpin, HIGH);
          while(curr_time - prev_time <= delay_time) {curr_time = millis(); }        // alternative to delay
          prev_time = curr_time;
        }
        else {
          digitalWrite(latchpin, LOW);
          shiftOut(datapin, clockpin, LSBFIRST, 0b00010100);
          digitalWrite(latchpin, HIGH);
          while(curr_time - prev_time <= delay_time) {curr_time = millis(); }
          prev_time = curr_time;      
        }
      }
    delay(10000);
    }
  } 
}