#!/usr/bin/env python3
"""
Universal MultiLat Localization Application.

Supports flexible input/output adapters for distance measurements and position output.
"""

import logging
import signal
import sys
import time
from typing import Dict, List, Tuple

import pymap3d as pm

from .config import MultiLatLocalizerConfig
from .core import LocalizationEngine
from .input_adapters import InputAdapter
from .output_adapters import ConsoleOutputAdapter, MultiOutputAdapter, OutputAdapter

logger = logging.getLogger(__name__)


class MultiLatLocalizerApp:
    """Main application class."""

    def __init__(self, config: MultiLatLocalizerConfig):
        """
        Initialize application with configuration.

        :param config: MultiLatLocalizerConfig instance
        """
        self.config: MultiLatLocalizerConfig = config
        self.engine: LocalizationEngine | None = None
        self.input_adapter: InputAdapter | None = None
        self.output_adapter: OutputAdapter | None = None
        self.running: bool = False
        self.last_position: Tuple[float, float, float] | None = None
        self.position_history: List[Tuple[float, float, float]] = []
        self.distances: Dict[int, Dict[str, float]] = {}
        self.last_timestamp: float = 0.0

    def _exit_signal_handler(self) -> None:
        """Handle shutdown signals."""
        logger.info("Shutting down...")
        self.stop()
        sys.exit(0)

    def _register_signal_handlers(self) -> None:
        """Register signal handlers."""
        signal.signal(signal.SIGINT, self._exit_signal_handler)
        signal.signal(signal.SIGTERM, self._exit_signal_handler)

    def _create_msg_adapters(self) -> None:
        """Create message adapters."""
        # Create input adapter
        self.input_adapter = self.config.input.adapter
        self.input_adapter.start(self._on_distance_update)
        logger.info("Input adapter started: %s", type(self.input_adapter).__name__)

        # Create output adapter
        self.output_adapter = self._create_output_adapter()
        logger.info("Output adapter initialized: %s", type(self.output_adapter).__name__)

    def _create_localization_engine(self) -> LocalizationEngine:
        """Create localization engine from config."""
        loc_config = self.config.localization

        return LocalizationEngine(loc_config)

    def _create_output_adapter(self) -> MultiOutputAdapter:
        """Create output adapter(s) from config."""
        output_config = self.config.output

        # Handle multiple outputs
        adapter_types = output_config.types
        if isinstance(adapter_types, str):
            adapter_types = [adapter_types]

        adapters = []

        for adapter_type in adapter_types:
            if adapter_type not in output_config.adapters:
                raise ValueError(f"Unknown adapter type: {adapter_type}")
            adapter = output_config.adapters[adapter_type]
            adapters.append(adapter)
        if not adapters:
            adapters.append(ConsoleOutputAdapter())

        if len(adapters) == 1:
            return adapters[0]
        return MultiOutputAdapter(adapters)

    def _clear_outdated_records(self, timestamp: float) -> Dict[int, Dict[str, float]]:
        """Clear outdated records from the distances dictionary."""
        for anchor_id, record in list(self.distances.items()):
            if record["timestamp"] < timestamp - self.config.localization.data_expiration_timeout_ms:
                del self.distances[anchor_id]
        return self.distances

    def _on_distance_update(self, distances: Dict[int, float]) -> None:
        """Handle distance update."""
        if not self.engine:
            return
        logger.debug(f"Distances: {distances}")
        # Apply calibration
        last_timestamp = 0
        anchor_ids = set(list(self.distances.keys()) + list(distances.keys()))
        if len(anchor_ids) == 0:
            return
        for anchor_id in anchor_ids:
            if anchor_id in distances:
                if distances[anchor_id]["distance"] <= self.config.localization.max_range:
                    self.distances[anchor_id] = distances[anchor_id]
                    continue
            if anchor_id in self.distances:
                self.distances.pop(anchor_id)

        calibrated_distances = {
            anchor_id: self.engine.calibrate(record["distance"])
            for anchor_id, record in self.distances.items()
        }
        logger.debug(f"Calibrated distances: {calibrated_distances}")
        logger.info("Max range: %s", self.config.localization.max_range)
        # Calculate position
        position = self.engine.calculate_position(calibrated_distances)
        print(f"Number of distances: {len(calibrated_distances)}")
        if position:
            self.position_history.append(position)
            self._send_position(position, last_timestamp)
            self._send_gps(position, last_timestamp, len(calibrated_distances))
            self.last_position = position

            if len(self.position_history) > 1000:  # Keep last 1000 positions
                self.position_history.pop(0)
        else:
            logger.warning("Could not calculate position from distances: %s", calibrated_distances)

    def _send_position(self, position: Tuple[float, float, float], timestamp: float) -> None:
        """Send position to output adapter."""
        if self.output_adapter:
            self.output_adapter.send_position(position, timestamp)

    def _send_gps(self, position: Tuple[float, float, float], timestamp: float, n_satellites: int) -> None:
        """Send GPS to output adapter."""
        if self.output_adapter:
            assert isinstance(self.engine, LocalizationEngine)
            # Convert ENU to GPS if origin is set
            if self.engine.origin_coordinates:
                lat, lon, alt = pm.enu2geodetic(
                    position[0],
                    position[1],
                    position[2],
                    self.engine.origin_coordinates[0],
                    self.engine.origin_coordinates[1],
                    self.engine.origin_coordinates[2],
                    ell=self.engine.ellipsoid,
                )
                self.output_adapter.send_gps((lat, lon, alt), timestamp, n_satellites)
            else:
                # If no origin, send position as-is (assuming it's already GPS)
                self.output_adapter.send_gps(position, timestamp, n_satellites)

    def start(self) -> None:
        """Start the localization system."""
        if not logging.getLogger().handlers:
            logging.basicConfig(
                level=logging.INFO,
                format="%(asctime)s [%(levelname)s] %(name)s: %(message)s",
                datefmt="%Y-%m-%d %H:%M:%S",
            )
        logger.info("Initializing MultiLat Localization System...")

        # Create localization engine
        self.engine = self._create_localization_engine()
        logger.info(
            "Localization engine initialized with %s anchors",
            len(self.engine.anchor_positions),
        )

        # Register message publishers
        self._create_msg_adapters()

        self.running = True
        logger.info("MultiLat Localization System started. Press Ctrl+C to stop.")

    def stop(self) -> None:
        """Stop the localization system."""
        self.running = False

        if self.input_adapter:
            self.input_adapter.stop()
            logger.info("Input adapter stopped")

        if self.output_adapter:
            self.output_adapter.close()
            logger.info("Output adapter closed")

        logger.info("MultiLat Localization System stopped")

    def run(self) -> None:
        """Run the main loop."""
        self.start()

        try:
            while self.running:
                time.sleep(0.1)
        except KeyboardInterrupt:
            pass
        finally:
            self.stop()
