#define leftSensorPin A1 // Sharp 2Y0A21 sensor (10 to 80 cm) - Left
#define rightSensorPin A2 // Sharp 2Y0A21 sensor (10 to 80 cm) - Right
#define relayPin 2 // Digital pin for relay
#define confirmationPin 3 // Digital pin for confirmation input
#define numReadings 10 // Number of readings to average for the moving average

int leftReadings[numReadings]; // Array to store the left sensor readings
int rightReadings[numReadings]; // Array to store the right sensor readings
int leftReadIndex = 0; // Index of the current left sensor reading
int rightReadIndex = 0; // Index of the current right sensor reading
int leftTotal = 0; // Sum of the left sensor readings
int rightTotal = 0; // Sum of the right sensor readings
int leftAverage = 0; // Average of the left sensor readings
int rightAverage = 0; // Average of the right sensor readings

void setup() {
  Serial.begin(9600);
  pinMode(leftSensorPin, INPUT);
  pinMode(rightSensorPin, INPUT);
  pinMode(relayPin, OUTPUT);
  pinMode(confirmationPin, INPUT);

  for (int i = 0; i < numReadings; i++) {
    leftReadings[i] = 0; // Initialize the left sensor readings array
    rightReadings[i] = 0; // Initialize the right sensor readings array
  }
}

void loop() {
  if (digitalRead(confirmationPin) == HIGH) {
    // Process left sensor readings
    leftTotal = leftTotal - leftReadings[leftReadIndex];
    int leftRawValue = analogRead(leftSensorPin);
    int leftDistance = 123.221814 * exp(-0.005895 * leftRawValue);
    leftTotal = leftTotal + leftDistance;
    leftReadings[leftReadIndex] = leftDistance;
    leftReadIndex = (leftReadIndex + 1) % numReadings;
    leftAverage = leftTotal / numReadings;

    // Process right sensor readings
    rightTotal = rightTotal - rightReadings[rightReadIndex];
    int rightRawValue = analogRead(rightSensorPin);
    int rightDistance = 123.221814 * exp(-0.005895 * rightRawValue);
    rightTotal = rightTotal + rightDistance;
    rightReadings[rightReadIndex] = rightDistance;
    rightReadIndex = (rightReadIndex + 1) % numReadings;
    rightAverage = rightTotal / numReadings;

    // Print the raw values and the averaged distances to the Serial Monitor
    Serial.print("Left Raw Value: ");
    Serial.print(leftRawValue);
    Serial.print(", Left Distance: ");
    Serial.print(leftAverage);
    Serial.println(" cm");

    Serial.print("Right Raw Value: ");
    Serial.print(rightRawValue);
    Serial.print(", Right Distance: ");
    Serial.print(rightAverage);
    Serial.println(" cm");

    // Activate relay if left distance is greater than or equal to right distance
    if (leftAverage >= rightAverage) {
      digitalWrite(relayPin, HIGH);
    } else {
      digitalWrite(relayPin, LOW);
    }

    delay(250); // Approximately 250 ms delay
  } else {
    digitalWrite(relayPin, LOW); // Ensure the relay is off when not in measurement mode
  }
}