#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

#define time 1000

int distance1, distance2;
int speed1, speed2;
char avg_distance1, avg_distance2, avg_speed1, avg_speed2;
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 xt1, xt2, xt3, xt4, xt5, xt6, xt7, xt8, xt9, xt10;
int yt1, yt2, yt3, yt4, yt5, yt6, yt7, yt8, yt9, yt10;
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() {
  int distance_1_sum = 0, distance_2_sum = 0;

  valueStore();
  int distancet1 = xt1 + xt2 + xt3 + xt4 + xt5 + xt6 + xt7 + xt8 + xt9 + xt10;
  int distancet2 = yt1 + yt2 + yt3 + yt4 + yt5 + yt6 + yt7 + yt8 + yt9 + yt10;
  int distance1 = distancet1 / 10;
  int distance2 = distancet2 / 10;
  int d1 = xt10-xt1;
  int d2 = yt10-yt1;
  int timex = time / 1000;
  int speed1 = d1 / timex;
  int speed2 = d2 / timex;

  char avg_distance1[8];
  sprintf(avg_distance1, "%d", distance1);
  char avg_distance2[8];
  sprintf(avg_distance2, "%d", distance2);
  char avg_speed1[8];
  sprintf(avg_speed1, "%d", speed1);
  char avg_speed2[8];
  sprintf(avg_speed2, "%d", speed2);

  if (speed1 < 0) {
    r1 = 5; // Set reverse indicator
  } else {
    r1 = 0; // Reset reverse indicator
  }

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

  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, "D");
u8g2.drawStr(6, 53, ".");
u8g2.drawStr(10, 52, "AvgDi");
u8g2.drawStr(32, 52, "AvgSp");
u8g2.drawStr(10, 60, avg_distance2);
u8g2.drawStr(32, 60, avg_speed2);
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_distance1);
u8g2.drawStr(87, 60, avg_speed1);
u8g2.setDrawColor(2);
u8g2.drawStr(111, 56, "R");
u8g2.setDrawColor(1);
u8g2.drawFrame(109, 47, 7, 13);
u8g2.drawBox(119, 47, r2, 13);
u8g2.drawStr(118, 52, "cm");
u8g2.drawStr(120, 59, "s");
u8g2.setDrawColor(2);
u8g2.drawBox(110, 48, r1, 11);
u8g2.setDrawColor(1);
u8g2.drawLine(118, 53, 124, 53);
u8g2.sendBuffer();

  Serial.print("Speed 1: ");
  Serial.print(speed1);
  Serial.println(" cm/s");
  Serial.print("Speed 2: ");
  Serial.print(speed2);
  Serial.println(" cm/s");

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

int 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
  int distance = (duration * 0.0343) / 2; // Divide by 2 because the pulse travels to the object and back

  return distance;
}

void valueStore() { 
  xt1 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1); 
  yt1 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt2 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt2 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt3 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt3 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt4 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt4 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt5 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt5 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt6 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt6 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt7 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt7 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt8 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt8 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt9 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt9 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  xt10 = measureDistance(TRIGGER_PIN_1, ECHO_PIN_1);
  yt10 = measureDistance(TRIGGER_PIN_2, ECHO_PIN_2);
  delay(time/10);

  x1 = map(xt1, 0, 400, 1, 36); // Scale speed for graph width
  x2 = map(xt2, 0, 400, 1, 36);
  x3 = map(xt3, 0, 400, 1, 36);
  x4 = map(xt4, 0, 400, 1, 36);
  x5 = map(xt5, 0, 400, 1, 36);
  x6 = map(xt6, 0, 400, 1, 36);
  x7 = map(xt7, 0, 400, 1, 36);
  x8 = map(xt8, 0, 400, 1, 36);
  x9 = map(xt9, 0, 400, 1, 36);
  x10 = map(xt10, 0, 400, 1, 36);

  y1 = map(yt1, 0, 400, 1, 36); // Scale speed for graph width
  y2 = map(yt2, 0, 400, 1, 36);
  y3 = map(yt3, 0, 400, 1, 36);
  y4 = map(yt4, 0, 400, 1, 36);
  y5 = map(yt5, 0, 400, 1, 36);
  y6 = map(yt6, 0, 400, 1, 36);
  y7 = map(yt7, 0, 400, 1, 36);
  y8 = map(yt8, 0, 400, 1, 36);
  y9 = map(yt9, 0, 400, 1, 36);
  y10 = map(yt10, 0, 400, 1, 36);
}