"""
Output adapters for sending position data to various destinations.
"""

import csv
import json
import logging
import socket
import time
from abc import ABC, abstractmethod
from typing import Any, Optional, Tuple

from multilat_solver.common_types import MessageType

logger = logging.getLogger(__name__)


class OutputAdapter(ABC):
    """Base class for output adapters."""

    def __init__(self, message_type: MessageType, frequency: int):
        """Initialize output adapter."""
        self._message_type = message_type
        self._frequency = frequency
        self._sleep_time = 1.0 / frequency if frequency > 0 else 0.1
        self._last_send_position_time = 0.0
        self._last_send_gps_time = 0.0

    def send_position(self, position: Tuple[float, float, float], timestamp: Optional[float] = None):
        """Check if position should be sent based on message type and frequency throttling."""
        if self._message_type not in [MessageType.POSITION, MessageType.BOTH]:
            return False
        if time.time() - self._last_send_position_time < self._sleep_time:
            return False
        self._last_send_position_time = time.time()
        return True

    def send_gps(
        self,
        coordinates: Tuple[float, float, float],
        timestamp: Optional[float] = None,
        n_satellites: int = 10,
    ):
        """Check if GPS should be sent based on message type and frequency throttling.

        :param coordinates: Tuple[float, float, float] - latitude, longitude, altitude in degrees and meters
        :param timestamp: Optional[float] - timestamp in seconds
        :param n_satellites: Number of satellites
        """
        if self._message_type not in [MessageType.GPS, MessageType.BOTH]:
            return False
        if time.time() - self._last_send_gps_time < self._sleep_time:
            return False
        self._last_send_gps_time = time.time()
        return True

    @abstractmethod
    def close(self):
        """Close the adapter."""
        pass


