simulation_modules.py

# ===================================================================
#    ملف: utils.py (نسخة v2.0 - متوافقة مع الخرائط الديناميكية)
# ===================================================================
import numpy as np
from collections import deque
import cv2
import math
from typing import Dict, List
import torch
from torchvision import transforms
import os
import json
import gzip
import torch
from torch.utils.data import Dataset
from dataclasses import dataclass
from typing import Dict, Tuple, Optional
import time
from dataclasses import dataclass
from typing import Dict, Tuple, Optional


def find_peak_box_and_classify(data):
    """
    [v2.0] تجد القمم في شبكة الكشف وتستخرج معلومات الكائنات.
    هذه النسخة مرنة وتعمل مع أي حجم خريطة.
    """
    # ✅ إصلاح: الحصول على أبعاد الخريطة ديناميكيًا
    H, W, _ = data.shape

    # ✅ إصلاح: إضافة إطار الأمان يعتمد على حجم الخريطة الفعلي
    det_data = np.zeros((H + 2, W + 2, 7))
    det_data[1:H+1, 1:W+1] = data

    detected_objects, object_counts = [], {"car": 0, "bike": 0, "pedestrian": 0, "unknown": 0}

    # ✅ إصلاح: حلقة التكرار تمر على حجم الخريطة الفعلي
    for i in range(1, H + 1):
        for j in range(1, W + 1):
            confidence = det_data[i, j, 0]
            # تم رفع حد الثقة قليلاً لتقليل الاكتشافات الخاطئة
            if confidence > 0.12:
                # البحث عن القمة المحلية
                if (confidence >= det_data[i,j-1,0] and confidence >= det_data[i,j+1,0] and
                    confidence >= det_data[i-1,j,0] and confidence >= det_data[i+1,j,0]):

                    # استخراج البيانات
                    length, width = det_data[i,j,4], det_data[i,j,5]

                    # تصنيف مبسط يعتمد على الأبعاد
                    if length > 3.5: obj_type = 'car'
                    elif length > 1.5 and width > 0.5: obj_type = 'bike'
                    else: obj_type = 'pedestrian'

                    object_counts[obj_type] += 1

                    # ✅ إصلاح: إحداثيات الشبكة الآن نسبة إلى الخريطة الأصلية (i-1, j-1)
                    detected_objects.append({
                        'grid_coords': (i - 1, j - 1),
                        'raw_data': det_data[i, j],
                        'type': obj_type
                    })
    return detected_objects, object_counts

def check_for_nearby_obstacle(meta_data, grid_conf, max_dist=15.0, threshold=0.4):
    """
    [v2.0] تتحقق من وجود أي عائق قريب.
    """
    detected_objects, _ = find_peak_box_and_classify(meta_data)
    if not detected_objects:
        return False

    # استخدام إعدادات الشبكة من القاموس
    x_res = grid_conf['x_res']
    y_res = grid_conf['y_res']
    x_min = grid_conf['x_min']
    y_min = grid_conf['y_min']

    for obj_dict in detected_objects:
        if obj_dict['raw_data'][0] > threshold:
            raw_data = obj_dict['raw_data']
            grid_i, grid_j = obj_dict['grid_coords']
            offset_x, offset_y = raw_data[1], raw_data[2]

            # ✅ إصلاح: استخدام نظام الإحداثيات الصحيح (Y-أمام, X-يسار)
            # لاحظ أن grid_j يرتبط بـ x_rel و grid_i يرتبط بـ y_rel
            x_rel = (grid_j * x_res) + x_min + (x_res / 2) + offset_x
            y_rel = (grid_i * y_res) + y_min + (y_res / 2) + offset_y

            distance = np.linalg.norm([x_rel, y_rel])

            if distance < max_dist:
                print(f"🛑 تم اكتشاف عائق قريب على مسافة {distance:.2f} متر.")
                return True, distance

    # print("✅ لا توجد عائق قريب.")

    return False


# ===================================================================
# وحدات مساعدة (Helpers) - وظائف نقية ومفصولة
# ===================================================================

def convert_grid_to_relative_xy(grid_i, grid_j, grid_conf):
    """
    يحول إحداثيات الشبكة (i, j) إلى إحداثيات مترية نسبية للسيارة (y-أمام, x-جانب).

    Args:
        grid_i (int): مؤشر الصف في الشبكة (المحور الأمامي).
        grid_j (int): مؤشر العمود في الشبكة (المحور الجانبي).
        grid_conf (dict): قاموس يحتوي على أبعاد ودقة الشبكة.

    Returns:
        tuple[float, float]: الإحداثيات النسبية (relative_y, relative_x) لمركز الخلية.
    """
    x_res = grid_conf['x_res']
    y_res = grid_conf['y_res']
    x_min = grid_conf['x_min']
    y_min = grid_conf['y_min']

    # حساب مركز الخلية
    relative_x = (grid_j * x_res) + x_min + (x_res / 2.0)
    relative_y = (grid_i * y_res) + y_min + (y_res / 2.0)

    return relative_y, relative_x

# ===================================================================
# الفئات الرئيسية (Core Classes)
# ===================================================================

