# Support for servos
#
# Copyright (C) 2017-2024  Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
from . import output_pin

SERVO_SIGNAL_PERIOD = 0.020

class PrinterServo:
    def __init__(self, config):
        self.printer = config.get_printer()
        self.min_width = config.getfloat('minimum_pulse_width', .001,
                                         above=0., below=SERVO_SIGNAL_PERIOD)
        self.max_width = config.getfloat('maximum_pulse_width', .002,
                                         above=self.min_width,
                                         below=SERVO_SIGNAL_PERIOD)
        self.max_angle = config.getfloat('maximum_servo_angle', 180.)
        self.signal_duration = config.getfloat('signal_duration', 0, minval=0.)
        if self.signal_duration:
            self.signal_duration = max(self.signal_duration, SERVO_SIGNAL_PERIOD)
            if abs(self.signal_duration / SERVO_SIGNAL_PERIOD - round(self.signal_duration / SERVO_SIGNAL_PERIOD)) < 1e-9:
                self.signal_duration -= 0.001
        self.steps_decomposed = config.getint('steps_decomposed', 0)
        self.angle_to_width = (self.max_width - self.min_width) / self.max_angle
        self.width_to_value = 1. / SERVO_SIGNAL_PERIOD
        self.last_value = 0.
        self.initial_pwm = 0.
        iangle = config.getfloat('initial_angle', None, minval=0., maxval=360.)
        if iangle is not None:
            self.initial_pwm = self._get_pwm_from_angle(iangle)
        else:
            iwidth = config.getfloat('initial_pulse_width', 0.,
                                     minval=0., maxval=self.max_width)
            self.initial_pwm = self._get_pwm_from_pulse_width(iwidth)
        # Setup mcu_servo pin
        ppins = self.printer.lookup_object('pins')
        self.mcu_servo = ppins.setup_pin('pwm', config.get('pin'))
        self.mcu_servo.setup_max_duration(0.)
        self.mcu_servo.setup_cycle_time(SERVO_SIGNAL_PERIOD)
        self.mcu_servo.setup_start_value(self.initial_pwm, 0.)
        # Register event handler
        self.printer.register_event_handler("klippy:ready", self._handle_ready)
        # Create gcode request queue
        self.gcrq = output_pin.GCodeRequestQueue(
            config, self.mcu_servo.get_mcu(), self.__set_pwm)
        # Register commands
        servo_name = config.get_name().split()[1]
        gcode = self.printer.lookup_object('gcode')
        gcode.register_mux_command("SET_SERVO", "SERVO", servo_name,
                                   self.cmd_SET_SERVO,
                                   desc=self.cmd_SET_SERVO_help)
    def get_status(self, eventtime):
        return {'value': self.last_value}
    def __set_pwm(self, print_time, value):
        if self.steps_decomposed:
            return self._set_low_pwm(print_time, value)
        else:
            return self._set_pwm(print_time, value)
    def _set_pwm(self, print_time, value):
        if value == self.last_value:
            return "discard", 0.
        self.last_value = value
        self.mcu_servo.set_pwm(print_time, value)
        if self.signal_duration:
            self.mcu_servo.set_pwm(print_time + self.signal_duration, 0)
    def _get_s_curve_value(self, last_value, value, t):
        smooth_factor = t * t * (3 - 2 * t)
        return last_value + smooth_factor * (value - last_value)
    def _set_low_pwm(self, print_time, value):
        if value == self.last_value:
            return "discard", 0.
        enter_value = self.initial_pwm if self.last_value == 0 else self.last_value
        self.last_value = value
        steps = self.steps_decomposed
        for step in range(steps):
            t = step / (steps - 1)
            current_value = self._get_s_curve_value(enter_value, value, t)
            next_time = print_time + SERVO_SIGNAL_PERIOD * step
            self.mcu_servo.set_pwm(next_time, current_value)
        if self.signal_duration:
            next_time = print_time + SERVO_SIGNAL_PERIOD * steps + self.signal_duration
            self.mcu_servo.set_pwm(next_time, 0)
    def _handle_ready(self):
        self.gcrq.queue_gcode_request(self.initial_pwm)
    def _get_pwm_from_angle(self, angle):
        angle = max(0., min(self.max_angle, angle))
        width = self.min_width + angle * self.angle_to_width
        return width * self.width_to_value
    def _get_pwm_from_pulse_width(self, width):
        if width:
            width = max(self.min_width, min(self.max_width, width))
        return width * self.width_to_value
    cmd_SET_SERVO_help = "Set servo angle"
    def cmd_SET_SERVO(self, gcmd):
        width = gcmd.get_float('WIDTH', None)
        if width is not None:
            value = self._get_pwm_from_pulse_width(width)
        else:
            angle = gcmd.get_float('ANGLE')
            value = self._get_pwm_from_angle(angle)
        self.gcrq.queue_gcode_request(value)

def load_config_prefix(config):
    return PrinterServo(config)