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

#define OLED_RESET -1
Adafruit_SSD1306 display(OLED_RESET);

#define TRIGGER_PIN  2
#define ECHO_PIN     3
#define MAX_DISTANCE 200

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

Servo myServo;

void setup() {
  myServo.attach(9);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
}

void loop() {
  for(int pos = 0; pos <= 180; pos += 2) {
    myServo.write(pos);
    delay(20);
    displayRadar(pos);
  }
  for(int pos = 180; pos >= 0; pos -= 2) {
    myServo.write(pos);
    delay(20);
    displayRadar(pos);
  }
}

void displayRadar(int angle) {
  delay(50); 
  int distance = sonar.ping_cm();

  display.clearDisplay();
  display.setCursor(0,0);
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.println("Portable Radar");
  display.setCursor(0, 10);
  display.print("Angle: ");
  display.print(angle);
  display.println(" deg");
  display.print("Distance: ");
  display.print(distance);
  display.println(" cm");

  int x = (int)(64 + (distance * cos(radians(angle))));
  int y = (int)(64 - (distance * sin(radians(angle))));

  display.drawLine(64, 64, x, y, WHITE);
  display.drawCircle(64, 64, distance, WHITE);
  display.display();
}