#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
#include <NewPing.h>
#include <math.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

// Radar parameters
int angle = 0;
const int radarCenterX = 64;   // مركز نصف الدائرة X
const int radarCenterY = 60;   // مركز نصف الدائرة Y (أسفل الشاشة)
const int radarRadius = 50;    // نصف قطر نصف الدائرة

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

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

  // Attach Servo
  radarServo.attach(SERVO_PIN);

  // Welcome message
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 10);
  display.println("Radar Nano OLED");
  display.println("Initializing...");
  display.display();
  delay(2000);
  display.clearDisplay();

  // Draw static half-circle radar
  drawRadarHalfCircle();
}

void loop() {
  // Sweep servo 0 -> 180
  for(angle = 0; angle <= 180; angle += 5) {
    radarServo.write(angle);
    delay(120);

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

    int objX = radarCenterX + (int)(distance * radarRadius / MAX_DISTANCE * cos(radians(angle)));
    int objY = radarCenterY - (int)(distance * radarRadius / MAX_DISTANCE * sin(radians(angle)));

    // Draw radar background
    drawRadarHalfCircle();

    // Draw sweep line
    display.drawLine(radarCenterX, radarCenterY, 
                     radarCenterX + (int)(radarRadius * cos(radians(angle))),
                     radarCenterY - (int)(radarRadius * sin(radians(angle))), SSD1306_WHITE);

    // Draw detected object
    display.fillCircle(objX, objY, 2, SSD1306_WHITE);

    display.display();
  }

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

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

    int objX = radarCenterX + (int)(distance * radarRadius / MAX_DISTANCE * cos(radians(angle)));
    int objY = radarCenterY - (int)(distance * radarRadius / MAX_DISTANCE * sin(radians(angle)));

    drawRadarHalfCircle();

    display.drawLine(radarCenterX, radarCenterY, 
                     radarCenterX + (int)(radarRadius * cos(radians(angle))),
                     radarCenterY - (int)(radarRadius * sin(radians(angle))), SSD1306_WHITE);

    display.fillCircle(objX, objY, 2, SSD1306_WHITE);

    display.display();
  }
}

// Function to draw static half-circle radar
void drawRadarHalfCircle() {
  display.clearDisplay();
  
  // Draw half circle (arc)
  for(int a=0; a<=180; a+=2) {
    int x = radarCenterX + (int)(radarRadius * cos(radians(a)));
    int y = radarCenterY - (int)(radarRadius * sin(radians(a)));