from i2c_handler import I2CSlaveHandler, Packet
from servo import Servo

s1 = Servo(0)
s1.middle()

def servo_Map(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
def servo_Angle(angle, servo):
    if angle < 0:
        angle = 0
    if angle > 180:
        angle = 180
    servo.goto(round(servo_Map(angle,0,180,0,1024))) # Convert range value to angle value
    
def move_body_parts(angle, servo):
    for i in range(90, 90 + angle, 5):
        servo_Angle(i, servo)
        utime.sleep(0.05)
    for i in range(90 + angle, 90 - angle, -5):
        servo_Angle(i, servo)
        utime.sleep(0.05)
    for i in range(90 - angle, 90, 5):
        servo_Angle(i, servo)
        utime.sleep(0.05)

class ServoHandler(I2CSlaveHandler):
    def process_packet(self, packet: Packet):
        """
        Example: Override process_packet to perform actions based on packet command/type.
        """
        if packet[0] == 0x01:
            if packet[2] == 0x00:
                s1.middle()
            if packet[2] == 0x01:
                s1.move_body_parts(15, s1)
        else:
            print('Unknown command')

# Usage example
def main():
    handler = ServoHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41)
    print("ServoHandler I2C slave started")
    try:
        handler.handle()
    except KeyboardInterrupt:
        handler.deinit()
        print("I2C slave stopped")

if __name__ == "__main__":
    main()