#define TRIG_A_PIN 23 //  TRIG pin ultrasonic sensor A
#define ECHO_A_PIN 22 //  ECHO pin

#define TRIG_B_PIN 32 //  TRIG pin ultrasonic sensor B
#define ECHO_B_PIN 33 //  ECHO pin

#define LED_A 15
#define LED_B 13

float duration_a, distance_a, limit_a; // ultrasonic sensor A 
float duration_b, distance_b, limit_b;// ultrasonic sensor B
// conter of repeats getting initial distance values
int  counter_a, counter_b,counter_max;


void setup() {
  // begin serial port
  Serial.begin (9600);

  // configure the trigger pin to output mode
  pinMode(TRIG_A_PIN, OUTPUT);
 // configure the echo pin to input mode
  pinMode(ECHO_A_PIN, INPUT);

  // configure the trigger pin to output mode
  pinMode(TRIG_B_PIN, OUTPUT);
  // configure the echo pin to input mode
  pinMode(ECHO_B_PIN, INPUT);

  // a = red  b = green led
  pinMode(LED_A, OUTPUT);
  pinMode(LED_B, OUTPUT);

  counter_a=0;
  counter_b=0;
  //distance to compare measured value -> lower value puts led on
  limit_a=limit_b=30;
  counter_max=2;
}

void loop() {
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_A_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_A_PIN, LOW);
 
  // measure duration of pulse from ECHO pin
  duration_a = pulseIn(ECHO_A_PIN, HIGH);
  
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_B_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_B_PIN, LOW);

 // measure duration of pulse from ECHO pin
  duration_b = pulseIn(ECHO_B_PIN, HIGH);

  // calculate the distance
  distance_a = 0.017 * duration_a;
  distance_b = 0.017 * duration_b;

  // check if several A values are below limit and remove counter in limits 0...counter_max
  if (distance_a < limit_a ) {
     ++counter_a;
     // flash led to tell if distance A is below limit
     FlashLed(LED_A);
     if (counter_a >counter_max) {counter_a = counter_max;}
  } else {
     --counter_a;
     if (counter_a<0) {counter_a=0;}
  }    

  // check if several B values are below limit and remove counter in limits 0...10
  if (distance_b < limit_b ) {
     ++counter_b;
     // flash led to tell if distance B is below limit
     FlashLed(LED_B);
     if (counter_b >counter_max) {counter_b = counter_max;}
  } else {
      --counter_b;
      if (counter_b<0) {counter_b=0;}
  }

//  check which side turtle is and lit the led on thst side
 if (counter_a==counter_max or counter_b==counter_max){
    if (counter_a>counter_b) {
       digitalWrite(LED_A, HIGH);
       digitalWrite(LED_B, LOW);
    }
    if (counter_a<counter_b) {
       digitalWrite(LED_A, LOW);
       digitalWrite(LED_B, HIGH);
    }
  }

  // print the value to Serial Monitor
  Serial.print(counter_a);
  Serial.print("-distanceA: ");
  Serial.print(distance_a);
  Serial.print(" cm    ");
  Serial.print(counter_b);
  Serial.print("-distanceB: ");
  Serial.print(distance_b);
  Serial.println(" cm  ");
  delay(500);
}

//SUBROUTINES-SUBROUTINE-SUBROUTINES-SUBROUTINES-SUBROUTINES
// flash led to tell if distance is under limit
void FlashLed(int x){
  if (digitalRead(x) == LOW) {
     digitalWrite(x, HIGH);
     delay(1);
     digitalWrite(x, LOW);
  }
   // UNflash led to tell if distance B is under limit
  if (digitalRead(x) == HIGH) {
     digitalWrite(x, LOW);
     delay(20);
    digitalWrite(x, HIGH);
  }
}