#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//#include <Servo.h>
#include <NewPing.h>

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

#define TRIGGER_PIN  9  
#define ECHO_PIN     10  
#define MAX_DISTANCE 400

int dotX = 0;
int dotY = 0;

//Servo myservo;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {
  Serial.begin(9600);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;
  }
  //myservo.attach(8);
  display.clearDisplay();
  display.display();
  delay(2000);
}

void loop() {
  static int prevDistance[61]; // Store previous distance values for comparison

  for (int i = 0; i <= 60; i++) {
    int pos = map(i, 0, 60, 0, 60); // Convert servo angle to degrees
    //myservo.write(pos);
    delay(15);
    int distance = sonar.ping_cm();
    
    dotX = map(pos, 0, 60, 0, SCREEN_WIDTH); // Map servo angle to screen width
    dotY = map(distance, 0, 400, SCREEN_HEIGHT, 0); // Map distance to screen height
    display.fillRect(dotX,0,1,SCREEN_HEIGHT,SSD1306_BLACK);
    display.drawPixel(dotX, dotY, SSD1306_WHITE);
    //display.drawPixel(prevdotX, prevdotY, SSD1306_BLACK);
    display.display();
    delay(100); // Adjust this delay to control the refresh rate of the display
  }
}