/*
Code by Harshdeep Singh
*/
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>

// Define OLED width and height
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Define I2C address for the OLED
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Define pins for the ultrasonic sensor
#define TRIG_PIN 9
#define ECHO_PIN 10

// Create a Servo object
Servo myservo;

void setup() {
  // Initialize the servo motor
  myservo.attach(6); // Attach the servo to pin 6

  // Initialize the ultrasonic sensor pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

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

  // Clear the buffer
  display.clearDisplay();

  // Draw the axes and labels
  drawAxes();

  // Initialize serial communication for debugging
  Serial.begin(9600);
}

void loop() {
  // Erase the previous scan
  display.clearDisplay();

  // Draw the axes and labels
  drawAxes();

  // Sweep the servo from 0 to 180 degrees in 30 degree increments
  for (int angle = 0; angle <= 180; angle += 30) {
    myservo.write(angle);
    delay(500); // Wait for the servo to reach the position

    // Get the distance from the ultrasonic sensor
    long distance = getDistance();

    // Plot the point on the OLED
    plotPoint(angle, distance);

    // Display the changes on the screen
    display.display();
  }
}

void drawAxes() {
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  // Draw x-axis (0 to 180)
  display.drawLine(20, 50, 120, 50, SSD1306_WHITE); // X-axis line

  // Draw x-axis tick marks
  for (int i = 30; i <= 180; i += 30) {
    int x = map(i, 0, 180, 20, 120);
    display.drawLine(x, 48, x, 52, SSD1306_WHITE);
  }

  // Label origin and maximum for x-axis
  display.setCursor(15, 52);
  display.print(F("0"));
  display.setCursor(110, 52);
  display.print(F("180"));

  // Draw y-axis (0 to 50)
  display.drawLine(20, 10, 20, 50, SSD1306_WHITE); // Y-axis line

  // Draw y-axis tick marks
  for (int i = 10; i <= 50; i += 10) {
    int y = map(i, 0, 50, 50, 10);
    display.drawLine(18, y, 22, y, SSD1306_WHITE);
  }

  // Label origin and maximum for y-axis
  display.setCursor(2, 45);
  display.print(F("0"));
  display.setCursor(2, 5);
  display.print(F("50"));
}

long getDistance() {
  // Send a 10us pulse to trigger the ultrasonic sensor
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Measure the duration of the echo pulse
  long duration = pulseIn(ECHO_PIN, HIGH);

  // Calculate the distance (cm) based on the duration
  long distance = duration * 0.034 / 2;

  // Cap the distance at 50 cm
  distance = min(distance, 50);

  return distance;
}

void plotPoint(int angle, long distance) {
  // Map angle to x-axis (0 to 180)
  int x = map(angle, 0, 180, 20, 120);

  // Map distance to y-axis (0 to 50)
  int y = map(distance, 0, 50, 50, 10);

  // Draw a small box at the specified coordinate
  display.fillRect(x - 1, y - 1, 3, 3, SSD1306_WHITE);
}