// #define TRIG_PIN 5
// #define ECHO_PIN 4
// #define DISTANCE_THRESHOLD 30
// #define RELAY_CONTROL_PIN 6
// #define POTENT_PIN 9

// volatile bool fired = false;
// volatile bool inter = false;
// volatile int duration = 0;
// volatile bool newDistance = false;
// volatile bool relayOn = true;
// volatile int threshold = 0;
// void setup() {
//   // put your setup code here, to run once:
//   Serial.begin(115200);
//   pinMode(TRIG_PIN, OUTPUT);
//   pinMode(ECHO_PIN, INPUT);
//   pinMode(RELAY_CONTROL_PIN, OUTPUT);
//   pinMode(POTENT_PIN, INPUT);
//   attachInterrupt(digitalPinToInterrupt(ECHO_PIN), distanceInterrupt, CHANGE);
//   threshold = analogRead(POTENT_PIN) / 40;
//   Serial.println("Threshold: ");
//   Serial.println(threshold);
//   enableRelay();
// }

// void loop() {
//   // Start a new measurement:
//   digitalWrite(TRIG_PIN, HIGH);
//   delayMicroseconds(10);
//   digitalWrite(TRIG_PIN, LOW);
  
//   delay(250);
// }

// void distanceInterrupt() {
//   if (digitalRead(ECHO_PIN) == HIGH) {
//     duration = micros();
//   }
//   else {
//     duration = micros() - duration;
//     newDistance = true;
//   }
//   if (newDistance == true) {
//     int distance = duration/58;
//     Serial.println("\n\n Distance in interrupt: ");
//     Serial.println(distance);
//     newDistance = false;
//     if (distance < threshold && relayOn == true) {
//       disableRelay();
//       relayOn = false;
//     }
//     else if (distance > threshold && relayOn == false) {
//       enableRelay();
//       relayOn = true;
//     }
//   }
// }

// void enableRelay() {
//   digitalWrite(RELAY_CONTROL_PIN, HIGH);
//   Serial.println("Relay Enabled");
// }
// void disableRelay() {
//   digitalWrite(RELAY_CONTROL_PIN, LOW);
//   Serial.println("Relay Disabled");
// }


//------------------------------------------------------
#define ECHO_PIN_1 9
#define TRIG_PIN_1 10
#define ECHO_PIN_2 11
#define TRIG_PIN_2 12
#define DISTANCE_THRESHOLD 30
#define RELAY_CONTROL_PIN 7
#define POTENT_PIN 6

volatile int duration_1 = 0;
volatile int duration_2 = 0;
volatile int distance_1 = 0;
volatile int distance_2 = 0;
volatile bool newDistance = false;
volatile bool relayOn = false;
volatile int threshold = 0;
void setup() {
  // put your setup code here, to run once:
  //Serial.begin(115200);
  pinMode(TRIG_PIN_1, OUTPUT);
  pinMode(ECHO_PIN_1, INPUT);
  pinMode(TRIG_PIN_2, OUTPUT);
  pinMode(ECHO_PIN_2, INPUT);
  pinMode(RELAY_CONTROL_PIN, OUTPUT);
  pinMode(POTENT_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(ECHO_PIN_1), distanceInterrupt1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ECHO_PIN_2), distanceInterrupt2, CHANGE);
  threshold = analogRead(POTENT_PIN) / 40;
  //Serial.println("Threshold: ");
  //Serial.println(threshold);
  disableRelay();
}

void loop() {
  // Start a new measurement:
  digitalWrite(TRIG_PIN_1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_1, LOW);
  digitalWrite(TRIG_PIN_2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_2, LOW);
  
  delay(250);
}

void ARDUINO_ISR_ATTR distanceInterrupt1() {
  //Serial.println("SIGNAL 1");
  if (digitalRead(ECHO_PIN_1) == HIGH) {
    duration_1 = micros();
  }
  else {
    duration_1 = micros() - duration_1;
    distance_1 = duration_1/58;
    newDistance = true;
  }
  if (newDistance == true) {
    newDistance = false;
    if ((distance_1 < threshold || distance_2 < threshold) && relayOn == false) {
      enableRelay();
      relayOn = true;
    }
    else if (distance_1 > threshold && distance_2 > threshold && relayOn == true) {
      disableRelay();
      relayOn = false;
    }
  }
}

void ARDUINO_ISR_ATTR distanceInterrupt2() {
  //Serial.println("SIGNAL 2");
  if (digitalRead(ECHO_PIN_2) == HIGH) {
    duration_2 = micros();
  }
  else {
    duration_2 = micros() - duration_2;
    distance_2 = duration_2/58;
  }
}

void enableRelay() {
  digitalWrite(RELAY_CONTROL_PIN, HIGH);
  //Serial.println("Relay Enabled");
}
void disableRelay() {
  digitalWrite(RELAY_CONTROL_PIN, LOW);
  //Serial.println("Relay Disabled");
}