"""Core localization module using multilateration algorithm."""

import logging
from typing import Dict, List, Optional, Tuple

import localization as lx
import pymap3d as pm

from multilat_solver.config import LocalizationConfig

logger = logging.getLogger(__name__)


def calibrated_linear(k: float, b: float, x: float) -> float:
    """Linear calibration: k * x + b."""
    return k * x + b


def calibrated_quadratic(a: float, b: float, c: float, x: float) -> float:
    """Quadratic calibration: a * x^2 + b * x + c."""
    return a * x**2 + b * x + c


def calibrated_cubic(a: float, b: float, c: float, d: float, x: float) -> float:
    """Cubic calibration: a * x^3 + b * x^2 + c * x + d."""
    return a * x**3 + b * x**2 + c * x + d


def multilateration(
    raw_data: Dict[int, float],
    anchor_positions: Dict[int, Tuple[float, float, float]] = {},
    z_sign: int = 0,
) -> Tuple[float, float, float] | None:
    """
    Multilateration algorithm using localization library.

    :param raw_data: Dictionary mapping anchor IDs to distances (meters)
    :param anchor_positions: Dictionary mapping anchor IDs to (x, y, z) positions
    :param z_sign: z sign, used if there are two solutions (0 = auto, +1 = positive, -1 = negative)
    :return: (x, y, z) position tuple
    """
    P = lx.Project(mode="3D", solver="LSE")
    non_zero_data = {}

    for anchor_id in raw_data.keys():
        if raw_data[anchor_id] is not None:
            non_zero_data[anchor_id] = raw_data[anchor_id]
    logger.debug(f"non_zero_data {non_zero_data}")
    if len(non_zero_data) < 3:
        raise ValueError(f"Not enough data for trilateration. Non-zero: {non_zero_data}, Input: {raw_data}")
    logger.debug(f"anchor_positions {anchor_positions}")
    if len(anchor_positions) < 3:
        raise ValueError(f"Not enough anchor positions for trilateration: {anchor_positions}")

    matched_ids = list(set(non_zero_data.keys()) & set(anchor_positions.keys()))
    if len(matched_ids) < 3:
        raise ValueError(f"Not enough common anchors for trilateration. Matched: {matched_ids}")

    for anchor_id in matched_ids:
        P.add_anchor(anchor_id, anchor_positions[anchor_id])

    t, _ = P.add_target()
    for anchor_id in matched_ids:
        t.add_measure(anchor_id, non_zero_data[anchor_id])

    P.solve()

    if z_sign != 0:
        t.loc.z = abs(t.loc.z) * z_sign

    return t.loc.x, t.loc.y, t.loc.z


class LocalizationEngine:
    """Main localization engine that processes distance measurements and calculates position."""

    __attributes__ = [
        "anchor_positions",
        "origin_coordinates",
        "ellipsoid",
        "calibration_type",
        "calibration_params",
        "z_sign",
        "min_range",
        "max_range",
    ]

    def __init__(self, config: LocalizationConfig):
        """
        Initialize localization engine from configuration.

        :param config: Localization configuration
        """
        self.ellipsoid: pm.Ellipsoid = pm.Ellipsoid.from_name(config.ellipsoid)
        self.anchor_positions: Dict[int, Tuple[float, float, float]] = config.anchor_positions
        self.origin_coordinates: Optional[Tuple[float, float, float]] = config.origin_coordinates
        self.calibration_type: str = config.calibration_type
        self.calibration_params: List[float] = config.calibration_params
        self.z_sign: int = config.z_sign
        self.min_range: float = config.min_range
        self.max_range: float = config.max_range

    def calibrate(self, raw_distance: float) -> float:
        """Apply calibration to raw distance measurement."""
        if self.calibration_type == "linear":
            if len(self.calibration_params) != 2:
                raise ValueError("Linear calibration requires 2 parameters [k, b]")
            return calibrated_linear(self.calibration_params[0], self.calibration_params[1], raw_distance)
        if self.calibration_type == "quadratic":
            if len(self.calibration_params) != 3:
                raise ValueError("Quadratic calibration requires 3 parameters [a, b, c]")
            return calibrated_quadratic(
                self.calibration_params[0],
                self.calibration_params[1],
                self.calibration_params[2],
                raw_distance,
            )
        if self.calibration_type == "cubic":
            if len(self.calibration_params) != 4:
                raise ValueError("Cubic calibration requires 4 parameters [a, b, c, d]")
            return calibrated_cubic(
                self.calibration_params[0],
                self.calibration_params[1],
                self.calibration_params[2],
                self.calibration_params[3],
                raw_distance,
            )
        return raw_distance

    def validate_range(self, distance: float) -> bool:
        """Check if distance is within valid range."""
        return self.min_range <= distance <= self.max_range

    def calculate_position(self, distances: Dict[int, float]) -> Optional[Tuple[float, float, float]]:
        """
        Calculate position from distance measurements.

        :param distances: Dictionary mapping anchor IDs to calibrated distances (meters)
        :return: (x, y, z) position tuple or None if calculation fails
        """
        logger.debug(f"distances {distances}")
        # Filter out None values and validate ranges
        valid_distances = {}
        for anchor_id, distance in distances.items():
            if distance is not None and self.validate_range(distance):
                valid_distances[anchor_id] = distance
        logger.debug(f"valid_distances {valid_distances}")
        if len(valid_distances) < 3:
            return None

        try:
            return multilateration(valid_distances, self.anchor_positions, self.z_sign)
        except ValueError as e:
            logger.error("Error in multilateration: %s", e)
            return None

    def _convert_to_gps(self, position: Tuple[float, float, float]) -> Tuple[float, float, float]:
        """Convert position to GPS coordinates."""
        if self.origin_coordinates is None:
            raise ValueError("Origin coordinates are not set")
        lat, lon, alt = pm.enu2geodetic(
            position[0],
            position[1],
            position[2],
            self.origin_coordinates[0],
            self.origin_coordinates[1],
            self.origin_coordinates[2],
            ell=self.ellipsoid,
        )
        return lat, lon, alt
