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

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

#define TRIG_PIN 2
#define ECHO_PIN 3
#define MAX_DISTANCE 200
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

Servo radarServo;
#define SERVO_PIN 9

const int radarCenterX = 64;
const int radarCenterY = 60;
const int radarRadius = 50;

#define MAX_OBJECTS 50
#define OBJECT_LIFETIME 3000 // عمر الهدف بالمللي ثانية (3 ثواني)

struct Object {
  int x;
  int y;
  unsigned long timestamp; // وقت اكتشاف الهدف
  bool active; // حالة الهدف (نشط/غير نشط)
};
Object objects[MAX_OBJECTS];
int objIndex = 0;

int angle = 0;
int sweepDir = 1;
unsigned long lastSweepTime = 0;
const int sweepInterval = 50; // سرعة المسح بالمللي ثانية

void setup() {
  Serial.begin(9600); // تفعيل المنفذ التسلسلي
  
  // تهيئة شاشة OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // توقف في حالة فشل التهيئة
  }
  
  radarServo.attach(SERVO_PIN);

  // عرض رسالة التهيئة
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(25, 20);
  display.println(F("Radar System"));
  display.setCursor(20, 35);
  display.println(F("Initializing..."));
  display.display();
  delay(2000);
  
  // تهيئة مصفوفة الأهداف
  for(int i = 0; i < MAX_OBJECTS; i++) {
    objects[i].active = false;
    objects[i].x = -100;
    objects[i].y = -100;
  }
}

void loop() {
  unsigned long now = millis();
  
  if(now - lastSweepTime >= sweepInterval) {
    lastSweepTime = now;
    
    // تحريك السيرفو
    radarServo.write(angle);
    
    // قراءة المسافة من السونار
    unsigned int distance = sonar.ping_cm();
    
    // إرسال البيانات للكمبيوتر عبر Serial
    Serial.print(F("Angle: "));
    Serial.print(angle);
    Serial.print(F("°, Distance: "));
    Serial.print(distance);
    Serial.println(F(" cm"));
    
    // تخزين الهدف إذا كانت المسافة صحيحة
    if(distance > 0 && distance <= MAX_DISTANCE) {
      float r = (float)distance / MAX_DISTANCE * radarRadius;
      int x = radarCenterX + r * cos(radians(angle));
      int y = radarCenterY - r * sin(radians(angle));
      
      // تخزين الهدف في المصفوفة
      objects[objIndex].x = x;
      objects[objIndex].y = y;
      objects[objIndex].timestamp = now;
      objects[objIndex].active = true;
      
      // تحديث الفهرس
      objIndex = (objIndex + 1) % MAX_OBJECTS;
    }
    
    // رسم شاشة الرادار
    drawRadar(angle, now);
    
    // تحديث زاوية المسح
    angle += sweepDir * 5;
    
    // عكس الاتجاه عند الوصول للنهايات
    if(angle >= 180) {
      sweepDir = -1;
      angle = 180;
    }
    if(angle <= 0) {
      sweepDir = 1;
      angle = 0;
    }
  }
}

void drawRadar(int sweepAngle, unsigned long currentTime) {
  display.clearDisplay();
  
  // رسم شبكة الرادار
  drawGrid();
  
  // رسم الأهداف النشطة فقط
  drawObjects(currentTime);
  
  // رسم شعاع المسح
  drawScanBeam(sweepAngle);
  
  // رسم معلومات إضافية
  drawInfo(currentTime);
  
  display.display();
}

void drawGrid() {
  // رسم نصف دائرة الرادار الرئيسية
  for(int a = 0; a <= 180; a += 10) {
    int x = radarCenterX + radarRadius * cos(radians(a));
    int y = radarCenterY - radarRadius * sin(radians(a));
    display.drawPixel(x, y, SSD1306_WHITE);
  }
  
  // رسم حلقات المسافات
  for(int r = 10; r <= radarRadius; r += 10) {
    display.drawCircle(radarCenterX, radarCenterY, r, SSD1306_WHITE);
  }
  
  // رسم مركز الرادار
  display.fillCircle(radarCenterX, radarCenterY, 2, SSD1306_WHITE);
  
  // رسم خطوط الاتجاهات
  for(int a = 0; a <= 180; a += 30) {
    int x = radarCenterX + radarRadius * cos(radians(a));
    int y = radarCenterY - radarRadius * sin(radians(a));
    display.drawLine(radarCenterX, radarCenterY, x, y, SSD1306_WHITE);
  }
}

