# target_simulator/gui/payload_router.py """ Payload router for buffering SFP payloads for the GUI. """ import threading import statistics import collections import datetime import os import logging import math import json import ctypes import time from queue import Queue, Empty, Full from typing import Dict, Optional, Any, List, Callable, Tuple from target_simulator.core.sfp_structures import ( SFPHeader, SfpRisStatusPayload, DataTag, SfpRisSyncPayload, ) from target_simulator.analysis.simulation_state_hub import SimulationStateHub from target_simulator.core.models import Target from target_simulator.utils.clock_synchronizer import ClockSynchronizer try: from pyproj import Geod _GEOD = Geod(ellps="WGS84") _HAS_PYPROJ = True except ImportError: _GEOD = None _HAS_PYPROJ = False logger = logging.getLogger(__name__) PayloadHandler = Callable[[bytearray], None] TargetListListener = Callable[[List[Target]], None] RawPacketListener = Callable[[bytes, tuple], None] PostProcessingListener = Callable[[bytes], None] M_TO_FT = 3.28084 PERFORMANCE_SAMPLES_BUFFER_SIZE = 10000 class DebugPayloadRouter: """ A unified router that handles payloads for the entire application. """ def __init__( self, simulation_hub: Optional[SimulationStateHub] = None, ): self.active_archive = None self.logger = logger self._log_prefix = "[DebugPayloadRouter]" self._lock = threading.Lock() self._raw_packet_listeners: List[RawPacketListener] = [] self._post_processing_listeners: List[PostProcessingListener] = [] self._latest_payloads: Dict[str, Any] = {} self._last_raw_packet: Optional[tuple] = None self._sfp_debug_history_size = 20 self._history = collections.deque(maxlen=self._sfp_debug_history_size) self._persist = False self._hub = simulation_hub self._last_ownship_update_time: Optional[float] = None self._ris_target_listeners: List[TargetListListener] = [] self._processing_queue: Queue = Queue(maxsize=100) self._sync_results_queue: Queue = Queue(maxsize=500) self._stop_worker = threading.Event() self._processing_thread = threading.Thread( target=self._processing_loop, name="PayloadProcessingThread", daemon=True ) self._processing_thread.start() from target_simulator.config import DEBUG_CONFIG self._profiling_enabled = DEBUG_CONFIG.get( "enable_performance_profiling", False ) self._perf_counters = { "packet_count": 0, "_total_packet_count": 0, "parse_time_total": 0.0, "hub_update_time_total": 0.0, "archive_time_total": 0.0, "listener_time_total": 0.0, "clock_sync_time_total": 0.0, "last_report_time": time.time(), "max_processing_time": 0.0, } self._perf_samples = ( collections.deque(maxlen=PERFORMANCE_SAMPLES_BUFFER_SIZE) if self._profiling_enabled else None ) project_root = os.path.abspath( os.path.join(os.path.dirname(__file__), "..", "..") ) self._persist_dir = os.path.join(project_root, "Temp") try: os.makedirs(self._persist_dir, exist_ok=True) except Exception: pass self._handlers: Dict[int, PayloadHandler] = { ord("M"): lambda p: self._update_last_payload("MFD", p), ord("S"): lambda p: self._update_last_payload("SAR", p), ord("B"): lambda p: self._update_last_payload("BIN", p), ord("J"): self._handle_json_payload, ord("R"): self._handle_ris_status, ord("r"): self._handle_ris_status, } self.logger.info( f"{self._log_prefix} Initialized (Hub: {self._hub is not None})." ) try: self._clock_sync = ClockSynchronizer() except Exception: self._clock_sync = None def add_raw_packet_listener(self, listener: RawPacketListener): with self._lock: if listener not in self._raw_packet_listeners: self._raw_packet_listeners.append(listener) self.logger.debug( f"Raw packet listener registered. Total: {len(self._raw_packet_listeners)}" ) def remove_raw_packet_listener(self, listener: RawPacketListener): with self._lock: try: self._raw_packet_listeners.remove(listener) self.logger.debug( f"Raw packet listener removed. Total: {len(self._raw_packet_listeners)}" ) except ValueError: pass def add_post_processing_listener(self, listener: PostProcessingListener): with self._lock: if listener not in self._post_processing_listeners: self._post_processing_listeners.append(listener) self.logger.debug( f"Post-processing listener registered. Total: {len(self._post_processing_listeners)}" ) def remove_post_processing_listener(self, listener: PostProcessingListener): with self._lock: try: self._post_processing_listeners.remove(listener) self.logger.debug( f"Post-processing listener removed. Total: {len(self._post_processing_listeners)}" ) except ValueError: pass def shutdown(self): self.logger.info("Shutting down payload router worker thread...") self._stop_worker.set() try: self._processing_queue.put_nowait(None) except Full: pass self._processing_thread.join(timeout=2.0) if self._processing_thread.is_alive(): self.logger.warning("Payload processing thread did not shut down cleanly.") def _processing_loop(self): self.logger.info("Payload processing worker thread started.") while not self._stop_worker.is_set(): try: item = self._processing_queue.get(timeout=1.0) if item is None: break payload, reception_timestamp, raw_packet = item self._dispatch_payload( bytearray(payload), reception_timestamp, raw_packet ) self._processing_queue.task_done() except Empty: continue except Exception: self.logger.exception("Unexpected error in payload processing loop.") self.logger.info("Payload processing worker thread stopped.") def _dispatch_payload( self, payload: bytearray, reception_timestamp: float, raw_bytes_for_profiling: bytes, ): try: if len(payload) < ctypes.sizeof(DataTag): return tag = DataTag.from_buffer_copy(payload) if tag.ID[0] == ord("S") and tag.ID[1] == ord("Y"): self._process_sync_reply_payload(payload, reception_timestamp) else: self._process_ris_status_payload(payload, reception_timestamp) except Exception: self.logger.exception("Error during payload dispatch.") finally: with self._lock: for listener in self._post_processing_listeners: try: listener(raw_bytes_for_profiling) except Exception as e: self.logger.error(f"Error in post-processing listener: {e}") def _process_sync_reply_payload( self, payload: bytearray, reception_timestamp: float ): try: sync_data = payload[ctypes.sizeof(DataTag) :] if len(sync_data) < 16: return sync_payload = SfpRisSyncPayload.from_buffer_copy(sync_data) result = { "cookie": sync_payload.cc_cookie, "server_timetag": sync_payload.ris_timetag, "reception_timestamp": reception_timestamp, "flags": sync_payload.flags, "tx_period_ms": sync_payload.tx_period_ms, } try: self._sync_results_queue.put_nowait(result) except Full: pass except Exception as e: self.logger.warning( f"Failed to parse SYNC reply payload: {e}", exc_info=True ) def _handle_ris_status(self, payload: bytearray): pass def _process_ris_status_payload( self, payload: bytearray, reception_timestamp: float ): t_start = time.perf_counter() if self._profiling_enabled else None if self._profiling_enabled: self._perf_counters["packet_count"] += 1 self._perf_counters["_total_packet_count"] += 1 parsed_payload = None t_parse_start = time.perf_counter() try: parsed_payload = SfpRisStatusPayload.from_buffer_copy(payload) except (ValueError, TypeError) as e: self.logger.error( f"Failed to parse SfpRisStatusPayload from buffer (len={len(payload)}). Error: {e}" ) return t_parse_end = time.perf_counter() if self._profiling_enabled: self._perf_counters["parse_time_total"] += t_parse_end - t_parse_start t_hub_start = time.perf_counter() if self._hub: try: sc = parsed_payload.scenario # Prendi la posizione di partenza della simulazione sim_origin = self._hub.get_simulation_origin() origin_lat = sim_origin.get("latitude") origin_lon = sim_origin.get("longitude") origin_pos_xy_ft = sim_origin.get("position_xy_ft", (0.0, 0.0)) # Posizione attuale dell'ownship inviata dal server current_lat = float(sc.latitude) current_lon = float(sc.longitude) new_pos_x_ft, new_pos_y_ft = origin_pos_xy_ft # Calcola la posizione cartesiana assoluta basata sulla Lat/Lon # solo se abbiamo un'origine valida. Altrimenti, la posizione rimane quella dell'origine. if origin_lat is not None and origin_lon is not None: delta_north_m = 0.0 delta_east_m = 0.0 if _HAS_PYPROJ and _GEOD: # Metodo accurato con pyproj fwd_az, back_az, dist = _GEOD.inv(origin_lon, origin_lat, current_lon, current_lat) delta_north_m = dist * math.cos(math.radians(fwd_az)) delta_east_m = dist * math.sin(math.radians(fwd_az)) else: # Metodo di approssimazione equirettangolare (fallback) R = 6378137.0 dlat = math.radians(current_lat - origin_lat) dlon = math.radians(current_lon - origin_lon) delta_north_m = dlat * R delta_east_m = dlon * R * math.cos(math.radians(origin_lat)) # Converti lo spostamento in metri in coordinate del nostro sistema (X=Nord, Y=Ovest) # e aggiungilo alla posizione cartesiana dell'origine. new_pos_x_ft = origin_pos_xy_ft[0] + (delta_north_m * M_TO_FT) new_pos_y_ft = origin_pos_xy_ft[1] - (delta_east_m * M_TO_FT) # Negativo perché Y è Ovest ownship_heading_deg = math.degrees(float(sc.true_heading)) % 360 ownship_state = { "timestamp": reception_timestamp, "position_xy_ft": (new_pos_x_ft, new_pos_y_ft), "altitude_ft": float(sc.baro_altitude) * M_TO_FT, "velocity_xy_fps": (float(sc.vx) * M_TO_FT, float(sc.vy) * M_TO_FT), "heading_deg": ownship_heading_deg, "latitude": current_lat, "longitude": current_lon, } self._hub.set_ownship_state(ownship_state) with self._lock: archive = self.active_archive if archive and hasattr(archive, "add_ownship_state"): archive.add_ownship_state(ownship_state) except Exception: self.logger.exception("Failed to update ownship state.") # ... (il resto del metodo è invariato) t_clock_start = time.perf_counter() if self._clock_sync is not None and parsed_payload is not None: try: server_timetag = int(parsed_payload.scenario.timetag) self._clock_sync.add_sample(server_timetag, reception_timestamp) est_gen = self._clock_sync.to_client_time(server_timetag) latency = reception_timestamp - est_gen if latency >= 0 and self.active_archive is not None: latency_ms = latency * 1000 with self._lock: archive = self.active_archive if archive and hasattr(archive, "add_latency_sample"): archive.add_latency_sample(reception_timestamp, latency_ms) except Exception: pass t_clock_end = time.perf_counter() if self._profiling_enabled: self._perf_counters["clock_sync_time_total"] += t_clock_end - t_clock_start real_targets, inactive_ids = self._parse_ris_payload_to_targets(payload) if self._hub: try: self._hub.add_real_packet(reception_timestamp) for tid in inactive_ids or []: self._hub.clear_real_target_data(tid) for target in real_targets: state_tuple = ( getattr(target, "_pos_x_ft", 0.0), getattr(target, "_pos_y_ft", 0.0), getattr(target, "_pos_z_ft", 0.0), ) self._hub.add_real_state( target.target_id, reception_timestamp, state_tuple ) self._hub.set_real_heading( target.target_id, target.current_heading_deg, raw_value=getattr(target, "_raw_heading", None), ) except Exception: self.logger.exception("Failed to process RIS targets for Hub.") t_hub_end = time.perf_counter() if self._profiling_enabled: self._perf_counters["hub_update_time_total"] += t_hub_end - t_hub_start t_archive_start = time.perf_counter() with self._lock: archive = self.active_archive if archive: for target in real_targets: state_tuple = ( getattr(target, "_pos_x_ft", 0.0), getattr(target, "_pos_y_ft", 0.0), getattr(target, "_pos_z_ft", 0.0), ) archive.add_real_state( target.target_id, reception_timestamp, state_tuple ) t_archive_end = time.perf_counter() if self._profiling_enabled: self._perf_counters["archive_time_total"] += t_archive_end - t_archive_start t_listener_start = time.perf_counter() with self._lock: for listener in self._ris_target_listeners: try: listener(real_targets) except Exception: self.logger.exception(f"Error in RIS target listener: {listener}") t_listener_end = time.perf_counter() if self._profiling_enabled: self._perf_counters["listener_time_total"] += ( t_listener_end - t_listener_start ) if ( self._profiling_enabled and t_start is not None and self.active_archive is not None ): total_processing_time = time.perf_counter() - t_start self._perf_counters["max_processing_time"] = max( self._perf_counters["max_processing_time"], total_processing_time ) if ( total_processing_time > 0.010 or self._perf_counters["_total_packet_count"] % 100 == 0 ): if self._perf_samples is not None: sample = { "timestamp": reception_timestamp, "total_ms": round(total_processing_time * 1000, 3), "parse_ms": round((t_parse_end - t_parse_start) * 1000, 3), "hub_ms": round((t_hub_end - t_hub_start) * 1000, 3), "archive_ms": round( (t_archive_end - t_archive_start) * 1000, 3 ), "listener_ms": round( (t_listener_end - t_listener_start) * 1000, 3 ), "clock_ms": round((t_clock_end - t_clock_start) * 1000, 3), } self._perf_samples.append(sample) current_time = time.time() if current_time - self._perf_counters["last_report_time"] >= 5.0: self._report_performance_stats() self._perf_counters["last_report_time"] = current_time self._update_debug_views(parsed_payload) def get_sync_result(self) -> Optional[Dict]: try: return self._sync_results_queue.get_nowait() except Empty: return None def set_archive(self, archive): with self._lock: self.active_archive = archive if archive is not None: if self._perf_samples is not None: self._perf_samples.clear() self._perf_counters.update( { "packet_count": 0, "_total_packet_count": 0, "parse_time_total": 0.0, "hub_update_time_total": 0.0, "archive_time_total": 0.0, "listener_time_total": 0.0, "clock_sync_time_total": 0.0, "max_processing_time": 0.0, "last_report_time": time.time(), } ) def add_ris_target_listener(self, listener: TargetListListener): with self._lock: if listener not in self._ris_target_listeners: self._ris_target_listeners.append(listener) def remove_ris_target_listener(self, listener: TargetListListener): with self._lock: try: self._ris_target_listeners.remove(listener) except ValueError: pass def get_handlers(self) -> Dict[int, PayloadHandler]: return self._handlers def _update_last_payload(self, flow_id: str, payload: Any): with self._lock: self._latest_payloads[flow_id] = payload def _parse_ris_payload_to_targets( self, payload: bytearray ) -> Tuple[List[Target], List[int]]: targets: List[Target] = [] inactive_ids: List[int] = [] try: parsed_payload = SfpRisStatusPayload.from_buffer_copy(payload) for i, ris_target in enumerate(parsed_payload.tgt.tgt): if ris_target.flags != 0: target = Target( target_id=i, trajectory=[], active=True, traceable=True ) # Server convention: x=North (+up), y=West (+left) # Internal convention: x=North, y=West (+left) # Conventions are aligned. Direct mapping is correct. pos_x_ft = float(ris_target.x) * M_TO_FT pos_y_ft = float(ris_target.y) * M_TO_FT pos_z_ft = float(ris_target.z) * M_TO_FT setattr(target, "_pos_x_ft", pos_x_ft) setattr(target, "_pos_y_ft", pos_y_ft) setattr(target, "_pos_z_ft", pos_z_ft) target._update_current_polar_coords() try: raw_h = float(ris_target.heading) target.current_heading_deg = ( math.degrees(raw_h) if abs(raw_h) <= 7.0 else raw_h ) % 360 setattr(target, "_raw_heading", raw_h) except (ValueError, TypeError): target.current_heading_deg = 0.0 targets.append(target) else: inactive_ids.append(int(i)) except Exception: self.logger.exception("Failed to parse RIS payload into Target objects.") return targets, inactive_ids # ... (il resto del file rimane invariato) def _update_debug_views(self, parsed_payload: SfpRisStatusPayload): try: sc = parsed_payload.scenario lines = ["RIS Status Payload:\n", "Scenario:"] text_out = "\n".join(lines) self._update_last_payload( "RIS_STATUS_TEXT", bytearray(text_out.encode("utf-8")) ) def _convert_ctypes(value): if hasattr(value, "_length_"): return list(value) if isinstance(value, ctypes._SimpleCData): return value.value return value scenario_dict = { f[0]: _convert_ctypes(getattr(sc, f[0])) for f in sc._fields_ } targets_list = [ {f[0]: _convert_ctypes(getattr(t, f[0])) for f in t._fields_} for t in parsed_payload.tgt.tgt ] struct = {"scenario": scenario_dict, "targets": targets_list} json_bytes = bytearray(json.dumps(struct, indent=2).encode("utf-8")) self._update_last_payload("RIS_STATUS_JSON", json_bytes) if self._hub: sweep_rad = scenario_dict.get("ant_nav_az") if sweep_rad is not None: try: sweep_deg = math.degrees(float(sweep_rad)) self._hub.set_antenna_azimuth( sweep_deg, timestamp=time.monotonic() ) except Exception: pass except Exception: self.logger.exception("Failed to generate text/JSON for RIS debug view.") def _handle_json_payload(self, payload: bytearray): try: self._update_last_payload("JSON", payload) if not self._hub: return try: obj = json.loads(payload.decode("utf-8")) except Exception: return def _find_val(keys, dct): if not isinstance(dct, dict): return None for key in keys: if key in dct: return dct[key] sc = dct.get("scenario", {}) if isinstance(sc, dict): for key in keys: if key in sc: return sc[key] return None sweep_val = _find_val(["ant_nav_az", "antenna_azimuth"], obj) if sweep_val is not None: try: s_rad = float(sweep_val) if abs(s_rad) > 7.0: s_rad = math.radians(s_rad) sweep_deg = math.degrees(s_rad) self._hub.set_antenna_azimuth(sweep_deg, timestamp=time.monotonic()) except Exception as e: self.logger.debug(f"Error processing antenna azimuth: {e}") except Exception: self.logger.exception("Error handling JSON payload") def get_and_clear_latest_payloads(self) -> Dict[str, Any]: with self._lock: new_payloads, self._latest_payloads = self._latest_payloads, {} return new_payloads def update_raw_packet(self, raw_bytes: bytes, addr: tuple): with self._lock: for listener in self._raw_packet_listeners: try: listener(raw_bytes, addr) except Exception as e: self.logger.error(f"Error in raw packet listener: {e}") header_size = SFPHeader.size() if len(raw_bytes) > header_size: try: flow = raw_bytes[6] if flow in (ord("R"), ord("r")): reception_timestamp = time.monotonic() payload = raw_bytes[header_size:] self._processing_queue.put_nowait( (payload, reception_timestamp, raw_bytes) ) except Full: self.logger.error( "Payload processing queue is full! A RIS packet was dropped." ) except Exception as e: self.logger.error(f"Error queuing RIS packet in update_raw_packet: {e}") with self._lock: self._last_raw_packet = (raw_bytes, addr) entry = {"ts": datetime.datetime.utcnow(), "addr": addr, "raw": raw_bytes} try: hdr = SFPHeader.from_buffer_copy(raw_bytes) entry.update(flow=int(hdr.SFP_FLOW), tid=int(hdr.SFP_TID)) flow_map = { ord(c): n for c, n in [ ("M", "MFD"), ("S", "SAR"), ("B", "BIN"), ("J", "JSON"), ("R", "RIS"), ] } entry["flow_name"] = flow_map.get(entry["flow"], str(entry["flow"])) except Exception: pass self._history.append(entry) if self._persist: try: ts = entry["ts"].strftime("%Y%m%dT%H%M%S.%f") fname = f"sfp_raw_{ts}_{addr[0].replace(':','_')}_{addr[1]}.bin" with open(os.path.join(self._persist_dir, fname), "wb") as f: f.write(raw_bytes) except Exception: pass def get_and_clear_raw_packet(self) -> Optional[tuple]: with self._lock: pkt, self._last_raw_packet = self._last_raw_packet, None return pkt def get_estimated_latency_s(self) -> float: try: if self._clock_sync: return self._clock_sync.get_average_latency_s() except Exception: return 0.0 return 0.0 def get_latency_samples(self, limit: Optional[int] = None) -> List[tuple]: try: if self._clock_sync: samples = self._clock_sync.get_latency_history() return samples[-limit:] if limit else samples except Exception: return [] return [] def get_latency_stats(self, sample_limit: int = 200) -> Dict[str, Any]: try: samples = self.get_latency_samples(limit=sample_limit) if not samples: return {"count": 0} ms = [s[1] * 1000.0 for s in samples] return { "mean_ms": round(statistics.mean(ms), 3), "std_ms": round(statistics.stdev(ms) if len(ms) > 1 else 0.0, 3), "min_ms": round(min(ms), 3), "max_ms": round(max(ms), 3), "count": len(ms), } except Exception: return {"count": 0} def get_history(self): with self._lock: return list(self._history) def get_performance_samples(self): result = ( list(self._perf_samples) if self._profiling_enabled and self._perf_samples else [] ) return result def _report_performance_stats(self): try: count = self._perf_counters["packet_count"] if count == 0: return avg_parse = (self._perf_counters["parse_time_total"] / count) * 1000 avg_hub = (self._perf_counters["hub_update_time_total"] / count) * 1000 avg_archive = (self._perf_counters["archive_time_total"] / count) * 1000 avg_listener = (self._perf_counters["listener_time_total"] / count) * 1000 avg_clock = (self._perf_counters["clock_sync_time_total"] / count) * 1000 max_proc = self._perf_counters["max_processing_time"] * 1000 total_avg = avg_parse + avg_hub + avg_archive + avg_listener + avg_clock self.logger.info( f"[PERF] Pkts: {count} | Avg: {total_avg:.2f}ms (parse:{avg_parse:.2f} hub:{avg_hub:.2f} arch:{avg_archive:.2f} listen:{avg_listener:.2f} sync:{avg_clock:.2f}) | Max: {max_proc:.2f}ms" ) self._perf_counters["packet_count"] = 0 self._perf_counters["parse_time_total"] = 0.0 self._perf_counters["hub_update_time_total"] = 0.0 self._perf_counters["archive_time_total"] = 0.0 self._perf_counters["listener_time_total"] = 0.0 self._perf_counters["clock_sync_time_total"] = 0.0 self._perf_counters["max_processing_time"] = 0.0 except Exception as e: self.logger.debug(f"Error reporting performance stats: {e}") def clear_history(self): with self._lock: self._history.clear() def set_history_size(self, n: int): with self._lock: self._sfp_debug_history_size = max(1, int(n)) self._history = collections.deque( self._history, maxlen=self._sfp_debug_history_size ) def set_persist(self, enabled: bool): with self._lock: self._persist = bool(enabled)