"""Conversion helpers for encoder/angle/speed units."""

from __future__ import annotations

from .constants import ServoParams


def clamp(value: float, min_value: float, max_value: float) -> float:
    """Clamp numeric value to [min_value, max_value]."""
    return max(min_value, min(value, max_value))


def encoder_to_angle(
    encoder: int, offset: int = ServoParams.POSITION_CENTER, direction: int = 1
) -> float:
    """Convert encoder value to joint angle in degrees."""
    delta = encoder - offset
    return round(delta * ServoParams.POSITION_RESOLUTION_DEG * direction, 3)


def angle_to_encoder(
    angle: float, offset: int = ServoParams.POSITION_CENTER, direction: int = 1
) -> int:
    """Convert joint angle in degrees to encoder value (0-4095)."""
    raw = int(offset + angle / ServoParams.POSITION_RESOLUTION_DEG / direction)
    return int(clamp(raw, ServoParams.POSITION_MIN, ServoParams.POSITION_MAX))


def angles_to_encoders(
    angles: list[float], offsets: list[int], directions: list[int]
) -> list[int]:
    """Batch angle->encoder conversion."""
    return [
        angle_to_encoder(angle, offset, direction)
        for angle, offset, direction in zip(angles, offsets, directions)
    ]


def encoders_to_angles(
    encoders: list[int], offsets: list[int], directions: list[int]
) -> list[float]:
    """Batch encoder->angle conversion."""
    return [
        encoder_to_angle(encoder, offset, direction)
        for encoder, offset, direction in zip(encoders, offsets, directions)
    ]


def voltage_raw_to_v(raw: int) -> float:
    """Convert raw voltage register to volts."""
    return round(raw * ServoParams.VOLTAGE_RESOLUTION_V, 3)


def current_raw_to_ma(raw: int) -> float:
    """Convert raw current register to mA."""
    return round(raw * ServoParams.CURRENT_RESOLUTION_MA, 3)
