# servo.py — MG995 (and other analog hobby servos), 50 Hz PWM
from machine import Pin, PWM

# MG995: treat as standard 1.0–2.0 ms for ~0–180°; many units reach full travel closer to 0.5–2.5 ms.
_DEFAULT_MIN_US = 1000
_DEFAULT_MAX_US = 2000
_WIDE_MIN_US = 500
_WIDE_MAX_US = 2500


class MG995:
    def __init__(
        self,
        pin,
        freq=50,
        min_us=_DEFAULT_MIN_US,
        max_us=_DEFAULT_MAX_US,
        angle_max=180,
        wide_range=False,
    ):
        """
        Drive one MG995 on `pin` (GPIO number or Pin).

        `wide_range=True` uses ~500–2500 µs pulse limits (often needed for full
        mechanical sweep); default 1000–2000 µs is gentler on the gearbox.

        `angle_max` is the upper limit passed to write_angle (default 180).
        """
        if wide_range:
            min_us, max_us = _WIDE_MIN_US, _WIDE_MAX_US
        self._pin_id = pin
        self._freq = int(freq)
        self._min_us = int(min_us)
        self._max_us = int(max_us)
        self._angle_max = float(angle_max)
        self._pwm = PWM(Pin(pin), freq=self._freq, duty_u16=0)
        self._use_ns = hasattr(self._pwm, 'duty_ns')
        self._period_us = 1_000_000 // self._freq if self._freq > 0 else 20_000

    def write_microseconds(self, us):
        """Set pulse width in microseconds (clamped to configured min/max)."""
        us = max(self._min_us, min(self._max_us, int(us)))
        if self._use_ns:
            self._pwm.duty_ns(us * 1000)
        else:
            # Fraction of period → duty_u16 (ESP32-style full-scale mapping)
            self._pwm.duty_u16(max(0, min(65535, int(us * 65535 / self._period_us)))))

    def write_angle(self, degrees):
        """Map `degrees` (0 … angle_max) to pulse between min_us and max_us."""
        a = max(0.0, min(self._angle_max, float(degrees)))
        span = self._max_us - self._min_us
        us = self._min_us + int(round(span * (a / self._angle_max)))
        self.write_microseconds(us)

    def center(self):
        """Mid pulse (~center position for default symmetric limits)."""
        self.write_microseconds((self._min_us + self._max_us) // 2)

    def off(self):
        """Stop PWM output (no holding torque from the signal)."""
        try:
            self._pwm.duty_u16(0)
        except Exception:
            pass
        try:
            if self._use_ns:
                self._pwm.duty_ns(0)
        except Exception:
            pass

    def deinit(self):
        self.off()
        self._pwm.deinit()