"""Default values provided are from the DS3218MG.

Pins GPIOs 34 to 39 can’t generate PWM on the ESP32.

Description:
    High-precision metal gears with hard anodizing
    CNC aluminum middle Shell
    water proof rubber seals screws
Specification:
    Stall Torque (5V): 19 kg/cm (263.8oz/in)
    Stall Torque (6.8V): 21.5 kg/cm (298.5 oz/in)
    Dead band: 3μs
    Speed : 0.16 sec/60°(5V) / 0.14 sec/60°(6.8V)
    Operating Voltage: 4.8 ~ 6.8 DC Volts
    Weight: 60 g (2.12 oz)
    Motor Type: DC Motor
    Gear Type: Copper & Aluminum
    Working frequence: 1520μs / 333hz
    CE Certification: Yes
    Size: 40 x 20 x 40.5 mm ( 1.58 x 0.79 x 1.60 in)
    Package Includes:
    4 X DS3218 Digital Servo 20KG
    4 X 25T Servo Arm

micro-seconds on 10**-6 seconds.


"""
from machine import Pin, PWM
import math


class Servo:

    def __init__(self, pin, freq=40, min_us=500, max_us=2500,
                 degrees=270):
        """
        :param pin: machine.Pin
            A PWM enabled Pin
        :param freq: int
            The frequency in Hz. Typical values for servos are 40 or 50.
        :param min_us: int
            The minimum number of microseconds
        :param max_us:
            The maximum number of microseconds
        :param degrees: int
            The range in degrees of the motor
        """
        self.period = 1000000 / freq
        self.min_duty = self._us2duty(min_us)
        self.max_duty = self._us2duty(max_us)
        self.degrees = degrees
        self.freq = freq
        self.pin = PWM(pin, freq=freq)

    def _us2duty(self, value):
        """Converts a microsecond value to a duty cycle value."""
        return int(1024 * value / self.period)

    def position(self, degrees=None, radians=None, us=None, duty=None):
        span = self.max_duty - self.min_duty  # 409.6
        if degrees is not None:
            duty = self.min_duty + span * degrees / self.degrees
        elif radians is not None:
            duty = self.min_duty + span * radians / math.radians(self.degrees)
        elif us is not None:
            duty = self._us2duty(us)
        elif duty is not None:
            pass
        else:
            return self.pin.duty()
        duty = min(self.max_duty, max(self.min_duty, int(duty)))
        self.pin.duty(duty)

    def release(self):
        self.pin.duty(0)