#define trigPin A2
#define echoPin A3
#define wlmotorPin2 2
#define wlmotorPin3 3
#define wlmotorPin4 4
#define wlmotorPin5 5
#define wlmotorPin6 6
#define wlmotorPin7 7



long duration;
int distance;
int dist_threshold = 5;
int curr_angle;
String inputString = ""; // String to hold incoming serial data
boolean stringComplete = false; // Whether the string is complete

void setup() {
  Serial.begin(9600);
  // put your setup code here, to run once:
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  for(int i=2; i<=7; i++){
    pinMode(i, OUTPUT);
  }
}

void loop() {
  int signal1 = map(analogRead(A0),0,1023,0,50);
  while(signal1>=18){
    Serial.println("Cleaning Time!");
    getHeight();
    firstLayer();
    secondLayer();

  }
}


void getHeight_v1(){
  // Measure distance using ultrasonic sensor

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  // Calculate distance in centimeters
  distance = map(duration, 118, 23390, 2, 400);
  //distance = duration * 0.034 / 2;
  // Map distance to percentage
  // binPcnt = map(distance, 0, MAX_DISTANCE_CM, 100, 0);
}

void reverse() {
  Serial.print('R');
}

void forward() {
  Serial.print('F');
}

void right() {
  Serial.print('R');
}

void left() {
  Serial.print('L');
}

void stop() {
  Serial.print('S');
}

void firstLayer(){
  getHeight();
  while(distance<dist_threshold){
    getHeight();
    forward();
    delay(100);
  }
  stop();
  delay(2000);
  
  curr_angle = map(analogRead(A1), 0, 1023, 0, 359); // change this to mpuangle
  while(abs(curr_angle-map(analogRead(A1), 0, 1023, 0, 359))<90){
    right();
    delay(100);
  }
  stop();
  delay(2000);
  forward();
  delay(5000); // change natin ito depende sa iuusog nya sa solar panel

  curr_angle = map(analogRead(A1), 0, 1023, 0, 359); // change this to mpuangle
  while(abs(curr_angle-map(analogRead(A1), 0, 1023, 0, 359))<90){
    right();
    delay(100);
  }
  stop();
  distance = 0;
  delay(2000);
}
void secondLayer(){
  getHeight();
  while(distance<dist_threshold){
    getHeight();
    forward();
    delay(100);
  }
  stop();
  delay(2000);
  
  curr_angle = map(analogRead(A1), 0, 1023, 0, 359); // change this to mpuangle
  while(abs(curr_angle-map(analogRead(A1), 0, 1023, 0, 359))<90){
    left();
    delay(100);
  }
  stop();
  delay(2000);
  forward();
  delay(5000); // change natin ito depende sa iuusog nya sa solar panel

  curr_angle = map(analogRead(A1), 0, 1023, 0, 359); // change this to mpuangle
  while(abs(curr_angle-map(analogRead(A1), 0, 1023, 0, 359))<90){
    left();
    delay(100);
  }
  stop();
  distance = 0;
  delay(2000);
}

void getHeight(){
  while (Serial.available()) {
    char inChar = (char)Serial.read(); // Read a character
    
    if (inChar == '\n') { // Check for newline character indicating end of message
      stringComplete = true; // Set flag to indicate that the string is complete
      break;
    }
    inputString += inChar; // Append the character to the input string
  }

  if (stringComplete) { // If the string is complete
    // Parse the string
    distance = parseDistance(inputString);
    
    // Do something with the parsed distance value
    Serial.print("Distance: ");
    Serial.println(distance);

    // Clear the string for the next input
    inputString = "";
    stringComplete = false;
  }
}
int parseDistance(String str) {
  // Find the position of the colon
  int colonPos = str.indexOf(':');
  
  // Extract the substring after the colon
  String distanceStr = str.substring(colonPos + 2);
  
  // Convert the substring to an integer
  int distance = distanceStr.toInt();
  
  return distance;
}