#include <U8g2lib.h>

U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);

#define TRIGGER_PIN_1 6
#define ECHO_PIN_1 7
#define TRIGGER_PIN_2 13
#define ECHO_PIN_2 12

float speed_1, speed_2;
char avg_distance1[10], avg_distance2[10], avg_speed1[10], avg_speed2[10];
int x1, x2, x3, x4, x5, x6, x7, x8, x9, x10; // For sensor 1
int y1, y2, y3, y4, y5, y6, y7, y8, y9, y10; // For sensor 2
int r1, r2; // For reverse indicators

void setup() {
  pinMode(TRIGGER_PIN_1, OUTPUT);
  pinMode(ECHO_PIN_1, INPUT);
  pinMode(TRIGGER_PIN_2, OUTPUT);
  pinMode(ECHO_PIN_2, INPUT);

  u8g2.begin();
  Serial.begin(9600);
}

void loop() {
  float distance_1_sum = 0, distance_2_sum = 0;
  
  // Measure distance over 1 second 10 times and sum it up
  for (int i = 0; i < 10; i++) {
    distance_1_sum += measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
    distance_2_sum += measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
    delay(100); // Adjust delay as needed
  }
  
  // Calculate average distance over 1 second
  float avg_distance_1 = distance_1_sum / 10;
  float avg_distance_2 = distance_2_sum / 10;
  
  // Calculate speed using the formula: speed = |v| = dr/dt
  speed_1 = avg_distance_1 / 1.0; // Divide by 1 second
  speed_2 = avg_distance_2 / 1.0; // Divide by 1 second
  
  // Store the speed values in variables
  storeSpeedValues(speed_1, speed_2);

  // Check for reverse and update indicators
  checkReverse();

  // Output speeds to serial monitor for debugging
  Serial.print("Speed 1: ");
  Serial.print(speed_1);
  Serial.println(" cm/s");
  Serial.print("Speed 2: ");
  Serial.print(speed_2);
  Serial.println(" cm/s");

  delay(1000); // Adjust delay as needed
}