class TrackedObject:
    """
    يمثل كائنًا واحدًا يتم تتبعه عبر الزمن.
    يحتفظ بمعرف فريد وتاريخ لمواقعه.
    """
    def __init__(self, obj_id, initial_detection, frame_num):
        self.id = obj_id
        self.type = initial_detection['type']
        self.history = deque(maxlen=20)  # تخزين آخر 20 موقعًا
        self.last_frame_seen = frame_num
        self.last_confidence = 0.0

        self.update(initial_detection, frame_num)

    def update(self, detection, frame_num):
        """تحديث حالة الكائن ببيانات اكتشاف جديدة."""
        self.history.append(detection['global_pos'])
        self.last_frame_seen = frame_num
        self.last_confidence = detection['raw_data'][0]

    @property
    def last_pos(self):
        """الحصول على آخر موقع مسجل للكائن."""
        return self.history[-1] if self.history else None

    def __repr__(self):
        """تمثيل نصي مفيد لتصحيح الأخطاء."""
        return f"Track(ID={self.id}, Type='{self.type}', LastSeen={self.last_frame_seen})"


class Tracker:
    """
    يتتبع الكائنات المكتشفة في خرائط BEV عبر الإطارات.
    يستخدم نظام إحداثيات عالمي للمطابقة والحفاظ على هوية الكائنات.
    """
    def __init__(self, grid_conf, match_threshold=2.5, prune_age=5):
        """
        Args:
            grid_conf (dict): إعدادات الشبكة لتحويل الإحداثيات.
            match_threshold (float): أقصى مسافة (بالمتر) لاعتبار كائن متطابقًا مع مسار.
            prune_age (int): عدد الإطارات التي يجب انتظارها قبل حذف مسار غير نشط.
        """
        self.grid_conf = grid_conf
        self.match_threshold = match_threshold
        self.prune_age = prune_age

        self.tracks = {}  # قاموس لتخزين المسارات باستخدام ID كـ مفتاح
        self.next_track_id = 0

    def process_frame(self, bev_map, ego_pos, ego_theta, frame_num):
        """
        الدالة الرئيسية: تعالج إطارًا واحدًا، تكتشف الكائنات، وتحدث المسارات.
        """
        # 1. اكتشاف الكائنات من خريطة BEV
        # (نفترض أن find_peak_box_and_classify معرفة في مكان آخر)
        detections, _ = find_peak_box_and_classify(bev_map)

        # 2. تحويل مواقع الكائنات إلى إحداثيات عالمية
        self._add_global_positions(detections, ego_pos, ego_theta)

        # 3. مطابقة الاكتشافات بالمسارات الموجودة
        matches, unmatched_detections = self._match_detections_to_tracks(detections)

        # 4. تحديث المسارات المتطابقة
        for track_id, detection_idx in matches.items():
            self.tracks[track_id].update(detections[detection_idx], frame_num)

        # 5. إنشاء مسارات جديدة للاكتشافات غير المتطابقة
        for detection_idx in unmatched_detections:
            self._create_new_track(detections[detection_idx], frame_num)

        # 6. حذف المسارات القديمة (التي لم يتم رؤيتها منذ فترة)
        self._prune_tracks(frame_num)

        return list(self.tracks.values())

    def _add_global_positions(self, detections, ego_pos, ego_theta):
        """يضيف مفتاح 'global_pos' لكل اكتشاف في القائمة."""
        R = np.array([[np.cos(ego_theta), -np.sin(ego_theta)],
                      [np.sin(ego_theta),  np.cos(ego_theta)]])

        for det in detections:
            grid_i, grid_j = det['grid_coords']
            raw_data = det['raw_data']

            # حساب الموقع النسبي (y-أمام, x-جانب)
            relative_y, relative_x = convert_grid_to_relative_xy(grid_i, grid_j, self.grid_conf)
            relative_x += raw_data[1]  # الإزاحة الجانبية بالأمتار
            relative_y += raw_data[2]  # الإزاحة الأمامية بالأمتار

            # التحويل إلى إحداثيات عالمية
            global_offset = R.dot(np.array([relative_y, relative_x]))
            det['global_pos'] = ego_pos + global_offset

    def _match_detections_to_tracks(self, detections):
        """مطابقة بسيطة وجشعة (Greedy Matching) بين الاكتشافات والمسارات."""
        matches = {}
        unmatched_detections = set(range(len(detections)))

        if not self.tracks or not detections:
            return matches, unmatched_detections

        active_track_ids = list(self.tracks.keys())

        for track_id in active_track_ids:
            track = self.tracks[track_id]
            min_dist = self.match_threshold
            best_det_idx = -1

            for i in range(len(detections)):
                if i not in unmatched_detections: continue

                dist = np.linalg.norm(track.last_pos - detections[i]['global_pos'])
                if dist < min_dist:
                    min_dist = dist
                    best_det_idx = i

            if best_det_idx != -1:
                matches[track_id] = best_det_idx
                unmatched_detections.remove(best_det_idx)

        return matches, unmatched_detections

    def _create_new_track(self, detection, frame_num):
        """إنشاء وإضافة مسار جديد إلى القاموس."""
        new_id = self.next_track_id
        new_track = TrackedObject(new_id, detection, frame_num)
        self.tracks[new_id] = new_track
        self.next_track_id += 1

    def _prune_tracks(self, current_frame_num):
        """حذف المسارات التي لم يتم تحديثها منذ فترة طويلة."""
        ids_to_delete = []
        for track_id, track in self.tracks.items():
            if current_frame_num - track.last_frame_seen > self.prune_age:
                ids_to_delete.append(track_id)

        for track_id in ids_to_delete:
            del self.tracks[track_id]


