#define led LED_BUILTIN
float S_distance = 0.4500;
float T_limit = 10;

const int echoPin1 = 2; //yellow line
const int trigPin1 = 3; //blue line
const int echoPin2 = 4;
const int trigPin2 = 5;
const int buzzerPin = 8;
const int ledPin = 13;
long duration1;
float distance1;
long duration2;
float distance2;
int data = 0;
float background_1 = 30;
float background_2 = 30;
float Timer1 = 0;
float Timer2 = 0;

//setting background
void setup() {
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  Serial.begin(9600);
  delay(1000);
  Serial.print("----------------------------\n");
  Serial.print("SET UP\n");
  Serial.print("----------------------------\n");
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  background_1 = duration1 * 0.034 / 2;
  Serial.print("background_1: ");
  Serial.print(background_1);
  Serial.println(" cm");
  delay(4.990);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  background_2 = duration2 * 0.034 / 2;
  Serial.print("background_2: ");
  Serial.print(background_2);
  Serial.println(" cm");
  Serial.println("----------------------------");

  Serial.print("3 ");
  delay(1000);
  Serial.print("2 ");
  delay(1000);
  Serial.print("1...\n");
  delay(1000);
  Serial.print("----------------------------\n");
  Serial.println("Recording...");
}

void loop() {
  if (Serial.available() > 0) {
    data = Serial.read();
    if (data == 49) {//command wait used "1"
      Serial.print("----------------------------\n");

      Serial.print("3 ");
      delay(1000);
      Serial.print("2 ");
      delay(1000);
      Serial.print("1...\n");
      delay(1000);
      Serial.print("----------------------------\n");
    }
    else if (data != 10) {//command reset background used "any"
      Serial.print("----------------------------\n");
      digitalWrite(trigPin1, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin1, LOW);
      duration1 = pulseIn(echoPin1, HIGH);
      background_1 = duration1 * 0.034 / 2;
      Serial.print("background_1: ");
      Serial.print(background_1);
      Serial.println(" cm");
      delay(4.990);
      digitalWrite(trigPin2, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin2, LOW);
      duration2 = pulseIn(echoPin2, HIGH);
      background_2 = duration2 * 0.034 / 2;
      Serial.print("background_2: ");
      Serial.print(background_2);
      Serial.println(" cm");
      Serial.print("----------------------------\n");

      Serial.print("3 ");
      delay(1000);
      Serial.print("2 ");
      delay(1000);
      Serial.print("1...\n");
      delay(1000);
      Serial.print("----------------------------");
      Serial.print("Recording...\n");
    }
  }

  //working to detect movement

  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  distance1 = duration1 * 0.034 / 2;
  delay(4.990);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = duration2 * 0.034 / 2;

  delay(49.980); //for test wait 0.05
      
  //when senser 1 is not same as background
  if ((distance1 < (background_1-25) || distance1 > (background_1+25))) {
    // check if sensor 2 is corrupt
    if ((distance2 < (background_2-25) || distance2 > (background_2+25))) {
      Serial.println("Sensor2 corrupt!!!");
      Serial.print("Background: ");
      Serial.print(background_2);
      Serial.println(" cm");
      Serial.print("Sensor2   : ");
      Serial.print(distance2);
      Serial.println(" cm");
      Serial.println("Reset background if possible, type any...");
      Serial.print("----------------------------\n");
      Serial.print("3 ");
      delay(1000);
      Serial.print("2 ");
      delay(1000);
      Serial.print("1...\n");
      delay(1000);
      Serial.print("----------------------------\n");
      Serial.print("Recording...");
    }
    else
    {
      Serial.println("Something pass by (sensor1)\n");
      Serial.println("Time delay ");
      Serial.println(T_limit);
      Serial.println(" sec");
    
      digitalWrite(led,HIGH);
      Timer1 = 0;
      Timer2 = 0;
      Serial.print("----------------------------\n");
      // count time
      for (float t = 0; t < T_limit ; t= t + 0.01) 
      {
        delay(4.990);

        digitalWrite(trigPin1, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin1, LOW);
        duration1 = pulseIn(echoPin1, HIGH);
        distance1 = duration1 * 0.034 / 2;
        delay(4.990);
        digitalWrite(trigPin2, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin2, LOW);
        duration2 = pulseIn(echoPin2, HIGH);
        distance2 = duration2 * 0.034 / 2;

        //when senser 2 is not same as background
        if ((distance2 < (background_2-25) || distance2 > (background_2+25)) && Timer2 <= 0.0000) {
          Timer2 = t;
          Serial.println("Something pass by (sensor2)");
          Serial.println("For ");
          Serial.print(t);
          Serial.print(" sec");
          Serial.print("----------------------------\n");
        }
        else if (t >= 9.95 && Timer2 == 0) { // timeout
          Serial.println("Error timeout");
          Serial.print("----------------------------\n");
        }
        else if ((distance1 >= (background_1-25) && distance1 <= (background_1+25)) && Timer1 <= 0) {
          //for save time sensor 1
          Timer1 = t;
          Serial.println("Something passed by (sensor1)");
          Serial.println("For ");
          Serial.print(t);
          Serial.print(" sec\n");
          Serial.print("----------------------------\n");
        }
        else if (Timer1 > 0 && Timer2 > 0) {
          t = T_limit;
          Serial.println("End");
          Serial.print("----------------------------\n");
        }
      }
      digitalWrite(led,LOW);
      Serial.print("------------Data------------\n");
      Serial.print("Speed = ");
      Serial.print(S_distance/(Timer2), 4);
      Serial.print("m/s\n");
      Serial.print("Length = ");
      Serial.print((S_distance/(Timer2)) * Timer1, 4);
      Serial.print("m\n");

      Serial.print("3 ");
      delay(1000);
      Serial.print("2 ");
      delay(1000);
      Serial.print("1...\n");
      delay(1000);
      Serial.print("----------------------------\n");
      Serial.print("Recording...");
    }
  }


  //when senser 2 is not same as background
  if ((distance2 < (background_2-25) || distance2 > (background_2+25))) {
    // check if sensor 1 is corrupt
    if ((distance1 < (background_1-25) || distance1 > (background_1+25))) {
      Serial.println("Sensor1 corrupt!!!");
      Serial.print("Background: ");
      Serial.print(background_1);
      Serial.println(" cm");
      Serial.print("Sensor1   : ");
      Serial.print(distance1);
      Serial.println(" cm");
      Serial.println("Reset background if possible, type any...");
      Serial.print("----------------------------\n");
      Serial.print("3 ");
      delay(1000);
      Serial.print("2 ");
      delay(1000);
      Serial.print("1...\n");
      delay(1000);
      Serial.print("----------------------------\n");
      Serial.print("Recording...");
    }
    else
    {
      Serial.println("Something pass by (sensor1)\n");
      Serial.println("Time delay ");
      Serial.println(T_limit);
      Serial.println(" sec");
    
      digitalWrite(led,HIGH);
      Timer1 = 0;
      Timer2 = 0;
      Serial.print("----------------------------\n");
      // count time
      for (float t = 0; t < T_limit ; t= t + 0.01) 
      {
        delay(4.990);

        digitalWrite(trigPin1, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin1, LOW);
        duration1 = pulseIn(echoPin1, HIGH);
        distance1 = duration1 * 0.034 / 2;
        delay(4.990);
        digitalWrite(trigPin2, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin2, LOW);
        duration2 = pulseIn(echoPin2, HIGH);
        distance2 = duration2 * 0.034 / 2;

        //when senser 1 is not same as background
        if ((distance1 < (background_1-25) || distance1 > (background_1+25)) && Timer1 <= 0.0000) {
          Timer1 = t;
          Serial.println("Something pass by (sensor1)");
          Serial.println("For ");
          Serial.print(t);
          Serial.print(" sec");
          Serial.print("----------------------------\n");
        }
        else if (t >= 9.95 && Timer1 == 0) { // timeout
          Serial.println("Error timeout");
          Serial.print("----------------------------\n");
        }
        else if ((distance2 >= (background_2-25) && distance2 <= (background_2+25)) && Timer2 <= 0) {
          //for save time sensor 2
          Timer2 = t;
          Serial.println("Something passed by (sensor2)");
          Serial.println("For ");
          Serial.print(t);
          Serial.print(" sec\n");
          Serial.print("----------------------------\n");
        }
        else if (Timer2 > 0 && Timer1 > 0) {
          t = T_limit;
          Serial.println("End");
          Serial.print("----------------------------\n");
        }
      }
      digitalWrite(led,LOW);
      Serial.print("------------Data------------\n");
      Serial.print("Speed = ");
      Serial.print(S_distance/(Timer1), 4);
      Serial.print("m/s\n");
      Serial.print("Length = ");
      Serial.print((S_distance/(Timer1)) * Timer2, 4);
      Serial.print("m\n");

      Serial.print("3 ");
      delay(1000);
      Serial.print("2 ");
      delay(1000);
      Serial.print("1...\n");
      delay(1000);
      Serial.print("----------------------------\n");
      Serial.print("Recording...");
    }
  }
}