import time
from adafruit_servokit import ServoKit

class MultiSmoothServoController:
    def __init__(self, channels=16, min_pulse=500, max_pulse=2500):
        self.kit = ServoKit(channels=channels)
        self.channels = channels
        self.min_pulse = min_pulse
        self.max_pulse = max_pulse
        
        # Track last angle and pulse per servo
        self._last_angles = [None] * channels
        self._last_pulses = [None] * channels
        
        # Set pulse width range for all channels upfront
        for ch in range(channels):
            self.kit.servo[ch].set_pulse_width_range(min_pulse, max_pulse)
    
    def angle_to_pulse(self, angle):
        """Convert angle (0-180) to pulse width in microseconds"""
        return int(self.min_pulse + (self.max_pulse - self.min_pulse) * angle / 180)
    
    def set_angle(self, channel, angle):
        """Set angle for a single servo channel, avoid jitter"""
        if channel < 0 or channel >= self.channels:
            raise ValueError(f"Channel must be between 0 and {self.channels - 1}")
        if angle < 0 or angle > 180:
            raise ValueError("Angle must be between 0 and 180")
        
        pulse = self.angle_to_pulse(angle)
        if self._last_angles[channel] != angle or self._last_pulses[channel] != pulse:
            self.kit.servo[channel].pulse_width = pulse
            self._last_angles[channel] = angle
            self._last_pulses[channel] = pulse
    
    def smooth_move(self, channel, start_angle, end_angle, step=1, delay=0.02):
        """Smoothly move one servo from start_angle to end_angle"""
        if start_angle < end_angle:
            angles = range(start_angle, end_angle + 1, step)
        else:
            angles = range(start_angle, end_angle - 1, -step)
        
        for angle in angles:
            self.set_angle(channel, angle)
            time.sleep(delay)
    
    def smooth_move_all(self, start_angles, end_angles, step=1, delay=0.02):
        """
        Smoothly move all servos from their respective start_angles to end_angles.
        Both start_angles and end_angles should be lists/tuples of length = number of channels.
        """
        if len(start_angles) != self.channels or len(end_angles) != self.channels:
            raise ValueError("start_angles and end_angles must have length equal to number of channels")
        
        max_steps = 0
        # Calculate how many steps each servo needs and track max
        steps_per_servo = []
        for start, end in zip(start_angles, end_angles):
            steps = abs(end - start) // step
            steps_per_servo.append(steps)
            if steps > max_steps:
                max_steps = steps
        
        for i in range(max_steps + 1):
            for ch in range(self.channels):
                start = start_angles[ch]
                end = end_angles[ch]
                
                if start < end:
                    angle = min(start + i * step, end)
                else:
                    angle = max(start - i * step, end)
                
                self.set_angle(ch, angle)
            time.sleep(delay)

    def stop_all(self):
        """Stop pulses to all servos (optional)"""
        for ch in range(self.channels):
            self.kit.servo[ch].pulse_width = 0
            self._last_angles[ch] = None
            self._last_pulses[ch] = None

controller = MultiSmoothServoController()

# Move servo channel 2 smoothly from 0 to 180 degrees
controller.smooth_move(channel=2, start_angle=0, end_angle=180)

# Smoothly move multiple servos simultaneously:
start_positions = [0] * 16          # all servos at 0°
end_positions = [90, 45, 180, 0, 30, 60, 90, 120, 150, 180, 0, 90, 45, 135, 180, 0]

controller.smooth_move_all(start_positions, end_positions)

# Stop all servos
controller.stop_all()