#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;
const int radarCenterY = 32;
const int radarRadius = 30;

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(0,0);
  display.println("Radar Nano OLED");
  display.println("Initializing...");
  display.display();
  delay(2000);
  display.clearDisplay();

  // Draw static radar circle once
  drawRadarCircle();
}

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

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

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

    // Draw sweep line
    drawRadarCircle(); // redraw static circle
    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);

    // Draw angle text every 15°
    if(angle % 15 == 0) {
      int txtX = radarCenterX + (int)((radarRadius + 5) * cos(radians(angle)));
      int txtY = radarCenterY - (int)((radarRadius + 5) * sin(radians(angle)));
      display.setTextSize(1);
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(txtX, txtY);
      display.print(angle);
      display.print((char)248); // درجة ° symbol
    }

    display.display();
  }

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

    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)));

    drawRadarCircle();
    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);

    if(angle % 15 == 0) {
      int txtX = radarCenterX + (int)((radarRadius + 5) * cos(radians(angle)));
      int txtY = radarCenterY - (int)((radarRadius + 5) * sin(radians(angle)));
      display.setTextSize(1);
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(txtX, txtY);
      display.print(angle);
      display.print((char)248);
    }

    display.display();
  }
}

// Function to draw static radar circle
void drawRadarCircle() {
  display.clearDisplay();
  display.drawCircle(radarCenterX, radarCenterY, radarRadius, SSD1306_WHITE);

  // Draw center dot
  display.fillCircle(radarCenterX, radarCenterY, 2, SSD1306_WHITE);

  // Draw angle markers every 15° on circle
  for(int a = 0; a <= 180; a += 15) {
    int mx = radarCenterX + (int)(radarRadius * cos(radians(a)));
    int my = radarCenterY - (int)(radarRadius * sin(radians(a)));
    display.drawPixel(mx, my, SSD1306_WHITE);
  }
}