Cognitive_Agent/src/cognitive_agent/application/vehicle_profile.py

"""Vehicle-specific seat calibration profiles."""

from __future__ import annotations

from dataclasses import dataclass
from pathlib import Path
from typing import Any

import yaml


@dataclass(frozen=True)
class SeatMotorProfile:
    id: str
    name: str
    unit: str
    range: float
    reference_zero: str
    positive_direction: str
    hall_total_pulses: int
    display_angle: dict[str, Any] | None = None

    def delta_to_pct(self, delta: float) -> float:
        if self.range <= 0:
            raise ValueError(f"motor {self.id} range must be positive")
        return float(delta) / self.range * 100

    def display_angle_for_pct(self, position_pct: int) -> float | None:
        if not self.display_angle:
            return None
        zero = float(self.display_angle["zero_deg"])
        maximum = float(self.display_angle["max_deg"])
        return zero + (maximum - zero) * _clamp_pct(position_pct) / 100

    def position_pct_for_display_angle(self, angle_deg: int | float) -> int:
        if not self.display_angle:
            raise ValueError(f"motor {self.id} does not define a display angle range")
        zero = float(self.display_angle["zero_deg"])
        maximum = float(self.display_angle["max_deg"])
        if maximum <= zero:
            raise ValueError(f"motor {self.id} display angle range must be positive")
        normalized = (float(angle_deg) - zero) / (maximum - zero)
        return _clamp_pct(round(normalized * 100))

    def contains_display_angle(self, angle_deg: int | float) -> bool:
        if not self.display_angle:
            return False
        zero = float(self.display_angle["zero_deg"])
        maximum = float(self.display_angle["max_deg"])
        return zero <= float(angle_deg) <= maximum

    def public_dict(self) -> dict[str, Any]:
        payload: dict[str, Any] = {
            "id": self.id,
            "name": self.name,
            "unit": self.unit,
            "range": _public_number(self.range),
            "reference_zero": self.reference_zero,
            "positive_direction": self.positive_direction,
            "hall_total_pulses": self.hall_total_pulses,
        }
        if self.display_angle is not None:
            payload["display_angle"] = dict(self.display_angle)
        return payload


@dataclass(frozen=True)
class VehicleProfile:
    id: str
    name: str
    seat_motors: dict[str, SeatMotorProfile]

    def relative_target_pct(
        self,
        axis: str,
        *,
        current_pct: int | float,
        delta: int | float,
        direction: str,
    ) -> int:
        motor = self.seat_motors[axis]
        delta_pct = motor.delta_to_pct(float(delta))
        if direction == "decrease":
            delta_pct = -delta_pct
        elif direction != "increase":
            raise ValueError(f"unsupported relative direction: {direction}")
        return _clamp_pct(round(float(current_pct) + delta_pct))

    def display_angle_for_pct(self, axis: str, position_pct: int) -> float | None:
        return self.seat_motors[axis].display_angle_for_pct(position_pct)

    def public_dict(self) -> dict[str, Any]:
        return {
            "id": self.id,
            "name": self.name,
            "seat": {
                "motors": {
                    key: motor.public_dict()
                    for key, motor in self.seat_motors.items()
                }
            },
        }


class VehicleProfileRegistry:
    def __init__(self, profile_root: Path) -> None:
        self.profile_root = profile_root
        self._profiles = self._load_profiles(profile_root)

    def get(self, vehicle_id: str | None) -> VehicleProfile:
        resolved = vehicle_id or self.default().id
        try:
            return self._profiles[resolved]
        except KeyError as exc:
            raise ValueError(f"unknown vehicle profile: {resolved}") from exc

    def default(self) -> VehicleProfile:
        if "denza_n7" in self._profiles:
            return self._profiles["denza_n7"]
        try:
            return next(iter(self._profiles.values()))
        except StopIteration as exc:
            raise ValueError(f"no vehicle profiles found in {self.profile_root}") from exc

    def public_profiles(self) -> list[dict[str, Any]]:
        return [profile.public_dict() for profile in self._profiles.values()]

    @staticmethod
    def _load_profiles(profile_root: Path) -> dict[str, VehicleProfile]:
        profiles: dict[str, VehicleProfile] = {}
        for path in sorted(profile_root.glob("*.yaml")):
            data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
            profile = _parse_vehicle_profile(data)
            profiles[profile.id] = profile
        return profiles


def _parse_vehicle_profile(data: dict[str, Any]) -> VehicleProfile:
    seat = data.get("seat") or {}
    motors = seat.get("motors") or {}
    seat_motors = {
        str(axis): SeatMotorProfile(
            id=str(axis),
            name=str(payload["name"]),
            unit=str(payload["unit"]),
            range=float(payload["range"]),
            reference_zero=str(payload["reference_zero"]),
            positive_direction=str(payload["positive_direction"]),
            hall_total_pulses=int(payload["hall_total_pulses"]),
            display_angle=payload.get("display_angle"),
        )
        for axis, payload in motors.items()
    }
    return VehicleProfile(
        id=str(data["id"]),
        name=str(data["name"]),
        seat_motors=seat_motors,
    )


def _clamp_pct(value: int | float) -> int:
    return max(0, min(100, int(value)))


def _public_number(value: float) -> int | float:
    return int(value) if value.is_integer() else value