void drawObjects(unsigned long currentTime) {
  int activeCount = 0;
  
  for(int i = 0; i < MAX_OBJECTS; i++) {
    if(objects[i].active) {
      // التحقق من انتهاء صلاحية الهدف
      if(currentTime - objects[i].timestamp > OBJECT_LIFETIME) {
        objects[i].active = false; // تعطيل الهدف المنتهي
      } else {
        // حساب مستوى السطوع بناءً على العمر المتبقي
        unsigned long age = currentTime - objects[i].timestamp;
        float brightness = 1.0 - (float)age / OBJECT_LIFETIME;
        
        // رسم الهدف مع تأثير التلاشي
        int radius = 2;
        display.fillCircle(objects[i].x, objects[i].y, radius, SSD1306_WHITE);
        
        // رسم هالة حول الهدف الجديد
        if(age < 500) { // في أول 500 مللي ثانية
          display.drawCircle(objects[i].x, objects[i].y, radius + 1, SSD1306_WHITE);
        }
        
        activeCount++;
      }
    }
  }
}

void drawScanBeam(int sweepAngle) {
  // رسم شعاع المسح الحالي
  for(int i = 0; i < 3; i++) {
    int a = sweepAngle - i * 2;
    if(a < 0) a = 0;
    if(a > 180) a = 180;
    
    int x = radarCenterX + radarRadius * cos(radians(a));
    int y = radarCenterY - radarRadius * sin(radians(a));
    
    // استخدام خط متقطع للشعاع
    display.drawLine(radarCenterX, radarCenterY, x, y, SSD1306_WHITE);
  }
  
  // تمييز الشعاع الحالي بلون مختلف (عكس اللون)
  int x = radarCenterX + radarRadius * cos(radians(sweepAngle));
  int y = radarCenterY - radarRadius * sin(radians(sweepAngle));
  display.drawLine(radarCenterX, radarCenterY, x, y, SSD1306_INVERSE);
}

void drawInfo(unsigned long currentTime) {
  // عد الأهداف النشطة
  int activeCount = 0;
  for(int i = 0; i < MAX_OBJECTS; i++) {
    if(objects[i].active && (currentTime - objects[i].timestamp <= OBJECT_LIFETIME)) {
      activeCount++;
    }
  }
  
  // عرض معلومات في الزاوية العلوية
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  
  // عرض عدد الأهداف
  display.setCursor(0, 0);
  display.print(F("Obj: "));
  display.print(activeCount);
  
  // عرض زاوية المسح الحالية
  display.setCursor(70, 0);
  display.print(F("Ang: "));
  display.print(angle);
  display.print(F("°"));
  
  // عرض اتجاه المسح
  display.setCursor(0, 10);
  display.print(F("Dir: "));
  display.print(sweepDir == 1 ? ">>" : "<<");
  
  // عرض المسافة الحالية (آخر قراءة)
  display.setCursor(70, 10);
  display.print(F("Dist: "));
  
  // قراءة المسافة الحالية
  unsigned int distance = sonar.ping_cm();
  if(distance > 0 && distance <= MAX_DISTANCE) {
    display.print(distance);
    display.print(F("cm"));
  } else {
    display.print(F("---"));
  }
  
  // رسم مؤشر المسافة
  display.setCursor(0, 55);
  display.print(F("0"));
  display.setCursor(30, 55);
  display.print(F("50"));
  display.setCursor(60, 55);
  display.print(F("100"));
  display.setCursor(90, 55);
  display.print(F("150"));
  display.setCursor(120, 55);
  display.print(F("200"));
}