"""MAVLink telemetry emulator for Synapse compatibility."""

from __future__ import annotations

import logging
import math
import threading
import time
from dataclasses import dataclass

from pymavlink import mavutil

logger = logging.getLogger(__name__)


@dataclass(slots=True)
class MavlinkTelemetryConfig:
    host: str = "127.0.0.1"
    port: int = 14550
    stream_hz: float = 10.0
    heartbeat_hz: float = 1.0
    source_system: int = 1
    source_component: int = 1


class MavlinkEmulator:
    """Sends the message set required by Synapse MavlinkController."""

    def __init__(self, config: MavlinkTelemetryConfig):
        self.config = config
        self._stop_event = threading.Event()
        self._thread: threading.Thread | None = None
        self._master = None

    def start(self) -> None:
        if self._thread and self._thread.is_alive():
            return

        endpoint = f"udpout:{self.config.host}:{self.config.port}"
        self._master = mavutil.mavlink_connection(
            endpoint,
            source_system=self.config.source_system,
            source_component=self.config.source_component,
        )

        self._thread = threading.Thread(target=self._run_loop, name="mavlink-emulator", daemon=True)
        self._thread.start()
        logger.info("MAVLink emulator started on %s", endpoint)

    def stop(self, join_timeout_s: float = 1.0) -> None:
        self._stop_event.set()
        if self._thread:
            self._thread.join(timeout=join_timeout_s)
        if self._master is not None:
            try:
                self._master.close()
            except Exception:
                pass

    def serve_forever(self) -> None:
        self.start()
        try:
            while not self._stop_event.is_set():
                time.sleep(0.25)
        finally:
            self.stop()

    def __enter__(self) -> "MavlinkEmulator":
        self.start()
        return self

    def __exit__(self, exc_type, exc, tb) -> None:  # type: ignore[no-untyped-def]
        self.stop()

    def _run_loop(self) -> None:
        assert self._master is not None
        stream_period = 1.0 / max(self.config.stream_hz, 0.1)
        heartbeat_period = 1.0 / max(self.config.heartbeat_hz, 0.1)

        next_stream = time.monotonic()
        next_heartbeat = time.monotonic()
        start = time.monotonic()

        while not self._stop_event.is_set():
            now = time.monotonic()

            if now >= next_heartbeat:
                self._send_heartbeat()
                next_heartbeat = now + heartbeat_period

            if now >= next_stream:
                elapsed = now - start
                self._send_telemetry(elapsed)
                next_stream = now + stream_period

            time.sleep(0.002)

    def _send_heartbeat(self) -> None:
        assert self._master is not None
        self._master.mav.heartbeat_send(
            mavutil.mavlink.MAV_TYPE_QUADROTOR,
            mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA,
            0,
            0,
            mavutil.mavlink.MAV_STATE_ACTIVE,
        )

    def _send_telemetry(self, elapsed_s: float) -> None:
        assert self._master is not None

        time_boot_ms = int(elapsed_s * 1000)
        time_usec = int(time.time() * 1_000_000)

        lat_deg = 46.800000 + 0.0001 * math.sin(elapsed_s / 8.0)
        lon_deg = 9.830000 + 0.0001 * math.cos(elapsed_s / 8.0)
        alt_m = 1520.0 + 0.5 * math.sin(elapsed_s / 5.0)

        x = 5.0 * math.sin(elapsed_s / 3.0)
        y = 5.0 * math.cos(elapsed_s / 3.0)
        z = -1.2
        vx = (5.0 / 3.0) * math.cos(elapsed_s / 3.0)
        vy = -(5.0 / 3.0) * math.sin(elapsed_s / 3.0)
        vz = 0.0

        q1 = 1.0
        q2 = 0.0
        q3 = 0.0
        q4 = 0.0

        self._master.mav.global_position_int_send(
            time_boot_ms,
            int(lat_deg * 1e7),
            int(lon_deg * 1e7),
            int(alt_m * 1000),
            int((alt_m - 10.0) * 1000),
            int(vx * 100),
            int(vy * 100),
            int(vz * 100),
            0,
        )

        self._master.mav.local_position_ned_send(
            time_boot_ms,
            float(x),
            float(y),
            float(z),
            float(vx),
            float(vy),
            float(vz),
        )

        self._master.mav.attitude_quaternion_send(
            time_boot_ms,
            q1,
            q2,
            q3,
            q4,
            0.0,
            0.0,
            0.02,
        )

        self._master.mav.gps_raw_int_send(
            time_usec,
            3,
            int(lat_deg * 1e7),
            int(lon_deg * 1e7),
            int(alt_m * 1000),
            100,
            120,
            int(math.hypot(vx, vy) * 100),
            0,
            14,
        )
