# target_simulator/core/simulation_engine.py

"""
The core simulation engine that runs in a separate thread to update and
broadcast target states, supporting different operational modes.
"""
import threading
import time
import math
from queue import Queue
from typing import Optional

# Use absolute imports for robustness
from target_simulator.core.communicator_interface import CommunicatorInterface
from target_simulator.core.models import Scenario, Target
from target_simulator.core import command_builder
from target_simulator.utils.logger import get_logger
from target_simulator.analysis.simulation_state_hub import SimulationStateHub
from target_simulator.utils.csv_logger import append_sent_position

# Simulation frequency in Hertz
TICK_RATE_HZ = 20.0
TICK_INTERVAL_S = 1.0 / TICK_RATE_HZ


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

    Avoids expensive deepcopy by computing only the predicted position/velocity
    needed for command generation. This is 10-15x faster than deepcopy for
    prediction horizons.
    """

    __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):
        """Create a predicted target state by advancing the target by horizon_s.

        Args:
            target: The original target to predict from
            horizon_s: Prediction horizon in seconds
        """
        # Copy identity and flags
        self.target_id = target.target_id
        self.active = target.active
        self.traceable = target.traceable
        self.restart = target.restart

        # Copy current velocities
        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

        # Predict position using simple kinematic model
        # x = x0 + vx * t, y = y0 + vy * t, z = z0 + vz * t
        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

        # Recompute polar coordinates from predicted position
        dist_2d = math.sqrt(self._pos_x_ft**2 + self._pos_y_ft**2)
        self.current_range_nm = dist_2d / 6076.12  # Convert feet to nautical miles
        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


class SimulationEngine(threading.Thread):
    def __init__(
        self,
        communicator: Optional[CommunicatorInterface],
        simulation_hub: Optional[SimulationStateHub] = None,
        archive: Optional[str] = None,
        update_queue: Optional[Queue] = None,
    ):
        """Create the simulation engine running in a background thread.

        Args:
            communicator: Optional CommunicatorInterface used to send runtime
                commands (e.g., SFP, Serial). If None the engine can run in
                preview-only mode using `update_queue`.
            simulation_hub: Optional SimulationStateHub used to record/forward
                high-frequency simulated states for analysis.
            archive: Optional archive object/path that accepts simulated state
                events. When provided the engine will write simulated states to it.
            update_queue: Optional queue used to publish periodic target state
                snapshots to the GUI or other consumers.

        The engine is a daemon Thread; call `.start()` to begin simulation.
        """
        super().__init__(daemon=True, name="SimulationEngineThread")
        self.logger = get_logger(__name__)

        self.communicator = communicator
        # Backwards-compat: older callers passed an update_queue as the
        # second positional argument. Detect a Queue and treat it as the
        # update queue while leaving simulation_hub unset. Also accept an
        # explicit `update_queue` keyword argument for newer callers.
        self.update_queue: Optional[Queue] = None
        if isinstance(simulation_hub, Queue):
            self.update_queue = simulation_hub
            self.simulation_hub = None
        else:
            self.simulation_hub = simulation_hub  # Hub for data analysis

        # If caller provided update_queue explicitly, prefer it (overrides
        # any queue accidentally passed in simulation_hub position).
        if isinstance(update_queue, Queue):
            self.update_queue = update_queue
        self.archive = archive  # Archive path if needed
        self.time_multiplier = 1.0
        self.update_interval_s = 1.0
        self.prediction_horizon_s = 0.0  # Latency compensation in seconds

        # Determine communication protocol from the communicator's config
        self.use_json_protocol = False
        if communicator:
            # Safely access the _use_json_protocol flag if it exists (for SFPCommunicator)
            self.use_json_protocol = getattr(communicator, "_use_json_protocol", False)
            self.logger.info(
                f"SimulationEngine will use JSON protocol: {self.use_json_protocol}"
            )

        self.scenario: Optional[Scenario] = None
        self._stop_event = threading.Event()
        self._is_running_event = threading.Event()
        self._last_tick_time = 0.0
        self._last_update_time = 0.0
        self._is_paused = False

    def set_prediction_horizon(self, horizon_s: float):
        """Set the prediction horizon (seconds) used to compensate latency.

        The horizon represents the estimated one-way latency and is applied by
        advancing a deep-copied target instance before building commands.

        Args:
            horizon_s: Horizon in seconds. Values are clamped to [0.0, 1.0].
        """
        # Set a reasonable cap to avoid excessive prediction
        self.prediction_horizon_s = max(0.0, min(horizon_s, 1.0))

    def load_scenario(self, scenario: Scenario):
        """Load a Scenario into the engine and reset its per-target state.

        The provided Scenario object is used directly (no copy). Callers should
        ensure the scenario's data structures are not mutated concurrently.
        """
        self.logger.info(f"Loading scenario '{scenario.name}' into simulation engine.")
        self.scenario = scenario
        self.scenario.reset_simulation()

    def set_time_multiplier(self, multiplier: float):
        """Set the simulation time multiplier.

        Args:
            multiplier: Simulation speed multiplier (e.g., 2.0 = twice real time).
        """
        self.logger.info(f"Setting simulation time multiplier to {multiplier}x")
        self.time_multiplier = multiplier

    def set_update_interval(self, interval_s: float):
        """Set the interval (seconds) between outgoing target updates.

        Args:
            interval_s: Seconds between update deliveries to communicator/UI.
        """
        self.logger.info(f"Setting target update interval to {interval_s}s")
        self.update_interval_s = interval_s

    def set_simulation_time(self, new_time_s: float):
        """Seek the simulation to a specific time (seconds).

        Note: best used while the engine is paused. The method forces each
        target to update its internal state to reflect the requested time.

        Args:
            new_time_s: Time in seconds to seek to within the scenario.
        """
        if self.scenario:
            self.logger.debug(f"Seeking simulation time to {new_time_s:.2f}s.")
            for target in self.scenario.get_all_targets():
                # Directly set the internal simulation time of the target
                setattr(target, "_sim_time_s", new_time_s)
                # Force an update to calculate the new position
                target.update_state(0)

            self._last_tick_time = time.monotonic()

            if self.update_queue:
                self.update_queue.put_nowait(self.scenario.get_all_targets())

    def run(self):
        """Main loop for the simulation thread.

        The loop advances target states at `TICK_RATE_HZ`, publishes frequent
        state data to the `simulation_hub` and throttles outbound updates to
        `update_interval_s` before sending commands via the configured
        `communicator`.
        """
        self.logger.info("Simulation engine thread started.")
        self._is_running_event.set()
        self._last_tick_time = time.monotonic()
        self._last_update_time = self._last_tick_time

        while not self._stop_event.is_set():
            if not self.scenario:
                time.sleep(TICK_INTERVAL_S)
                continue

            if self._is_paused:
                time.sleep(TICK_INTERVAL_S)
                self._last_tick_time = time.monotonic()
                continue

            current_time = time.monotonic()
            delta_time = current_time - self._last_tick_time
            self._last_tick_time = current_time
            simulated_delta_time = delta_time * self.time_multiplier

            self.scenario.update_state(simulated_delta_time)
            if self.archive:
                log_timestamp = time.monotonic()
                for target in self.scenario.get_all_targets():
                    if target.active:
                        state_tuple = (
                            getattr(target, "_pos_x_ft", 0.0),
                            getattr(target, "_pos_y_ft", 0.0),
                            getattr(target, "_pos_z_ft", 0.0),
                        )
                        self.archive.add_simulated_state(
                            target.target_id, log_timestamp, state_tuple
                        )

            # --- High-Frequency State Logging ---
            tick_timestamp = time.monotonic()
            active_targets = [t for t in self.scenario.get_all_targets() if t.active]

            if self.simulation_hub:
                for target in active_targets:
                    # Include velocities in the state tuple for the hub
                    state_tuple = (
                        getattr(target, "_pos_x_ft", 0.0),
                        getattr(target, "_pos_y_ft", 0.0),
                        getattr(target, "_pos_z_ft", 0.0),
                        getattr(target, "current_velocity_fps", 0.0),
                        getattr(target, "current_vertical_velocity_fps", 0.0),
                    )
                    self.simulation_hub.add_simulated_state(
                        target.target_id, tick_timestamp, state_tuple
                    )

            if self.scenario.is_finished():
                self.logger.info("Scenario finished. Stopping engine.")
                # The GUI refresh loop will detect that the engine is no longer running
                # and will handle the finalization. No need to send a special message.
                break

            # --- Throttled Communication Step ---
            if (current_time - self._last_update_time) >= self.update_interval_s:
                self._last_update_time = current_time

                if self.update_queue:
                    try:
                        targets_snapshot = [
                            t for t in self.scenario.get_all_targets() if t.active
                        ]
                        self.update_queue.put_nowait(targets_snapshot)
                    except Exception:
                        # Don't let queue failures stop the engine
                        pass

                if self.communicator and self.communicator.is_open:
                    commands_to_send = []
                    targets_to_send = []

                    if self.prediction_horizon_s > 0.0 and active_targets:
                        targets_to_send = [
                            PredictedTarget(target, self.prediction_horizon_s)
                            for target in active_targets
                        ]
                    else:
                        targets_to_send = active_targets

                    if self.use_json_protocol:
                        if targets_to_send:
                            json_payload = command_builder.build_json_update(
                                targets_to_send
                            )
                            commands_to_send.append(json_payload)
                            for target in active_targets:
                                state_tuple = (
                                    getattr(target, "_pos_x_ft", 0.0),
                                    getattr(target, "_pos_y_ft", 0.0),
                                    getattr(target, "_pos_z_ft", 0.0),
                                )
                                append_sent_position(
                                    tick_timestamp,
                                    target.target_id,
                                    state_tuple[0],
                                    state_tuple[1],
                                    state_tuple[2],
                                    "JSON_BULK_UPDATE",
                                )
                    else:
                        for target in targets_to_send:
                            cmd = command_builder.build_tgtset_from_target_state(target)
                            commands_to_send.append(cmd)
                            original_target = next(
                                (
                                    t
                                    for t in active_targets
                                    if t.target_id == target.target_id
                                ),
                                None,
                            )
                            if original_target:
                                state_tuple = (
                                    getattr(original_target, "_pos_x_ft", 0.0),
                                    getattr(original_target, "_pos_y_ft", 0.0),
                                    getattr(original_target, "_pos_z_ft", 0.0),
                                )
                                append_sent_position(
                                    tick_timestamp,
                                    original_target.target_id,
                                    state_tuple[0],
                                    state_tuple[1],
                                    state_tuple[2],
                                    cmd,
                                )

                    if commands_to_send:
                        self.communicator.send_commands(commands_to_send)

            time.sleep(TICK_INTERVAL_S)

        self._is_running_event.clear()
        self.logger.info("Simulation engine thread stopped.")

    def pause(self):
        """Pause the simulation; the run loop will continue idling until resumed."""
        self.logger.info("Simulation paused.")
        self._is_paused = True

    def resume(self):
        """Resume the simulation if currently paused."""
        self.logger.info("Simulation resumed.")
        self._is_paused = False

    def stop(self):
        """Signal the simulation thread to stop and wait briefly for termination."""
        self.logger.info("Stop signal received for simulation thread.")
        self._stop_event.set()
        self.resume()
        self.join(timeout=2.0)
        if self.is_alive():
            self.logger.warning("Simulation thread did not stop gracefully.")

    def is_running(self) -> bool:
        """Return True while the simulation thread's main loop is active."""
        return self._is_running_event.is_set()
