"""
Gestion des servomoteurs avec mapping deg↔µs↔ticks et courbes
"""

import math
from typing import Tuple, Dict, Any
from .config import ServoConfig
from .utils import clamp
from .exceptions import ServoError


class ServoController:
    """Contrôleur pour un servomoteur"""

    def __init__(self, name: str, config: ServoConfig, pca_freq: int = 50):
        self.name = name
        self.config = config
        self.pca_freq = pca_freq
        self._current_ticks = None
        self._target_ticks = None

        # Précalcul des constantes
        self._us_per_tick = 1_000_000 / (pca_freq * 4096)
        self._deg_to_us_ratio = (config.max_us - config.min_us) / (
            config.max_deg - config.min_deg
        )

        # Parser la courbe
        self._parse_curve()

    def _parse_curve(self):
        """Parse et configure la courbe de réponse"""
        if self.config.curve == "linear":
            self._curve_func = lambda x: x
        elif self.config.curve.startswith("gamma:"):
            try:
                gamma = float(self.config.curve.split(":")[1])
                self._curve_func = lambda x: math.pow(x, gamma)
            except (ValueError, IndexError):
                raise ServoError(f"Courbe gamma invalide: {self.config.curve}")
        else:
            raise ServoError(f"Type de courbe non supporté: {self.config.curve}")

    def deg_to_us(self, deg: float) -> int:
        """Convertit degrés → microsecondes avec courbe"""
        # Clamp en degrés
        deg_clamped = clamp(deg, self.config.min_deg, self.config.max_deg)

        # Normalisation [0, 1]
        norm = (deg_clamped - self.config.min_deg) / (
            self.config.max_deg - self.config.min_deg
        )

        # Appliquer courbe
        norm_curved = self._curve_func(norm)

        # Mapping µs
        us = self.config.min_us + norm_curved * (
            self.config.max_us - self.config.min_us
        )
        return int(round(us))

    def us_to_ticks(self, us: int) -> int:
        """Convertit microsecondes → ticks PCA9685"""
        ticks = round(us / self._us_per_tick)
        return clamp(ticks, 0, 4095)

    def deg_to_ticks(self, deg: float) -> int:
        """Convertit degrés → ticks (pipeline complet)"""
        us = self.deg_to_us(deg)
        return self.us_to_ticks(us)

    def ticks_to_us(self, ticks: int) -> int:
        """Convertit ticks → microsecondes"""
        return int(round(ticks * self._us_per_tick))

    def us_to_deg(self, us: int) -> float:
        """Convertit microsecondes → degrés (inverse)"""
        # Normalisation depuis µs
        norm_curved = (us - self.config.min_us) / (
            self.config.max_us - self.config.min_us
        )
        norm_curved = clamp(norm_curved, 0.0, 1.0)

        # Inverse de la courbe (approximation pour gamma)
        if self.config.curve == "linear":
            norm = norm_curved
        elif self.config.curve.startswith("gamma:"):
            gamma = float(self.config.curve.split(":")[1])
            norm = math.pow(norm_curved, 1.0 / gamma) if gamma != 0 else norm_curved
        else:
            norm = norm_curved

        # Dénormalisation
        deg = self.config.min_deg + norm * (self.config.max_deg - self.config.min_deg)
        return deg

    def ticks_to_deg(self, ticks: int) -> float:
        """Convertit ticks → degrés (pipeline inverse)"""
        us = self.ticks_to_us(ticks)
        return self.us_to_deg(us)

    def get_center_ticks(self) -> int:
        """Retourne les ticks pour la position centre"""
        return self.deg_to_ticks(self.config.center_deg)

    def validate_and_clamp_deg(self, deg: float) -> Tuple[float, bool]:
        """Valide et clamp un angle en degrés"""
        original = deg
        clamped = clamp(deg, self.config.min_deg, self.config.max_deg)
        was_clamped = abs(original - clamped) > 1e-6
        return clamped, was_clamped

    def get_capabilities(self) -> Dict[str, Any]:
        """Retourne les capacités du servo"""
        return {
            "min_deg": self.config.min_deg,
            "max_deg": self.config.max_deg,
            "center_deg": self.config.center_deg,
            "freq": self.pca_freq,
            "pca_addr": hex(0x40),  # Config centralisée
            "channel": self.config.channel,
            "curve": self.config.curve,
        }