#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <NewPing.h>
#include <math.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define IR_SENSOR_PIN 13
#define TRIGGER_PIN   14
#define ECHO_PIN      12
#define LED_YELLOW_PIN 15 // Connect LED for "TOO CLOSE!" indication
#define LED_GREEN_PIN 2  // Connect LED for "TOO FAR!" indication
#define BUZZER_PIN 27     // Connect Buzzer
#define MAX_DISTANCE  30 // Maximum distance in cm
#define MIN_DISTANCE  4  // Minimum distance in cm

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

bool buzzerNotified = false; // Flag to ensure buzzer notification occurs only once

void setup() {
  Serial.begin(9600);

  // Initialize the OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }

  // Initialize IR sensor pin
  pinMode(IR_SENSOR_PIN, INPUT);

  // Initialize LED and buzzer pins
  pinMode(LED_YELLOW_PIN, OUTPUT);
  pinMode(LED_GREEN_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
}

void loop() {
  // Check if an object is detected by the IR sensor
  if (digitalRead(IR_SENSOR_PIN) == HIGH) {
    // Read distance from the ultrasonic distance sensor
    int distance = sonar.ping_cm();

    // Clear the display
    display.clearDisplay();

    // Check if the distance is within the range (MIN_DISTANCE to MAX_DISTANCE)
    if (distance < MIN_DISTANCE) {
      // Display "TOO FAR!" and light up LED_YELLOW
      displayMessageAndLED("TOO CLOSE", LED_YELLOW_PIN);

    } else if (distance > 12) {
      // Display "TOO CLOSE!" and light up LED_GREEN
      displayMessageAndLED(" TOO FAR", LED_GREEN_PIN);

    } else {
      // Display distance and draw pie graph
      displayDistanceAndPieGraph(distance);

      // Turn off LEDs if distance is within acceptable range
      digitalWrite(LED_YELLOW_PIN, LOW);
      digitalWrite(LED_GREEN_PIN, LOW);
    }
      Serial.println(distance); 

    // Display the content
    display.display();

    // Check if buzzer notification is required
    if (!buzzerNotified) {
      buzzerNotification();
      buzzerNotified = true; // Set the flag to true to ensure notification only once
    }
  } else {
    // If object is not detected, reset the buzzer notification flag
    buzzerNotified = false;
  }
}

void displayMessageAndLED(String message, int ledPin) {
  // Display message on OLED
  display.setTextSize(2.5);
  display.setTextColor(WHITE);
  display.setCursor(10, 25); // (x,y)
  display.println(message);

  // Light up LED
  digitalWrite(ledPin, HIGH);
}

void displayDistanceAndPieGraph(int distance) {
  // Display distance as text
  display.setTextSize(0.4);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("Distance: ");
  display.print(distance);
  display.println(" cm");
  display.startscrollright(0x00, 0x00);
  // Draw pie graph
  drawPieGraph(distance);
}

void drawPieGraph(int distance) {
  // Calculate the angle based on the distance
  int angle = map(distance, MIN_DISTANCE, 12, 360, 0);

  // Draw the pie slice
  int cx = SCREEN_WIDTH / 2;
  int cy = SCREEN_HEIGHT / 1.75;
  int radius = 25;
  float startAngle = radians(-90);
  float endAngle = radians(-90 + angle);
  float angleStep = radians(1); // Adjust this value for smoother or rougher curve

  for (float a = startAngle; a <= endAngle; a += angleStep) {
    int x1 = cx + (radius * cos(a));
    int y1 = cy + (radius * sin(a));
    int x2 = cx;
    int y2 = cy;
    display.drawLine(x1, y1, x2, y2, WHITE);
  }
}

void buzzerNotification() {
  // Turn on buzzer for notification
  digitalWrite(BUZZER_PIN, HIGH);
  delay(500); // Buzzer on for 1 second
  digitalWrite(BUZZER_PIN, LOW); // Turn off the buzzer
  delay(500);
}