S1005403_RisCC/tools/test_prediction_performance.py

"""Test performance comparison: deepcopy vs PredictedTarget for prediction."""

import sys
import time
import copy
import math
from dataclasses import dataclass, field
from typing import List, Tuple


# Mock minimal Target class for testing
@dataclass
class Target:
    target_id: int
    active: bool = True
    traceable: bool = True
    restart: bool = False
    current_velocity_fps: float = field(default=0.0)
    current_vertical_velocity_fps: float = field(default=0.0)
    current_heading_deg: float = field(default=0.0)
    current_pitch_deg: float = field(default=0.0)
    current_range_nm: float = field(default=0.0)
    current_azimuth_deg: float = field(default=0.0)
    current_altitude_ft: float = field(default=0.0)
    _pos_x_ft: float = field(default=0.0)
    _pos_y_ft: float = field(default=0.0)
    _pos_z_ft: float = field(default=0.0)
    trajectory: List = field(default_factory=list)
    _path: List[Tuple] = field(default_factory=list)

    def update_state(self, dt: float):
        """Simple kinematic update."""
        heading_rad = math.radians(self.current_heading_deg)
        vx = self.current_velocity_fps * math.sin(heading_rad)
        vy = self.current_velocity_fps * math.cos(heading_rad)
        self._pos_x_ft += vx * dt
        self._pos_y_ft += vy * dt
        self._pos_z_ft += self.current_vertical_velocity_fps * dt


class PredictedTarget:
    """Lightweight wrapper for predicted target state."""

    __slots__ = (
        "target_id",
        "active",
        "traceable",
        "restart",
        "_pos_x_ft",
        "_pos_y_ft",
        "_pos_z_ft",
        "current_velocity_fps",
        "current_vertical_velocity_fps",
        "current_heading_deg",
        "current_pitch_deg",
        "current_range_nm",
        "current_azimuth_deg",
        "current_altitude_ft",
    )

    def __init__(self, target: Target, horizon_s: float):
        self.target_id = target.target_id
        self.active = target.active
        self.traceable = target.traceable
        self.restart = target.restart

        self.current_velocity_fps = target.current_velocity_fps
        self.current_vertical_velocity_fps = target.current_vertical_velocity_fps
        self.current_heading_deg = target.current_heading_deg
        self.current_pitch_deg = target.current_pitch_deg

        heading_rad = math.radians(target.current_heading_deg)
        vx = target.current_velocity_fps * math.sin(heading_rad)
        vy = target.current_velocity_fps * math.cos(heading_rad)
        vz = target.current_vertical_velocity_fps

        self._pos_x_ft = target._pos_x_ft + vx * horizon_s
        self._pos_y_ft = target._pos_y_ft + vy * horizon_s
        self._pos_z_ft = target._pos_z_ft + vz * horizon_s

        dist_2d = math.sqrt(self._pos_x_ft**2 + self._pos_y_ft**2)
        self.current_range_nm = dist_2d / 6076.12
        self.current_azimuth_deg = (
            math.degrees(math.atan2(self._pos_x_ft, self._pos_y_ft)) % 360
        )
        self.current_altitude_ft = self._pos_z_ft


def benchmark_deepcopy(targets: List[Target], horizon_s: float, iterations: int):
    """OLD approach: deepcopy + update_state."""
    start = time.perf_counter()

    for _ in range(iterations):
        predicted = []
        for target in targets:
            pred = copy.deepcopy(target)
            pred.update_state(horizon_s)
            predicted.append(pred)

    elapsed = time.perf_counter() - start
    return elapsed


def benchmark_lightweight(targets: List[Target], horizon_s: float, iterations: int):
    """NEW approach: PredictedTarget lightweight wrapper."""
    start = time.perf_counter()

    for _ in range(iterations):
        predicted = [PredictedTarget(t, horizon_s) for t in targets]

    elapsed = time.perf_counter() - start
    return elapsed


def main():
    print("=" * 70)
    print("Prediction Performance Comparison: deepcopy vs PredictedTarget")
    print("=" * 70)

    # Create test targets
    num_targets = 32
    targets = []
    for i in range(num_targets):
        t = Target(
            target_id=i,
            current_velocity_fps=300.0,
            current_heading_deg=45.0,
            current_vertical_velocity_fps=10.0,
            _pos_x_ft=10000.0 + i * 1000,
            _pos_y_ft=20000.0 + i * 500,
            _pos_z_ft=5000.0 + i * 100,
        )
        # Add some complex data to make deepcopy slower
        t.trajectory = [f"waypoint_{j}" for j in range(10)]
        t._path = [
            (i, j, k, l)
            for i, j, k, l in zip(range(100), range(100), range(100), range(100))
        ]
        targets.append(t)

    horizon_s = 0.2  # 200ms prediction horizon
    iterations = 1000  # Simulate 1000 prediction cycles

    print(f"\nTest configuration:")
    print(f"  Targets: {num_targets}")
    print(f"  Prediction horizon: {horizon_s}s")
    print(f"  Iterations: {iterations}")
    print(f"  Total predictions: {num_targets * iterations}")

    # Warm-up
    benchmark_deepcopy(targets[:2], horizon_s, 10)
    benchmark_lightweight(targets[:2], horizon_s, 10)

    # Benchmark OLD approach
    print(f"\n{'OLD (deepcopy + update_state)':<40}", end="")
    old_time = benchmark_deepcopy(targets, horizon_s, iterations)
    print(f"{old_time*1000:>8.2f} ms")

    # Benchmark NEW approach
    print(f"{'NEW (PredictedTarget lightweight)':<40}", end="")
    new_time = benchmark_lightweight(targets, horizon_s, iterations)
    print(f"{new_time*1000:>8.2f} ms")

    # Results
    speedup = old_time / new_time
    reduction_pct = ((old_time - new_time) / old_time) * 100

    print(f"\n{'='*70}")
    print(f"Speedup: {speedup:.1f}x faster")
    print(f"Time reduction: {reduction_pct:.1f}%")
    print(f"Time saved per cycle: {(old_time - new_time) / iterations * 1000:.3f} ms")
    print(f"\nAt 20Hz simulation rate:")
    print(f"  OLD overhead: {old_time / iterations * 1000:.2f} ms/frame")
    print(f"  NEW overhead: {new_time / iterations * 1000:.2f} ms/frame")
    print(
        f"  Saved per second: {(old_time - new_time) / iterations * 20 * 1000:.2f} ms"
    )
    print(f"{'='*70}")


if __name__ == "__main__":
    main()