#include <Wire.h>
#include <U8glib.h>

// Pin assignments for ultrasonic sensors
const int trigPin1 = 7, echoPin1 = 6;  // Left sensor
const int trigPin2 = 5, echoPin2 = 4;  // Middle sensor
const int trigPin3 = 3, echoPin3 = 2;  // Right sensor

// Pin for the buzzer
const int buzzerPin = 8;

// Pin for battery level
const int batteryPin = A0;  // Analog pin connected to battery voltage divider

// OLED display setup using U8glib
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);

// Sleep control
unsigned long lastActiveTime = 0;
const unsigned long sleepTime = 180000;  // 3 minutes in milliseconds
bool isSleeping = false;
bool isDisplayOn = true; // Track if the display is on or off

// Battery level (dynamically updated)
int batteryLevel = 100; // Placeholder for battery level

// Buzzer control
unsigned long lastBuzzTime = 0; // Track time for non-blocking buzzer

// Sensor reading intervals	
const unsigned long sensorInterval = 100; // Time between sensor readings
unsigned long lastSensorTime = 0;  // Last time sensors were checked

// Store previous sensor readings and battery level to reduce unnecessary updates
long prevDistance1 = 0, prevDistance2 = 0, prevDistance3 = 0;
int prevBatteryLevel = 100;

// Function to read distance from ultrasonic sensor
long getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);  // Shorter delay to avoid blocking
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH, 20000);  // Reduce timeout to 20ms (around 340 cm)
  if (duration == 0) return 9999;  // Return a large value if no echo detected
  return (duration * 0.034 / 2);  // Convert time to distance in cm
}

// Function to read battery level and return percentage
int readBatteryLevel() {
  int sensorValue = analogRead(batteryPin); // Read the voltage from the battery
  float voltage = (sensorValue / 1023.0) * 5.0; // Convert to voltage (assuming 5V reference)
  int level = (voltage / 4.2) * 100; // Assuming fully charged at 4.2V (adjust as necessary)

  // Clamp the value between 0 and 100
  level = constrain(level, 0, 100);

  return level;
}

// Function to draw a large warning icon with direction indicator
void drawLargeWarningIcon(String direction) {
  // Draw the outline of the triangle instead of a filled one
  u8g.drawLine(44, 10, 84, 10);  // Top side
  u8g.drawLine(44, 10, 64, 50);  // Left side
  u8g.drawLine(84, 10, 64, 50);  // Right side

  // Draw exclamation mark inside the triangle (moved higher)
  u8g.drawBox(62, 15, 4, 20);  // Vertical line for the exclamation mark (y-coordinate raised by 2 pixels)
  u8g.drawCircle(64, 40, 2);   // Dot for the exclamation mark (y-coordinate raised by 3 pixels)

  // Draw direction arrows based on which sensor is closest
  if (direction == "left") {
    // Arrow pointing left
    u8g.drawTriangle(30, 32, 20, 42, 30, 52);  // Left-pointing triangle arrow
  } else if (direction == "middle") {
    // Vertical line to indicate the middle
    u8g.drawBox(60, 15, 8, 35);  // Vertical box to show middle
  } else if (direction == "right") {
    // Arrow pointing right
    u8g.drawTriangle(98, 32, 108, 42, 98, 52);  // Right-pointing triangle arrow
  }
}

// Function to draw the battery icon
void drawBatteryIcon(int level) {
  u8g.setPrintPos(0, 0);
  u8g.setFont(u8g_font_6x10);
  u8g.print("Batt: ");
  u8g.drawFrame(40, 0, 50, 10); // Battery outline
  u8g.drawBox(42, 2, (level / 2) - 2, 6);  // Battery bar length based on level
}