class MavlinkOutputAdapter(OutputAdapter):
    """MAVLink output adapter using pymavlink."""

    def __init__(
        self,
        connection_string: str = "udpout:127.0.0.1:14550",
        system_id: int = 1,
        component_id: int = 1,
        message_type: MessageType = MessageType.BOTH,
        frequency: int = 10,
        reconnect_attempts: int = 3,
        heartbeat_wait_s: float = 10.0,
    ):
        """
        Initialize MAVLink adapter.

        :param connection_string: MAVLink connection string
                (e.g., "udpout:127.0.0.1:14550", "tcp:127.0.0.1:5760")
        :param system_id: MAVLink system ID
        :param component_id: MAVLink component ID
        :param message_type: Message type
        :param frequency: Frequency in Hz
        """
        logger.info(
            "MavlinkOutputAdapter initialized: %s, %s, %s, %s",
            connection_string,
            system_id,
            component_id,
            frequency,
        )
        self.connection = None
        self.reconnect_attempts = reconnect_attempts
        self.heartbeat_wait_s = heartbeat_wait_s
        self.last_heartbeat_time = time.monotonic()
        self.component_id = component_id
        self.system_id = system_id
        self.connection_string = connection_string
        self.connected = False

        self._try_reconnect()
        super().__init__(message_type, frequency)

    def _connect(self):
        from pymavlink import mavutil

        return mavutil.mavlink_connection(
            self.connection_string, source_system=self.system_id, source_component=self.component_id
        )

    def _try_reconnect(self):
        attempts = self.reconnect_attempts
        while attempts > 0:
            if self.connection is not None:
                self.connection.close()
            self.connection = self._connect()
            res = self.connection.wait_heartbeat(timeout=self.heartbeat_wait_s)
            if res is not None:
                self.last_heartbeat_time = time.monotonic()
                self.connected = True
                return
            attempts -= 1
            print(f"MavlinkOutputAdapter heartbeat timeout. Attempts left: {attempts}")
        self.connected = False

    def wait_heartbeat(self):
        """Wait for heartbeat from PX4."""
        while not self.connected:
            time.sleep(0.2)
            self._try_reconnect()

    def send_position(self, position: Tuple[float, float, float], timestamp: Optional[float] = None):
        """Send position via MAVLink LOCAL_POSITION_NED message."""
        if not self.connected:
            time.sleep(0.2)
            self._try_reconnect()
            return
        if time.monotonic() - self.last_heartbeat_time > self.heartbeat_wait_s:
            self.connected = False
            self._try_reconnect()
            return

        if not super().send_position(position, timestamp):
            return

        x, y, z = position
        timestamp_ms = int(timestamp) if timestamp else int(time.time_ns() / 1000)
        # Send LOCAL_POSITION_NED message
        self.connection.mav.local_position_ned_send(
            timestamp_ms % 4294967295,
            x,  # x position (m)
            y,  # y position (m)
            z,  # z position (m)
            0,  # vx velocity (m/s)
            0,  # vy velocity (m/s)
            0,  # vz velocity (m/s),
        )
        logger.info(f"Mavlink Local position send: {(x, y, z)}")

    def send_gps(
        self,
        coordinates: Tuple[float, float, float],
        timestamp: Optional[float] = None,
        n_satellites: int = 10,
    ):
        """Send GPS coordinates.

        :param coordinates: Tuple[float, float, float] - latitude, longitude, altitude in degrees and meters
        :param timestamp: Optional[float] - timestamp in seconds
        :param n_satellites: Number of satellites
        """
        if not self.connected:
            time.sleep(0.2)
            self._try_reconnect()
            return
        if time.monotonic() - self.last_heartbeat_time > self.heartbeat_wait_s:
            self.connected = False
            self._try_reconnect()
            return
        if not super().send_gps(coordinates, timestamp):
            return

        lat, lon, alt = coordinates
        timestamp_ms = int(timestamp) if timestamp else int(time.time_ns() // 1000)

        vn = ve = vd = 0  # cm/s (NED)
        vel = 0  # ground speed cm/s
        cog = 0  # course over ground cdeg (0..35999)

        self.connection.mav.hil_gps_send(
            timestamp_ms,  # time_usec (microseconds)
            3,  # fix_type (3 = GPS_FIX_TYPE_3D_FIX)
            int(lat * 1e7),  # lat (degrees * 1e7)
            int(lon * 1e7),  # lon (degrees * 1e7)
            int(alt * 1000),  # alt (mm above sea level)
            80,  # eph (cm) - horizontal position uncertainty
            120,  # epv (cm) - vertical position uncertainty
            vel,  # vel (cm/s) - ground speed
            vn,  # vn (cm/s) - velocity north
            ve,  # ve (cm/s) - velocity east
            vd,  # vd (cm/s) - velocity down
            cog,  # cog (cm/s) - Course over ground (NOT heading, but direction of movement),
            # 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
            n_satellites,  # satellites_visible
        )
        logger.info(f"Mavlink GPS send: {lat, lon, alt}")

    def close(self):
        """Close MAVLink connection."""
        if self.connection:
            self.connection.close()


class MavrosOutputAdapter(OutputAdapter):
    """MAVROS output adapter (ROS2)."""

    def __init__(
        self,
        topic_prefix: str = "mavros",
        frame_id: str = "map",
        frequency: int = 10,
        message_type: MessageType = MessageType.BOTH,
    ):
        """
        Initialize MAVROS adapter.

        :param topic_prefix: ROS2 topic prefix (default: "mavros")
        :param frame_id: Frame ID for messages
        """
        self.topic_prefix = topic_prefix
        self.frame_id = frame_id
        self.node = None
        self.pose_pub = None
        self.fake_gps_pub = None
        self._initialize()
        super().__init__(message_type, frequency)

    def _initialize(self):
        """Initialize ROS2 node and publisher."""
        try:
            # ignore import error
            import rclpy  # noqa: F401
            from geometry_msgs.msg import PoseStamped
            from sensor_msgs.msg import NavSatFix

            if not rclpy.ok():
                rclpy.init()

            from rclpy.node import Node

            self.node = Node("multilat_solver_output")
            self.PoseStamped = PoseStamped
            self.NavSatFix = NavSatFix

            topic = f"{self.topic_prefix}/fake_gps/mocap/pose"
            self.pose_pub = self.node.create_publisher(PoseStamped, topic, self.frequency)
            self.fake_gps_pub = self.node.create_publisher(NavSatFix, "uwb/gps", self.frequency)

            logger.info("MAVROS adapter initialized: %s", topic)
        except ImportError:
            raise ImportError("rclpy is required for MAVROS adapter. Install ROS2 dependencies.")

    def _spin_once(self):
        """Spin once to process callbacks."""
        if self.node:
            import rclpy

            rclpy.spin_once(self.node, timeout_sec=0.01)

    def send_position(self, position: Tuple[float, float, float], timestamp: Optional[float] = None):
        """Send position via ROS2 PoseStamped message."""
        if not super().send_position(position, timestamp):
            return
        if self.pose_pub is None:
            return

        x, y, z = position

        pose = self.PoseStamped()
        if self.node:
            pose.header.stamp = self.node.get_clock().now().to_msg()
        pose.header.frame_id = self.frame_id
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.position.z = z
        pose.pose.orientation.w = 1.0

        self.pose_pub.publish(pose)

        # Spin once to process callbacks
        if self.node:
            self._spin_once()

    def send_gps(
        self,
        coordinates: Tuple[float, float, float],
        timestamp: Optional[float] = None,
        n_satellites: int = 10,
    ):
        """Send GPS coordinates.

        :param coordinates: Tuple[float, float, float] - latitude, longitude, altitude in degrees and meters
        :param timestamp: Optional[float] - timestamp in seconds
        :param n_satellites: Number of satellites
        """
        if not super().send_gps(coordinates, timestamp, n_satellites):
            return
        if self.fake_gps_pub is None:
            return
        nav = self.NavSatFix()
        if self.node:
            nav.header.stamp = self.node.get_clock().now().to_msg()
        nav.header.frame_id = self.frame_id
        nav.latitude = coordinates[0]
        nav.longitude = coordinates[1]
        nav.altitude = coordinates[2]
        self.fake_gps_pub.publish(nav)

        # Spin once to process callbacks
        if self.node:
            import rclpy

            rclpy.spin_once(self.node, timeout_sec=0.01)

    def close(self):
        """Close ROS2 node."""
        if self.node:
            try:
                import rclpy

                rclpy.shutdown()
            except ImportError as exc:
                raise ImportError(
                    "rclpy is required for MAVROS adapter. Install ROS2 dependencies."
                ) from exc  # noqa: B904


class UDPOutputAdapter(OutputAdapter):
    """UDP output adapter."""

    def __init__(
        self,
        host: str = "127.0.0.1",
        port: int = 5006,
        message_type: MessageType = MessageType.BOTH,
        frequency: int = 10,
    ):
        """
        Initialize UDP adapter.

        :param host: Target host
        :param port: Target port
        :param message_type: Message type
        :param frequency: Frequency in Hz
        """
        self.host = host
        self.port = port
        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        logger.info("UDP output adapter initialized: %s:%s", host, port)
        super().__init__(message_type, frequency)

    def send_position(self, position: Tuple[float, float, float], timestamp: Optional[float] = None):
        """Send position via UDP as JSON.

        :param position: Tuple[float, float, float] - x, y, z in meters
        :param timestamp: Optional[float] - timestamp in seconds
        """
        if not super().send_position(position, timestamp):
            return
        x, y, z = position
        message = {
            "position": {"x": x, "y": y, "z": z},
            "timestamp": timestamp or time.time(),
        }

        data = json.dumps(message).encode()
        self.socket.sendto(data, (self.host, self.port))

    def send_gps(
        self,
        coordinates: Tuple[float, float, float],
        timestamp: Optional[float] = None,
        n_satellites: int = 10,
    ):
        """Send GPS coordinates.

        :param coordinates: Tuple[float, float, float] - latitude, longitude, altitude in degrees and meters
        :param timestamp: Optional[float] - timestamp in seconds
        :param n_satellites: Number of satellites
        """
        if not super().send_gps(coordinates, timestamp, n_satellites):
            return
        if self.socket is None:
            return
        lat, lon, alt = coordinates
        message = {
            "latitude": lat,
            "longitude": lon,
            "altitude": alt,
            "timestamp": timestamp or time.time(),
            "satellites": n_satellites,
        }
        data = json.dumps(message).encode()
        self.socket.sendto(data, (self.host, self.port))

    def close(self):
        """Close UDP socket."""
        if self.socket:
            self.socket.close()


class FileOutputAdapter(OutputAdapter):
    """File output adapter for logging positions."""

    def __init__(
        self,
        filepath: str | None = None,
        mode: str | None = None,
        message_type: MessageType = MessageType.BOTH,
        frequency: int = 10,
        **kwargs,
    ):
        """
        Initialize file adapter.

        :param filepath: Path to output file
        :param mode: File mode ('a' for append, 'w' for write)
        :param message_type: Message type
        :param frequency: Frequency in Hz
        :param kwargs: Additional keyword arguments
        """
        if filepath is None:
            filepath = kwargs.get("filepath", "positions.jsonl")
        extention = filepath.split(".")[-1]

        if extention not in ["jsonl", "csv"]:
            raise ValueError(f"Invalid file extension: {extention}")

        filename = filepath.replace(extention, "")[:-1]

        if mode is None:
            mode = kwargs.get("mode", "a")
        self.files = {}
        self.writers = {}
        self.is_csv = extention == "csv"

        if message_type in [MessageType.POSITION, MessageType.BOTH]:
            self.files[MessageType.POSITION] = open(f"{filename}_positions.{extention}", mode)
            if self.is_csv:
                self.writers[MessageType.POSITION] = csv.writer(self.files[MessageType.POSITION])
                self.writers[MessageType.POSITION].writerow(["timestamp", "x", "y", "z"])

        if message_type in [MessageType.GPS, MessageType.BOTH]:
            self.files[MessageType.GPS] = open(f"{filename}_gps.{extention}", mode)
            if self.is_csv:
                self.writers[MessageType.GPS] = csv.writer(self.files[MessageType.GPS])
                self.writers[MessageType.GPS].writerow(
                    ["timestamp", "latitude", "longitude", "altitude", "satellites"]
                )

        logger.info("File output adapter initialized: %s", filepath)
        super().__init__(message_type, frequency)

    def send_position(self, position: Tuple[float, float, float], timestamp: Optional[float] = None):
        """Write position to file as JSON line.

        :param position: Tuple[float, float, float] - x, y, z in meters
        :param timestamp: Optional[float] - timestamp in seconds
        """
        if not super().send_position(position, timestamp):
            return
        if self.files[MessageType.POSITION] is None:
            return
        x, y, z = position
        message = {
            "position": {"x": x, "y": y, "z": z},
            "timestamp": timestamp or time.time(),
        }
        if self.is_csv:
            self.writers[MessageType.POSITION].writerow([timestamp or time.time(), x, y, z])
        else:
            self.files[MessageType.POSITION].write(json.dumps(message) + "\n")
        self.files[MessageType.POSITION].flush()

    def send_gps(
        self,
        coordinates: Tuple[float, float, float],
        timestamp: Optional[float] = None,
        n_satellites: int = 10,
    ):
        """Send GPS coordinates.

        :param coordinates: Tuple[float, float, float] - latitude, longitude, altitude in degrees and meters
        :param timestamp: Optional[float] - timestamp in seconds
        :param n_satellites: Number of satellites
        """
        if not super().send_gps(coordinates, timestamp):
            return

        if self.files[MessageType.GPS] is None:
            return
        lat, lon, alt = coordinates
        message = {
            "latitude": lat,
            "longitude": lon,
            "altitude": alt,
            "timestamp": timestamp or time.time(),
            "satellites": n_satellites,
        }
        if self.is_csv:
            self.writers[MessageType.GPS].writerow([timestamp or time.time(), lat, lon, alt, n_satellites])
        else:
            self.files[MessageType.GPS].write(json.dumps(message) + "\n")
        self.files[MessageType.GPS].flush()

    def close(self):
        """Close file."""
        for file in self.files.values():
            file.close()


class ConsoleOutputAdapter(OutputAdapter):
    """Console output adapter for debugging."""

    def __init__(
        self,
        format: str | None = None,
        message_type: MessageType = MessageType.BOTH,
        frequency: int = 10,
        **kwargs,
    ):
        """
        Initialize console adapter.

        :param format: Output format ('json' or 'human')
        :param message_type: Message type
        :param frequency: Frequency in Hz
        :param kwargs: Additional keyword arguments
        """
        if format is None:
            format = kwargs.get("format", "json")
        self.format = format
        logger.info("Console output adapter initialized")
        super().__init__(message_type, frequency)

    def send_position(self, position: Tuple[float, float, float], timestamp: Optional[float] = None):
        """Print position to console (stdout) for debugging.

        :param position: Tuple[float, float, float] - x, y, z in meters
        :param timestamp: Optional[float] - timestamp in seconds
        """
        if not super().send_position(position, timestamp):
            return
        x, y, z = position
        if self.format == "json":
            message = {
                "position": {"x": x, "y": y, "z": z},
                "timestamp": timestamp or time.time(),
            }
            print(json.dumps(message))
        else:
            print(f"Position: x={x:.3f}, y={y:.3f}, z={z:.3f}")

    def send_gps(
        self,
        coordinates: Tuple[float, float, float],
        timestamp: Optional[float] = None,
        n_satellites: int = 10,
    ):
        """Send GPS coordinates to console (stdout) for debugging.

        :param coordinates: Tuple[float, float, float] - latitude, longitude, altitude in degrees and meters
        :param timestamp: Optional[float] - timestamp in seconds
        :param n_satellites: Number of satellites
        """
        if not super().send_gps(coordinates, timestamp):
            return
        if self.format == "json":
            message = {
                "latitude": coordinates[0],
                "longitude": coordinates[1],
                "altitude": coordinates[2],
                "timestamp": timestamp or time.time(),
                "satellites": n_satellites,
            }
            print(json.dumps(message))
        else:
            print(
                f"GPS: latitude={coordinates[0]:.6f},\
                longitude={coordinates[1]:.6f},\
                altitude={coordinates[2]:.3f},\
                satellites={n_satellites}"
            )

    def close(self):
        """Nothing to close for console output."""
        pass


class MultiOutputAdapter:
    """Adapter that sends to multiple output adapters."""

    def __init__(self, adapters: list[OutputAdapter]):
        """
        Initialize multi-output adapter.

        :param adapters: List of output adapters
        """
        self.adapters: list[OutputAdapter] = adapters
        logger.info("Multi-output adapter initialized with %s adapters", len(adapters))

    def send_position(self, position: Tuple[float, float, float], timestamp: Optional[float] = None):
        """Send position to all adapters.

        :param position: Tuple[float, float, float] - x, y, z in meters
        :param timestamp: Optional[float] - timestamp in seconds
        """
        for adapter in self.adapters:
            adapter.send_position(position, timestamp)

    def send_gps(
        self,
        coordinates: Tuple[float, float, float],
        timestamp: Optional[float] = None,
        n_satellites: int = 10,
    ):
        """Send GPS coordinates to all adapters.

        :param coordinates: Tuple[float, float, float] - latitude, longitude, altitude in degrees and meters
        :param timestamp: Optional[float] - timestamp in seconds
        :param n_satellites: Number of satellites
        """
        for adapter in self.adapters:
            adapter.send_gps(coordinates, timestamp, n_satellites)

    def close(self):
        """Close all adapters."""
        for adapter in self.adapters:
            adapter.close()


OUTPUT_ADAPTERS = {
    "mavlink": MavlinkOutputAdapter,
    "mavros": MavrosOutputAdapter,
    "udp": UDPOutputAdapter,
    "file": FileOutputAdapter,
    "console": ConsoleOutputAdapter,
    "multi": MultiOutputAdapter,
}


def register_output_adapter(adapter_type: str, adapter: Any) -> None:
    """Register output adapter."""
    if adapter_type not in OUTPUT_ADAPTERS:
        raise ValueError(f"Invalid adapter type: {adapter_type}")
    if adapter is None:
        raise ValueError("Adapter cannot be None")
    if not issubclass(type(adapter), OutputAdapter):
        raise ValueError("Adapter must be an instance of OutputAdapter")
    if adapter_type in OUTPUT_ADAPTERS:
        raise ValueError(f"Adapter type {adapter_type} already registered")
    OUTPUT_ADAPTERS[adapter_type] = adapter