class PIDController:

    def __init__(self, K_P=1.0, K_I=0.0, K_D=0.0, n=20):
        self._K_P, self._K_I, self._K_D = K_P, K_I, K_D
        self._window = deque([0 for _ in range(n)], maxlen=n)
    def step(self, error):
        self._window.append(error)
        integral = np.mean(self._window) if len(self._window) > 1 else 0.0
        derivative = (self._window[-1] - self._window[-2]) if len(self._window) > 1 else 0.0
        return self._K_P * error + self._K_I * integral + self._K_D * derivative
    def reset(self):
        self._window.clear()
        self._window.extend([0 for _ in range(self._window.maxlen)])



class InterfuserController:
    """
    [النسخة 34.0] المتحكم ذو الذاكرة (Memory-Enhanced).
    يعالج مشكلة "التردد المميت" عن طريق:
    1. عدم الثقة بتقديرات السرعة الأولية للكائنات المكتشفة حديثًا.
    2. إضافة "فترة سماح" (ذاكرة قصيرة المدى) للحفاظ على الحذر بعد فقدان أثر
       المركبة التي يتم متابعتها، مما يمنع التسارع غير الآمن.
    """
    def __init__(self, config_dict):
        self.config = config_dict
        c = self.config['controller_params']
        self.freq = self.config.get('frequency', 10.0) # تردد النظام

        # 1. إعداد وحدات التحكم الأساسية
        self.turn_controller = PIDController(c['turn_KP'], c['turn_KI'], c['turn_KD'], c['turn_n'])
        self.speed_controller = PIDController(c['speed_KP'], c['speed_KI'], c['speed_KD'], c['speed_n'])

        # 2. دمج المتتبع
        self.tracker = Tracker(grid_conf=config_dict.get('grid_conf', {}),
                               match_threshold=c.get('tracker_match_thresh', 2.5),
                               prune_age=c.get('tracker_prune_age', 5))

        # 3. إعداد متغيرات الحالة
        self.current_target_speed = 0.0
        self.last_steer = 0

        # [تعديل] إضافة متغيرات الذاكرة والسلوك الحذر
        self.last_followed_track_id = -1
        self.follow_grace_period_timer = 0

        # متغيرات الحالة الأخرى
        self.stop_sign_timer, self.red_light_block_timer = 0, 0
        self.stop_steps_counter, self.forced_move_timer = 0, 0

    def run_step(self, speed, waypoints, junction, traffic_light, stop_sign, bev_map, ego_pos, ego_theta, frame_num):
        active_tracks = self.tracker.process_frame(bev_map, ego_pos, ego_theta, frame_num)
        self._update_system_states(speed, traffic_light)
        steer = self._get_steering_command(waypoints, speed)

        final_goal_speed, reason = self._get_goal_speed(
            speed, waypoints, traffic_light, stop_sign, junction, active_tracks, ego_pos
        )

        self._apply_speed_smoothing(final_goal_speed)
        throttle, brake = self._get_longitudinal_control(speed, self.current_target_speed)

        final_reason = reason if brake else "Cruising"
        if self.forced_move_timer > 0:
            throttle, brake, final_reason = self.config['controller_params']['forced_throttle'], False, "Forced Move"

        return steer, throttle, brake, {'target_speed': self.current_target_speed, 'brake_reason': final_reason, 'active_tracks': len(active_tracks)}

    def _get_goal_speed(self, current_speed, waypoints, traffic_light, stop_sign, junction, active_tracks, ego_pos):
        c = self.config['controller_params']
        max_speed = c['max_speed']

        # القاعدة 1: قواعد السلامة الثابتة
        if (traffic_light > c['light_threshold']): return 0.0, "Red Light"
        if self._is_stop_sign_active(stop_sign, junction): return 0.0, "Stop Sign"

        # القاعدة 2: منطق تجنب العوائق الديناميكي مع الذاكرة
        obstacle_speed_limit, obstacle_reason, is_following = self._obstacle_avoidance_logic(active_tracks, ego_pos, current_speed)
        if is_following:
            return obstacle_speed_limit, obstacle_reason

        # القاعدة 3: منطق الملاحة (نقاط المسار)
        return self._navigation_logic(waypoints, max_speed)

    def _obstacle_avoidance_logic(self, active_tracks, ego_pos, current_speed):
        """[دالة جديدة] تحتوي على كل منطق التعامل مع العوائق."""
        c = self.config['controller_params']
        obstacle_speed_limit = c['max_speed']
        is_following_a_track = False

        if self.follow_grace_period_timer > 0:
            self.follow_grace_period_timer -= 1

        for track in active_tracks:
            distance = np.linalg.norm(track.last_pos - ego_pos)

            if distance < c.get('critical_distance', 4.0):
                self.last_followed_track_id = -1
                return 0.0, f"Critical Obstacle (ID: {track.id})", True

            if distance < c.get('follow_distance', 12.0):
                is_following_a_track = True
                self.last_followed_track_id = track.id
                self.follow_grace_period_timer = c.get('follow_grace_period', int(2 * self.freq)) # ثانيتان من الذاكرة

                track_speed = self._estimate_track_speed(track)

                # [منطق جديد] تجاهل التقديرات الأولية غير الموثوقة
                if track_speed < 0.1 and len(track.history) < 3:
                    # لا تثق بالتقدير، حافظ على سرعتك الحالية مؤقتًا لمنع الفرملة غير الضرورية
                    target_speed = current_speed
                else:
                    target_speed = track_speed * c.get('speed_match_factor', 0.9)

                if target_speed < obstacle_speed_limit:
                    obstacle_speed_limit = target_speed

        if is_following_a_track:
            return obstacle_speed_limit, f"Following ID {self.last_followed_track_id}", True

        # [منطق جديد] إذا لم نعد نرى السيارة ولكن الذاكرة نشطة
        if self.follow_grace_period_timer > 0:
            cautious_speed = c.get('cautious_speed', 5.0) # سرعة منخفضة حذرة
            return min(current_speed, cautious_speed), f"Cautious (Lost Track {self.last_followed_track_id})", True

        return -1, "No Obstacle", False # إشارة لعدم وجود عائق

    def _navigation_logic(self, waypoints, max_speed):
        """[دالة جديدة] تحتوي على منطق الاقتراب من الهدف النهائي."""
        if not waypoints.any(): return max_speed, "Cruising (No Waypoints)"

        APPROACH_ZONE, STOPPING_ZONE, MIN_APPROACH_SPEED = 15.0, 3.0, 2.5
        distance_to_target = np.linalg.norm(waypoints[-1].numpy()) # .numpy() للأمان

        if distance_to_target > APPROACH_ZONE:
            return max_speed, "Cruising"
        elif distance_to_target > STOPPING_ZONE:
            ratio = (distance_to_target - STOPPING_ZONE) / (APPROACH_ZONE - STOPPING_ZONE)
            return MIN_APPROACH_SPEED + ratio * (max_speed - MIN_APPROACH_SPEED), "Approaching Target"
        else:
            return 0.0, "Stopping at Target"

    def _apply_speed_smoothing(self, final_goal_speed):
        """[دالة جديدة] تحتوي على منطق تنعيم السرعة."""
        c = self.config['controller_params']
        accel_rate = c.get('accel_rate', 0.1)
        decel_rate = c.get('decel_rate', 0.2)

        if final_goal_speed > self.current_target_speed:
            self.current_target_speed = min(self.current_target_speed + accel_rate, final_goal_speed)
        elif final_goal_speed < self.current_target_speed:
            self.current_target_speed = max(self.current_target_speed - decel_rate, final_goal_speed)

    def _estimate_track_speed(self, track):
        if len(track.history) < 2: return 0.0
        dist_moved = np.linalg.norm(track.history[-1] - track.history[-2])
        return dist_moved * self.freq # السرعة = المسافة * التردد

    # --- باقي الدوال المساعدة تبقى كما هي ---
    def _is_stop_sign_active(self, stop_sign, junction): # ...
        c = self.config['controller_params']
        is_active = self.stop_sign_timer > 0
        if self.stop_sign_timer > 0: self.stop_sign_timer -= 1
        elif stop_sign > c['stop_threshold']: self.stop_sign_timer = c['stop_sign_duration']
        if junction < 0.1: self.stop_sign_timer = 0
        return is_active

    def _get_longitudinal_control(self, current_speed, target_speed): # ...
        c = self.config['controller_params']
        speed_error = target_speed - current_speed
        control_signal = self.speed_controller.step(speed_error)
        if control_signal > 0:
            throttle, brake = np.clip(control_signal, 0.0, c['max_throttle']), False
        else:
            throttle, brake = 0.0, abs(control_signal) > c['brake_sensitivity']
        if target_speed < c['min_speed']: throttle, brake = 0.0, True
        return throttle, brake

    def _update_system_states(self, speed, traffic_light): # ...
        c = self.config['controller_params']
        if speed < 0.1: self.stop_steps_counter += 1
        else: self.stop_steps_counter = 0
        if self.stop_steps_counter > c['max_stop_time']:
            self.forced_move_timer, self.stop_steps_counter = c['forced_move_duration'], 0
        if self.forced_move_timer > 0: self.forced_move_timer -= 1
        if self.red_light_block_timer > 0: self.red_light_block_timer -= 1
        elif speed < 0.1 and traffic_light > c['light_threshold']:
            self.red_light_block_timer = c['max_red_light_time']
        else:
            self.red_light_block_timer = 0

    def _get_steering_command(self, waypoints, speed):
        if not waypoints.any() or speed < 0.2: return 0.0
        # .numpy() للأمان عند التعامل مع NumPy
        aim_point = (waypoints[1].numpy() + waypoints[0].numpy()) / 2.0
        angle_rad = np.arctan2(aim_point[0], aim_point[1])
        steer = self.turn_controller.step(np.degrees(angle_rad) / -90.0)
        steer = self.last_steer * 0.4 + steer * 0.6
        self.last_steer = steer
        return np.clip(steer, -1.0, 1.0)


