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

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

// HC-SR04 setup
#define TRIG_PIN 2
#define ECHO_PIN 3
#define MAX_DISTANCE 200 // سم
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// Servo setup
Servo radarServo;
#define SERVO_PIN 9

// Variables
int angle = 0;

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

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

  // Attach Servo
  radarServo.attach(SERVO_PIN);

  // Welcome message
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("Raspberry Pi Nano Radar");
  display.println("Initializing...");
  display.display();
  delay(2000);
  display.clearDisplay();
}

void loop() {
  // Sweep servo from 0 to 180
  for(angle = 0; angle <= 180; angle += 5) {
    radarServo.write(angle);
    delay(200); // small delay to allow servo to reach position

    // Measure distance
    unsigned int distance = sonar.ping_cm();
    if(distance == 0) distance = MAX_DISTANCE; // if out of range

    // Draw radar
    display.clearDisplay();
    display.drawCircle(64, 32, 30, SSD1306_WHITE); // main radar circle
    display.drawLine(64,32, 64 + (int)(30 * cos(radians(angle))), 
                               32 - (int)(30 * sin(radians(angle))), SSD1306_WHITE); // radar sweep

    // Plot detected object
    int objX = 64 + (int)(distance * 30.0 / MAX_DISTANCE * cos(radians(angle)));
    int objY = 32 - (int)(distance * 30.0 / MAX_DISTANCE * sin(radians(angle)));
    display.fillCircle(objX, objY, 2, SSD1306_WHITE);

    display.display();
  }

  // Sweep back from 180 to 0
  for(angle = 180; angle >= 0; angle -= 5) {
    radarServo.write(angle);
    delay(200);

    unsigned int distance = sonar.ping_cm();
    if(distance == 0) distance = MAX_DISTANCE;

    display.clearDisplay();
    display.drawCircle(64, 32, 30, SSD1306_WHITE);
    display.drawLine(64,32, 64 + (int)(30 * cos(radians(angle))), 
                               32 - (int)(30 * sin(radians(angle))), SSD1306_WHITE);

    int objX = 64 + (int)(distance * 30.0 / MAX_DISTANCE * cos(radians(angle)));
    int objY = 32 - (int)(distance * 30.0 / MAX_DISTANCE * sin(radians(angle)));
    display.fillCircle(objX, objY, 2, SSD1306_WHITE);

    display.display();
  }
}