// Function to draw distance readings
void drawDistanceReadings(long distance1, long distance2, long distance3) {
  u8g.setFont(u8g_font_9x18); // Larger font for better visibility
  
  // Calculate center positions for each text line
  int xPos = 64; // Midpoint of 128px width screen

  // Distance for Sensor 1
  u8g.setPrintPos(xPos - (u8g.getStrWidth("S1: 000 cm") / 2), 25);
  u8g.print("S1: ");
  u8g.print(distance1);
  u8g.print(" cm");
  
  // Distance for Sensor 2
  u8g.setPrintPos(xPos - (u8g.getStrWidth("S2: 000 cm") / 2), 40);
  u8g.print("S2: ");
  u8g.print(distance2);
  u8g.print(" cm");

  // Distance for Sensor 3
  u8g.setPrintPos(xPos - (u8g.getStrWidth("S3: 000 cm") / 2), 55);
  u8g.print("S3: ");
  u8g.print(distance3);
  u8g.print(" cm");
}

void setup() {
  Serial.begin(9600);
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(buzzerPin, OUTPUT);
}

void loop() {
  unsigned long currentTime = millis();  // Capture current time

  if (!isSleeping) {
    // Check sensors at regular intervals
    if (currentTime - lastSensorTime >= sensorInterval) {
      lastSensorTime = currentTime;

      // Read battery level dynamically
      batteryLevel = readBatteryLevel();

      long distance1 = getDistance(trigPin1, echoPin1);
      long distance2 = getDistance(trigPin2, echoPin2);
      long distance3 = getDistance(trigPin3, echoPin3);

      String direction = "";
      long minDistance = min(distance1, min(distance2, distance3));

      // If no object is within 350 cm, turn off the display
      if (minDistance > 350) {
        if (isDisplayOn) {
          u8g.sleepOn();  // Turn off the OLED display
          isDisplayOn = false;
        }
      } else {
        // Object detected within 350 cm, turn on the display
        if (!isDisplayOn) {
          u8g.sleepOff();  // Turn on the OLED display
          isDisplayOn = true;
        }
      }

      // Update last active time
      lastActiveTime = millis();

      if (isDisplayOn) {
        // Only update the OLED if it's turned on
        u8g.firstPage();
        do {
          // Beeping logic: only beep if the distance is below a certain threshold
          if (minDistance < 350) {  // Start buzzing from 350 cm
            if (distance1 <= distance2 && distance1 <= distance3) {
              direction = "left";
            } else if (distance2 <= distance1 && distance2 <= distance3) {
              direction = "middle";
            } else {
              direction = "right";
            }

            // Only draw the warning icon if within 5 feet (152 cm)
            if (minDistance <= 152) {
              drawLargeWarningIcon(direction);
            }

            // Non-blocking buzzer control (periodic short beeps)
            if (millis() - lastBuzzTime >= map(minDistance, 350, 50, 1000, 200)) {
              tone(buzzerPin, 1000);  // Start the beep
              delay(100);  // Short beep duration
              noTone(buzzerPin);  // Stop the beep
              lastBuzzTime = millis();  // Update last buzz time
            }
          } else {
            noTone(buzzerPin);  // Turn off the buzzer
          }

          // Draw battery level
          drawBatteryIcon(batteryLevel);

          // Show distance readings if no nearby object is detected
          if (minDistance > 152) {
            drawDistanceReadings(distance1, distance2, distance3);
          }
        } while (u8g.nextPage());
      }

      // Check for sleep mode
      if (millis() - lastActiveTime > sleepTime) {
        isSleeping = true;
      }
    }
  } else {
    // Sleep mode: display a sleeping indicator
    u8g.firstPage();
    do {
      u8g.setFont(u8g_font_9x15);
      u8g.setPrintPos(0, 30);
      u8g.print("SLEEP");
    } while (u8g.nextPage());

    // Wake up condition (any distance detected)
    long distance1 = getDistance(trigPin1, echoPin1);
    long distance2 = getDistance(trigPin2, echoPin2);
    long distance3 = getDistance(trigPin3, echoPin3);

    if (distance1 < 350 || distance2 < 350 || distance3 < 350) {
      isSleeping = false;
      lastActiveTime = millis();  // Reset last active time
    }
  }
}