def unnormalize_image(tensor: torch.Tensor) -> np.ndarray:
    """
    يعكس عملية تطبيع الصورة في PyTorch ويحولها إلى صورة BGR
    جاهزة للعرض باستخدام OpenCV.

    Args:
        tensor: موتر (Tensor) الصورة المُطبع، بالشكل (C, H, W).

    Returns:
        صورة NumPy array بتنسيق BGR، بالشكل (H, W, C).
    """
    # 1. تحديد قيم المتوسط والانحراف المعياري المستخدمة في التطبيع الأصلي
    # (هذه هي القيم القياسية لمجموعة بيانات ImageNet)
    mean = torch.tensor([0.485, 0.456, 0.406], device=tensor.device)
    std = torch.tensor([0.229, 0.224, 0.225], device=tensor.device)

    # 2. عكس العملية الحسابية: (tensor * std) + mean
    # يتم تغيير شكل mean و std لتتناسب مع أبعاد الموتر (C, H, W)
    tensor = tensor * std[:, None, None] + mean[:, None, None]

    # 3. قص القيم للتأكد من أنها ضمن النطاق [0, 1]
    # قد تتجاوز العملية الحسابية هذا النطاق بشكل طفيف بسبب أخطاء التقريب.
    tensor = torch.clamp(tensor, 0, 1)

    # 4. تحويل الموتر إلى مصفوفة NumPy
    img_np = tensor.cpu().numpy()

    # 5. تغيير ترتيب الأبعاد من (C, H, W) إلى (H, W, C)
    # PyTorch: (القنوات، الارتفاع، العرض)
    # NumPy/OpenCV: (الارتفاع، العرض، القنوات)
    img_np = np.transpose(img_np, (1, 2, 0))

    # 6. تحويل نطاق الألوان من [0, 1] إلى [0, 255] وتغيير النوع
    img_np = (img_np * 255).astype(np.uint8)

    # 7. تحويل قنوات الألوان من RGB إلى BGR
    # معظم مكتبات التعلم العميق (بما في ذلك PyTorch) تستخدم RGB.
    # مكتبة OpenCV تتوقع صيغة BGR.
    img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR)

    return img_bgr
