from i2c_handler import I2CSlaveHandler, Packet

class MyRobotHandler(I2CSlaveHandler):
    def process_packet(self, packet: Packet):
        """
        Example: Override process_packet to perform actions based on packet command/type.
        """
        cmd = packet[0]
        target = packet[1]
        speed = (packet[2] << 8) | packet[3]
        # Convert speed from unsigned to signed if needed
        if speed & 0x8000:
            speed -= 0x10000

        print(f"Custom handler: Command 0x{cmd:02X}, Target 0x{target:02X}, Speed {speed}")

        # Example: Perform a motor action for command 0x01
        if cmd == 0x01:  # Set Motor Speed
            print(f"Setting motor {target} speed to {speed}")
            # TODO: Add your motor control code here

        elif cmd == 0xFF:  # Emergency Stop
            print("Emergency stop received!")
            # TODO: Add your emergency stop code here

        else:
            print("Unknown command")

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

if __name__ == "__main__":
    main()