import utime
from machine import Pin, PWM


class ServoMotor:
    """微型伺服电机"""

    def __init__(self, pin, frequency=50, min_angle=0, max_angle=180, min_pulsewidth=500, max_pulsewidth=2500, deadband_pulsewidth=8, duration_per_60deg=0.13):
        """
        :param pin: 绑定引脚
        :param frequency: PWM信号输出频率，单位为赫兹
        :param min_angle: 最小转动角度，单位为度
        :param max_angle: 最大转动角度，单位为度
        :param min_pulsewidth: 最小转动角度对应的脉宽，单位为微秒
        :param max_pulsewidth: 最大转动角度对应的脉宽，单位为微秒
        :param deadband_pulsewidth: 死区对应的脉宽，单位为微秒
        :param duration_per_60deg: 转动六十度对应的时长，单位为秒/六十度
        """
        print()
        print(f"正在初始化ServoMotor（绑定引脚{pin:2d}）...", end="")

        # 初始化最小转动角度和最大转动角度，单位为度
        self.min_angle, self.max_angle = min_angle, max_angle
        # 初始化最小转动角度对应的脉宽和最大转动角度对应的脉宽，单位为纳秒
        self.min_pulsewidth, self.max_pulsewidth = min_pulsewidth * 1000, max_pulsewidth * 1000
        # 初始化死区对应的脉宽，单位为纳秒
        self.deadband_pulsewidth = deadband_pulsewidth * 1000
        # 初始化单位角度对应的脉宽，单位为纳秒/度
        self.pulsewidth_per_degree = (self.max_pulsewidth - self.min_pulsewidth) / (self.max_angle - self.min_angle)
        # 初始化单位时长对应的脉宽，单位为纳秒/毫秒
        self.pulsewidth_per_millisecond = self.pulsewidth_per_degree * 60 / duration_per_60deg / 1000

        # 初始化目标角度对应的脉宽，单位为纳秒
        self.target_pulsewidth = self.min_pulsewidth
        # 初始化开始角度对应的脉宽和当前角度对应的脉宽，单位为纳秒
        self.start_pulsewidth = self.current_pulsewidth = self.min_pulsewidth
        # 初始化转动脉宽差，单位为纳秒
        self.pulsewidth_difference = 0

        # 初始化转动时长，单位为毫秒
        self.duration = 1000
        # 初始化转动步骤间延迟
        self.delay = int(round(1000 / frequency)) + 10
        # 初始化开始角度对应的时间戳和当前角度对应的时间戳，单位为毫秒
        self.start_time = self.current_time = utime.ticks_ms()
        # 初始化转动步骤迭代器
        self.iterator = None

        try:
            self.pwm = PWM(Pin(pin, Pin.OUT))  # 初始化PWM
            self.pwm.freq(frequency)  # 设置频率
        except Exception as exception:
            raise RuntimeError(f"初始化发生异常：{str(exception)}")

        # 打开PWM信号输出，最小转动角度对应的脉宽
        self.pwm.duty_ns(self.min_pulsewidth)
        utime.sleep_ms(self.duration)
        print("已完成")

    def rotate(self, target_angle, duration=1000):
        """
        将设置目标角度和执行转动步骤迭代器封装为转动接口
        """
        print(f"由{self._pulsewidth_to_angle(self.current_pulsewidth):3d}转动至{target_angle:3d}，预期转动{duration:4d}毫秒...", end="")

        if self._set_target_angle(target_angle, duration):
            while self._execute():
                pass
            print(f"已完成，实际转动{utime.ticks_diff(self.current_time, self.start_time):4d}毫秒")
            return True

        print("设置目标角度失败：微型伺服电机正在转动")
        return False

    def _set_target_angle(self, target_angle, duration):
        """
        设置目标角度
        :param target_angle: 目标角度，单位为度
        :param duration: 转动时长，单位为毫秒
        """
        if not isinstance(target_angle, int):
            raise TypeError("target_angle数据类型应为整数")

        if not isinstance(duration, int):
            raise TypeError("duration数据类型应为整数")

        # 若正在转动则返回失败
        if self._rotating:
            return False

        # 更新目标角度对应的脉宽
        self.target_pulsewidth = self._angle_to_pulsewidth(target_angle)
        # 更新开始角度对应的脉宽
        self.start_pulsewidth = self.current_pulsewidth
        # 更新转动脉宽差
        self.pulsewidth_difference = self.target_pulsewidth - self.start_pulsewidth

        # 若转动脉宽差的绝对值小于死区对应的脉宽则返回成功
        if abs(self.pulsewidth_difference) < self.deadband_pulsewidth:
            return True

        # 更新转动时长
        self.duration = max(
            self._estimate_min_duration(self.pulsewidth_difference),
            duration
        )
        # 更新当前角度对应的时间戳
        self.current_time = utime.ticks_ms()
        # 更新开始角度对应的时间戳
        self.start_time = self.current_time
        # 更新转动步骤迭代器
        self.iterator = self._generate_iterator()
        return True

    @property
    def _rotating(self):
        """是否正在转动"""
        return self.iterator is not None

    def _generate_iterator(self):
        """创建转动迭代器"""
        while True:
            # 当前角度对应的时间戳
            current_time = utime.ticks_ms()
            # 当前角度对应的进程
            process = max(
                0,
                min(
                    1,
                    utime.ticks_diff(current_time, self.start_time) / self.duration
                )
            )

            # 若当前角度对应的时间戳和上一次当前角度对应的时间戳大于等于转动步骤间延迟则继续，否则跳过
            if utime.ticks_diff(current_time, self.current_time) >= self.delay:
                # 当前角度对应的脉宽
                current_pulsewidth = max(
                    self.min_pulsewidth,
                    min(
                        self.max_pulsewidth,
                        int(round(self.start_pulsewidth + self.pulsewidth_difference * self._easing(process)))
                    )
                )

                # 若转动脉宽差的绝对值大于等于死区对应的脉宽则继续
                if abs(current_pulsewidth - self.current_pulsewidth) >= self.deadband_pulsewidth:
                    # 打开PWM信号输出，当前角度对应的脉宽
                    self.pwm.duty_ns(current_pulsewidth)
                    # 更新当前角度对应的脉宽
                    self.current_pulsewidth = current_pulsewidth
                    # 更新当前角度对应的时间戳
                    self.current_time = current_time

            # 若当前角度对应的进程大于等于1则继续
            if process >= 1:
                # 打开PWM信号输出，当前角度对应的脉宽
                self.pwm.duty_ns(self.target_pulsewidth)
                # 更新当前角度对应的脉宽
                self.current_pulsewidth = self.target_pulsewidth
                # 更新当前角度对应的时间戳
                self.current_time = utime.ticks_ms()
                yield
                break

            yield

    @staticmethod
    def _easing(process):
        """
        缓动函数：根据当前角度对应的进程计算当前角度
        :param process: 当前角度对应的进程
        """
        return 3 * process ** 2 - 2 * process ** 3

    def _execute(self):
        """执行转动步骤迭代器"""
        if self._rotating:
            try:
                next(self.iterator)
                return True
            except StopIteration:
                self._stop()
        return False

    def _stop(self):
        """停止转动"""
        self.iterator = None

    def _estimate_min_duration(self, pulsewidth_difference):
        """
        估计最小转动时长
        :param pulsewidth_difference: 转动脉宽差，单位为纳秒
        """
        min_duration = int(round(abs(pulsewidth_difference) / self.pulsewidth_per_millisecond))
        return min_duration

    def _angle_to_pulsewidth(self, angle):
        """
        将角度转为角度对应的脉宽
        :param angle: 角度，单位为度
        """
        pulsewidth = max(
            self.min_pulsewidth,
            min(
                self.max_pulsewidth,
                int(round(self.min_pulsewidth + (angle - self.min_angle) * self.pulsewidth_per_degree))
            )
        )
        return pulsewidth

    def _pulsewidth_to_angle(self, pulsewidth):
        """
        将角度对应的脉宽转为角度
        :param pulsewidth: 角度对应的脉宽，单位为纳秒
        """
        angle = max(
            self.min_angle,
            min(
                self.max_angle,
                int(round(self.min_angle + (pulsewidth - self.min_pulsewidth) / self.pulsewidth_per_degree))
            )
        )
        return angle


class SG90(ServoMotor):
    """SG90（适用于SG90和MG90S等舵机）"""
    def __init__(self, pin):
        super().__init__(
            pin=pin,
        )


"""
使用示例
引脚0→舵机信号引脚

import utime
from utils.servo import SG90

servo = SG90(pin=0)
servo.rotate(target_angle=180, duration=1000)
"""