# =========================================================
#  الخطوة 1: دالة world_to_pixel (تبقى كما هي)
# =========================================================
def world_to_pixel(world_points: np.ndarray, grid_size_pixels: tuple, grid_size_meters: tuple) -> np.ndarray:
    pixel_per_meter_x = grid_size_pixels[0] / grid_size_meters[0]
    pixel_per_meter_y = grid_size_pixels[1] / grid_size_meters[1]
    pixel_x = (grid_size_pixels[0] / 2) + (world_points[:, 1] * pixel_per_meter_x)
    pixel_y = grid_size_pixels[1] - (world_points[:, 0] * pixel_per_meter_y)
    return np.vstack((pixel_x, pixel_y)).T

# ===================================================================
# ===================================================================
def render_bev(
    active_tracks: List,
    waypoints_to_draw: np.ndarray,
    ego_pos_global: np.ndarray,
    ego_theta_global: float,
    pixels_per_meter: int = 20,
    grid_size_meters: tuple = (20, 20),
    future_time_steps: tuple = (1.0, 2.0)
) -> Dict[str, np.ndarray]:

    side_m, fwd_m = grid_size_meters
    width_px, height_px = int(side_m * pixels_per_meter), int(fwd_m * pixels_per_meter)

    # ... (كود مصفوفة التحويل وإنشاء الخرائط يبقى كما هو)
    R_world_to_ego = np.array([[math.cos(ego_theta_global), math.sin(ego_theta_global)],
                               [-math.sin(ego_theta_global), math.cos(ego_theta_global)]])
    bev_maps = {
        't0': np.zeros((height_px, width_px, 3), dtype=np.uint8),
        't1': np.zeros((height_px, width_px, 3), dtype=np.uint8),
        't2': np.zeros((height_px, width_px, 3), dtype=np.uint8)
    }


    for track in active_tracks:
        pos_global=track.last_pos;yaw_rad_global=getattr(track,'last_yaw',0);
        speed=getattr(track,'speed',0);extent=getattr(track,'last_extent',(1.0,2.0));
        relative_yaw_rad=yaw_rad_global-ego_theta_global;angle_deg=90-math.degrees(relative_yaw_rad);
        width_px_obj=extent[1]*2*pixels_per_meter;
        length_px_obj=extent[0]*2*pixels_per_meter;
        box_size_px=(float(width_px_obj),float(length_px_obj));
        relative_pos_t0=R_world_to_ego.dot(pos_global-ego_pos_global);
        center_pixel_t0=world_to_pixel(np.array([relative_pos_t0]),(width_px,height_px),grid_size_meters)[0];
        box_points_t0=cv2.boxPoints(((float(center_pixel_t0[0]),float(center_pixel_t0[1])-40),box_size_px,angle_deg));
        cv2.drawContours(bev_maps['t0'],[box_points_t0.astype(np.int32)],0,(0,0,255),2);
        for i,t in enumerate(future_time_steps):
            offset=np.array([math.cos(yaw_rad_global),math.sin(yaw_rad_global)])*speed*t;
            future_pos_global=pos_global+offset;
            relative_pos_future=R_world_to_ego.dot(future_pos_global-ego_pos_global);
            center_pixel_future=world_to_pixel(np.array([relative_pos_future]),(width_px,height_px-40),grid_size_meters)[0];key=f't{i+1}';box_points_future=cv2.boxPoints(((float(center_pixel_future[0]),float(center_pixel_future[1])),box_size_px,angle_deg));
            cv2.drawContours(bev_maps[key],[box_points_future.astype(np.int32)],0,(255,0,128),2);
    ego_center_pixel=((width_px/2)+20,height_px);
    ego_size_px=(1.8*pixels_per_meter,3.8*pixels_per_meter);
    ego_box=cv2.boxPoints((ego_center_pixel,ego_size_px,0));
    for key in bev_maps:
        cv2.drawContours(bev_maps[key],[ego_box.astype(np.int32)],0,(0,255,255),-1);

    # =========================================================================
    # =========================================================================
    if waypoints_to_draw is not None and waypoints_to_draw.size > 0:
        waypoints_corrected = waypoints_to_draw.copy()
        waypoints_corrected[:, 1] *= 1  # [Forward, Left] -> [Forward, -Left] which is [Forward, Right]


        waypoints_pixels = world_to_pixel(
            waypoints_corrected,
            (width_px, height_px), 
            grid_size_meters
        )

        for point in waypoints_pixels:
            center = (int(point[0])+20, int(point[1]))
            cv2.circle(
                bev_maps['t0'],
                center,
                radius=3,           
                color=(0, 255, 0), 
                thickness=-1,      
                lineType=cv2.LINE_AA
            )

    return bev_maps