float measureDistance(int triggerPin, int echoPin) {
  // Send ultrasonic pulse
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  // Measure the duration of the pulse
  unsigned long duration = pulseIn(echoPin, HIGH);

  // Calculate distance based on the speed of sound
  // Speed of sound at sea level = 343 m/s or 0.0343 cm/us
  float distance = (duration * 0.0343) / 2; // Divide by 2 because the pulse travels to the object and back

  return distance;

  u8g2.clearBuffer();
u8g2.setFontMode(1);
u8g2.setBitmapMode(1);
u8g2.setDrawColor(2);
u8g2.drawFrame(2, 7, 7, y1);
u8g2.drawFrame(8, 7, 7, y2);
u8g2.drawFrame(14, 7, 7, y3);
u8g2.drawFrame(20, 7, 7, y4);
u8g2.drawFrame(26, 7, 7, y5);
u8g2.drawFrame(32, 7, 7, y6);
u8g2.drawFrame(38, 7, 7, y7);
u8g2.drawFrame(44, 7, 7, y8);
u8g2.drawFrame(50, 7, 7, y9);
u8g2.drawFrame(56, 7, 7, y10);
u8g2.setDrawColor(1);
u8g2.setFont(u8g2_font_4x6_tr);
u8g2.drawStr(1, 2, " ");
u8g2.setDrawColor(2);
u8g2.drawFrame(64, 7, 7, x1);
u8g2.drawFrame(70, 7, 7, x2);
u8g2.drawFrame(76, 7, 7, x3);
u8g2.drawFrame(82, 7, 7, x4);
u8g2.drawFrame(88, 7, 7, x5);
u8g2.drawFrame(94, 7, 7, x6);
u8g2.drawFrame(100, 7, 7, x7);
u8g2.drawFrame(106, 7, 7, x8);
u8g2.drawFrame(112, 7, 7, x9);
u8g2.drawFrame(118, 7, 7, x10);
u8g2.setDrawColor(1);
u8g2.drawLine(1, 7, 126, 7);
u8g2.drawLine(41, 5, 46, 3);
u8g2.drawLine(41, 1, 46, 3);
u8g2.drawStr(21, 6, "Time1");
u8g2.drawLine(63, 1, 63, 5);
u8g2.drawStr(4, 6, "1s");
u8g2.drawStr(54, 6, "5s");
u8g2.drawStr(117, 6, "5s");
u8g2.drawStr(66, 6, "1s");
u8g2.drawLine(104, 5, 109, 3);
u8g2.drawLine(104, 1, 109, 3);
u8g2.drawStr(84, 6, "Time2");
u8g2.drawLine(3, 55, 4, 60);
u8g2.drawLine(1, 44, 1, 61);
u8g2.drawLine(5, 55, 4, 60);
u8g2.drawStr(3, 52, "S");
u8g2.drawStr(6, 53, ".");
u8g2.drawStr(10, 52, "AvgDi");
u8g2.drawStr(32, 52, "AvgSp");
u8g2.drawStr(10, 60, avg_distance1);
u8g2.drawStr(32, 60, speed_1);
u8g2.setDrawColor(2);
u8g2.drawStr(56, 56, "R");
u8g2.setDrawColor(1);
u8g2.drawFrame(54, 47, 7, 13);
u8g2.drawStr(87, 52, "AvgSp");
u8g2.setDrawColor(2);
u8g2.drawBox(55, 48, 0, 11);
u8g2.setDrawColor(1);
u8g2.drawStr(64, 52, "AvgDi");
u8g2.drawStr(64, 60, avg_distance2);
u8g2.drawStr(87, 60, speed_2);
u8g2.setDrawColor(2);
u8g2.drawStr(111, 56, "R");
u8g2.setDrawColor(1);
u8g2.drawFrame(109, 47, 7, 13);
u8g2.drawBox(119, 47, r1, 13);
u8g2.drawStr(118, 52, "cm");
u8g2.drawStr(120, 59, "s");
u8g2.setDrawColor(2);
u8g2.drawBox(110, 48, r2, 11);
u8g2.setDrawColor(1);
u8g2.drawLine(118, 53, 124, 53);
u8g2.sendBuffer();
}

void storeSpeedValues(float speed1, float speed2) {
  // Store the speed values in variables
  x1 = map(speed1, 0, 36, 0, 7); // Scale speed for graph width
  x2 = map(speed1, 0, 36, 0, 7);
  x3 = map(speed1, 0, 36, 0, 7);
  x4 = map(speed1, 0, 36, 0, 7);
  x5 = map(speed1, 0, 36, 0, 7);
  x6 = map(speed1, 0, 36, 0, 7);
  x7 = map(speed1, 0, 36, 0, 7);
  x8 = map(speed1, 0, 36, 0, 7);
  x9 = map(speed1, 0, 36, 0, 7);
  x10 = map(speed1, 0, 36, 0, 7);

  y1 = map(speed2, 0, 36, 0, 7); // Scale speed for graph width
  y2 = map(speed2, 0, 36, 0, 7);
  y3 = map(speed2, 0, 36, 0, 7);
  y4 = map(speed2, 0, 36, 0, 7);
  y5 = map(speed2, 0, 36, 0, 7);
  y6 = map(speed2, 0, 36, 0, 7);
  y7 = map(speed2, 0, 36, 0, 7);
  y8 = map(speed2, 0, 36, 0, 7);
  y9 = map(speed2, 0, 36, 0, 7);
  y10 = map(speed2, 0, 36, 0, 7);
}

void checkReverse() {
  // Check if the speed is in reverse for sensor 1
  if (x1 == 5) {
    r1 = 5; // Set reverse indicator
  } else {
    r1 = 0; // Reset reverse indicator
  }

  // Check if the speed is in reverse for sensor 2
  if (y1 == 5) {
    r2 = 5; // Set reverse indicator
  } else {
    r2 = 0; // Reset reverse indicator
  }
}