#include <WiFi.h>
#include <Ultrasonic.h>
#include <OLED_Display_SSD1306.h>

// Define constants
const int TRIGGER_PIN = 34;  // Ultrasonic sensor trigger pin
const int ECHO_PIN = 35;    // Ultrasonic sensor echo pin
const int LED1_PIN = 27;     // LED pin
const int LED2_PIN = 14;     // LED pin
const int OLED_SDA = 33;   // OLED display SDA pin
const int OLED_SCL = 25;   // OLED display SCL pin

// Initialize ultrasonic sensor
Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);

// Initialize OLED display
OLED_Display_SSD1306 display(OLED_SDA, OLED_SCL);

void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Initialize LED pin as output
  pinMode(LED1_PIN, OUTPUT);
  pinMode(LED2_PIN, OUTPUT);

  // Initialize OLED display
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Speed Gun");
  display.display();

  // Initialize ultrasonic sensor
  ultrasonic.begin();
}

void loop() {
  // Measure distance using ultrasonic sensor
  int distance = ultrasonic.read();

  // Calculate speed
  float speed = calculateSpeed(distance);

  // Display speed on OLED display
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Speed: ");
  display.print(speed);
  display.println(" km/h");
  display.display();

  // Blink LED if speed is above a certain threshold
  if (speed > 50) {
    digitalWrite(LED1_PIN, HIGH);
    delay(500);
    digitalWrite(LED1_PIN, LOW);
    delay(500);
  }
  else if (speed < 50) {
    digitalWrite(LED2_PIN, HIGH);
    delay(500);
    digitalWrite(LED2_PIN, LOW);
    delay(500);
  }
  // Wait for 1 second before taking the next measurement
  delay(1000);
}

float calculateSpeed(int distance) {
  // Calculate time taken to travel the distance
  float time = distance / 340.0; // speed of sound in air (m/s)

  // Calculate speed (km/h)
  float speed = distance / time * 3.6;

  return speed;
}