# =========================================================
# # (دالة world_to_pixel تبقى كما هي من الرد السابق)
# def world_to_pixel(world_points, grid_size_pixels, grid_size_meters):
#     pixel_per_meter_x = grid_size_pixels[0] / grid_size_meters[0]
#     pixel_per_meter_y = grid_size_pixels[1] / grid_size_meters[1]

#     pixel_x = (world_points[:, 1] * pixel_per_meter_x) + (grid_size_pixels[0] / 2)
#     pixel_y = (grid_size_pixels[1]) - (world_points[:, 0] * pixel_per_meter_y)
#     return np.vstack((pixel_x, pixel_y)).T

# ==========================================================
#      الدالة الثانية: generate_bev_image (النسخة النهائية)
# ==========================================================
def generate_bev_image(sample, grid_size_pixels=(400, 400), grid_size_meters=(20, 20)):
    """
    تأخذ عينة بيانات وتنشئ صورة BEV خام باستخدام OpenCV.
    :return: صورة (numpy array) بتنسيق BGR.
    """
    side_meters, forward_meters = grid_size_meters
    width_px, height_px = grid_size_pixels

    bev_image = np.zeros((height_px, width_px, 3), dtype=np.uint8)

    route_data = sample['measurements'].get('route', [])
    if route_data:
        route_world = np.array([[point[0], point[1]] for point in route_data])
        route_pixels = world_to_pixel(route_world, (width_px, height_px), (side_meters, forward_meters))
        cv2.polylines(bev_image, [route_pixels.astype(np.int_)], isClosed=False, color=(0, 255, 0), thickness=2)

    objects_data = sample['objects']
    for obj in objects_data:
        obj_class = obj.get('class', 'غير معروف')
        if obj_class in ['weather', 'ego_info', 'static']: continue

        pos = obj.get('position', [0,0,0])
        ext = obj.get('extent', [0,0,0])
        yaw = obj.get('yaw', 0)

        center_world = np.array([[pos[0], pos[1]]])
        center_pixel = world_to_pixel(center_world, (width_px, height_px), (side_meters, forward_meters))[0]

        length_m, width_m = ext[0]*2, ext[1]*2

        width_px_obj = width_m * (width_px / side_meters)
        length_px_obj = length_m * (height_px / forward_meters)

        color = (255, 255, 0) if obj_class == 'ego_car' else (0, 0, 255)

        center_tuple = (float(center_pixel[0]), float(center_pixel[1]))
        # ملاحظة: OpenCV تتوقع (العرض، الطول) في size_tuple
        size_tuple = (float(width_px_obj), float(length_px_obj))
        angle_deg = np.degrees(yaw) # عكس الزاوية لتناسب OpenCV

        box = cv2.boxPoints((center_tuple, size_tuple, angle_deg))
        box = box.astype(np.int_)
        cv2.drawContours(bev_image, [box], 0, color, 2)

    return bev_image


# # ==========================================================
# #       الكلاس الثالث: الواجهة الاحترافية للعرض
# # ==========================================================


# ==============================================================================
#                      الكلاس الأول: إعدادات العرض
# ==============================================================================
@dataclass
class DisplayConfig:
    width: int = 1920
    height: int = 1080
    camera_ratio: float = 0.65
    panel_margin: int = 30
    section_spacing: int = 25
    min_section_height: int = 180

# ==============================================================================
#               الكلاس الثاني: الواجهة الاحترافية للعرض (النسخة المحسنة)
# ==============================================================================
class DisplayInterface:
    def __init__(self, config: Optional[DisplayConfig] = None):
        self.config = config if config else DisplayConfig()
        self._init_colors_and_fonts()
        self._error_log = []

    def _init_colors_and_fonts(self):
        self.colors = {'panel_bg':(28,30,34), 'text':(240,240,240), 'text_header':(0,165,255), 'separator':(75,75,75), 'throttle':(100,200,100), 'brake':(30,30,240), 'steer_bar':(0,190,255), 'steer_neutral':(100,100,100), 'light_red':(30,30,240), 'light_green':(100,200,100), 'stop_sign':(200,100,255), 'gauge_bg':(50,50,50), 'gauge_needle':(30,30,240), 'error':(30,30,240)}
        self.fonts = {'header': cv2.FONT_HERSHEY_DUPLEX, 'normal': cv2.FONT_HERSHEY_SIMPLEX}

    def run_interface(self, data: Dict) -> np.ndarray:
        """الدالة الرئيسية التي تنشئ وتعيد لوحة التحكم النهائية."""
        try:
            dashboard = np.zeros((self.config.height, self.config.width, 3), dtype=np.uint8)
            cam_w = int(self.config.width * self.config.camera_ratio)

            # --- 1. رسم المكونات الرئيسية ---
            self._draw_camera_view(dashboard, data.get('camera_view'), cam_w)
            self._draw_info_overlay(dashboard, data, cam_w)

            # --- 2. رسم لوحة المعلومات الجانبية ---
            dashboard[:, cam_w:] = self.colors['panel_bg']
            current_y = self.config.panel_margin
            current_y = self._draw_bev_maps_section(dashboard, data, cam_w, current_y)
            self._draw_controls_section(dashboard, data, cam_w, current_y + self.config.section_spacing)

            return dashboard
        except Exception as e:
            self._log_error(str(e))
            return self._create_error_display(str(e))

    # --------------------------------------------------------------------------
    #                         الدوال المساعدة للرسم
    # --------------------------------------------------------------------------

    def _draw_camera_view(self, db: np.ndarray, view: np.ndarray, cam_w: int):
        if view is not None:
            db[:, :cam_w] = cv2.resize(view, (cam_w, self.config.height))

    def _draw_info_overlay(self, db: np.ndarray, data: Dict, cam_w: int):
        """يرسم شريط المعلومات الشفاف فوق عرض الكاميرا."""
        overlay = np.zeros_like(db); panel_h=90; alpha=0.6
        cv2.rectangle(overlay, (0, self.config.height-panel_h), (cam_w, self.config.height), self.colors['panel_bg'], -1)
        db[:,:cam_w] = cv2.addWeighted(db[:,:cam_w], 1, overlay[:,:cam_w], alpha, 0)

        base_y = self.config.height - panel_h + 35
        self._draw_text(db, f"Frame: {data.get('frame_num', 'N/A')}", (20, base_y), 1.2)
# ============
        counts=data.get('object_counts',{});
        self._draw_text(db, f"Detections: C:{counts.get('car',0)}", (20, base_y+35), 1.0, font_type='normal')
        # self._draw_text(db, f"Detections: C:{counts.get('car',0)} B:{counts.get('bike',0)} P:{counts.get('pedestrian',0)}",
                                  # (20, base_y + 35), self.fonts['normal'], 1.0)

        right_x=cam_w-320
        light_prob = data.get('light_prob', 0.0)
        light_color = self.colors['light_red'] if light_prob > 0.5 else self.colors['light_green']
        cv2.circle(db, (right_x, base_y), 12, light_color, -1)
        self._draw_text(db, f"Light: {light_prob:.2f}", (right_x+25, base_y+8), 1.0, font_type='normal')

        stop_prob = data.get('stop_prob', 0.0)
        stop_color = self.colors['stop_sign'] if stop_prob > 0.5 else self.colors['steer_neutral']
        cv2.circle(db, (right_x, base_y+35), 12, stop_color, -1)
        self._draw_text(db, f"Stop: {stop_prob:.2f}", (right_x+25, base_y+43), 1.0, font_type='normal')

    def _draw_bev_maps_section(self, db: np.ndarray, data: Dict, info_x: int, start_y: int) -> int:
        """يرسم قسم خرائط BEV (الحالية والمستقبلية)."""
        x = info_x + self.config.panel_margin
        panel_w = (self.config.width - info_x) - 2 * self.config.panel_margin

        main_bev_h = 380; future_bev_h = 180

        # الخريطة الرئيسية
        bev_t0 = cv2.resize(data.get('map_t0', np.zeros((1,1,3))), (panel_w, main_bev_h))
        self._draw_text(bev_t0, "BEV t+0.0s", (10, 30), 1.2, color=self.colors['text_header'])
        db[start_y:start_y+main_bev_h, x:x+panel_w] = bev_t0

        # الخرائط المستقبلية
        future_y = start_y + main_bev_h + 10
        future_bev_w = panel_w // 2

        bev_t1 = cv2.resize(data.get('map_t1', np.zeros((1,1,3))), (future_bev_w, future_bev_h))
        self._draw_text(bev_t1, "t+1.0s", (10, 20), 0.7, font_type='normal')

        bev_t2 = cv2.resize(data.get('map_t2', np.zeros((1,1,3))), (future_bev_w, future_bev_h))
        self._draw_text(bev_t2, "t+2.0s", (10, 20), 0.7, font_type='normal')

        db[future_y:future_y+future_bev_h, x:x+future_bev_w] = bev_t1
        db[future_y:future_y+future_bev_h, x+future_bev_w:x+panel_w] = bev_t2

        separator_y = future_y + future_bev_h + self.config.section_spacing
        cv2.line(db, (info_x, separator_y), (self.config.width, separator_y), self.colors['separator'], 2)
        return separator_y

    def _draw_controls_section(self, db: np.ndarray, data: Dict, info_x: int, start_y: int):
        """يرسم قسم عناصر التحكم بالمركبة."""
        x = info_x + self.config.panel_margin
        self._draw_text(db, "VEHICLE CONTROL", (x, start_y+40), 1.2, color=self.colors['text_header'])

        self._draw_gauge_display(db, data, x, start_y + 80)
        self._draw_control_bars(db, data, x + 180, start_y + 80)

    def _draw_gauge_display(self, db: np.ndarray, data: Dict, x: int, y: int):
        """يرسم عداد السرعة."""
        radius = 65
        self._draw_speed_gauge(db, x+radius, y+radius, radius, data.get('speed',0), data.get('target_speed',0), 60.0)

    def _draw_control_bars(self, db: np.ndarray, data: Dict, x: int, y: int):
        """يرسم مؤشرات التوجيه، الوقود، والمكابح."""
        bar_w = self.config.width - x - self.config.panel_margin

        self._draw_text(db, f"Steer: {data.get('steer', 0.0):.2f}", (x, y), 0.8, font_type='normal')
        self._draw_steer_indicator(db, x, y+15, bar_w, 20, data.get('steer', 0.0))

        self._draw_text(db, f"Throttle: {data.get('throttle', 0.0):.2f}", (x, y+50), 0.8, font_type='normal')
        self._draw_bar(db, x, y+65, bar_w, 20, data.get('throttle', 0.0), 1.0, self.colors['throttle'])

        brake_on = data.get('brake', False)
        brake_color = self.colors['brake'] if brake_on else self.colors['text']
        self._draw_text(db, f"Brake: {'ON' if brake_on else 'OFF'}", (x, y+100), 0.8, brake_color, font_type='normal')
        self._draw_bar(db, x, y+115, bar_w, 20, float(brake_on), 1.0, self.colors['brake'])

    # --------------------------------------------------------------------------
    #                         الدوال الأساسية للرسم (Primitives)
    # --------------------------------------------------------------------------

    def _draw_text(self, img, text, pos, size, color=None, font_type='header', thickness=1):
        color = color if color is not None else self.colors['text']
        font = self.fonts[font_type]
        cv2.putText(img, text, (pos[0]+1, pos[1]+1), font, size, (0,0,0), thickness+1, cv2.LINE_AA)
        cv2.putText(img, text, pos, font, size, color, thickness, cv2.LINE_AA)

    def _draw_bar(self, img, x, y, w, h, val, max_val, color):
        val=np.clip(val,0,max_val); ratio=val/max_val
        cv2.rectangle(img,(x,y),(x+w,y+h),self.colors['steer_neutral'],-1)
        cv2.rectangle(img,(x,y),(x+int(w*ratio),y+h),color,-1)
        cv2.rectangle(img,(x,y),(x+w,y+h),self.colors['text'],1)

    def _draw_steer_indicator(self, img, x, y, w, h, val):
        center_x=x+w//2; val=np.clip(val,-1.0,1.0)
        cv2.rectangle(img,(x,y),(x+w,y+h),self.colors['steer_neutral'],-1)
        cv2.line(img,(center_x,y),(center_x,y+h),self.colors['text'],1)
        indicator_x=center_x+int(val*(w//2*0.95))
        cv2.line(img,(indicator_x,y),(indicator_x,y+h),self.colors['steer_bar'],6)

    def _draw_speed_gauge(self, img, cx, cy, r, spd, t_spd, m_spd):
        cv2.circle(img,(cx,cy),r,self.colors['gauge_bg'],-1); cv2.circle(img,(cx,cy),r,self.colors['text'],2)
        ratio=np.clip(spd/m_spd,0,1); angle=math.radians(135+ratio*270)
        ex=cx+int(r*0.85*math.cos(angle)); ey=cy+int(r*0.85*math.sin(angle))
        cv2.line(img,(cx,cy),(ex,ey),self.colors['gauge_needle'],3); cv2.circle(img,(cx,cy),5,self.colors['gauge_needle'],-1)
        (w,h),_=cv2.getTextSize(f"{spd:.1f}",self.fonts['header'],1.5,3)
        self._draw_text(img,f"{spd:.1f}",(cx-w//2,cy+10),1.5); self._draw_text(img,"km/h",(cx-25,cy+35),0.5, font_type='normal')
        self._draw_text(img,f"Target: {t_spd:.1f}",(cx-50,cy-r-10),0.7)

    def _create_error_display(self, error_msg: str):
        error_display = np.zeros((self.config.height, self.config.width, 3), dtype=np.uint8)
        error_display[:] = self.colors['panel_bg']
        self._draw_text(error_display, "SYSTEM ERROR", (self.config.width//2-200, self.config.height//2-50), 1.5, self.colors['error'])
        self._draw_text(error_display, error_msg, (50, self.config.height//2+20), 0.8, self.colors['steer_bar'], font_type='normal')
        return error_display

    def _log_error(self, error_msg: str):
        self._error_log.append(f"{time.strftime('%Y-%m-%d %H:%M:%S')} - {error_msg}")
        if len(self._error_log) > 50: self._error_log.pop(0)