from i2c_handler import I2CSlaveHandler, Packet
from machine import Pin

# -high + low -> nach vorne
# andersrum nach hinten

in1_1 = Pin(18, Pin.OUT) # front right -
in1_2 = Pin(19, Pin.OUT) # front right +
in1_3 = Pin(20, Pin.OUT) # front left -
in1_4 = Pin(21, Pin.OUT) # front left + 

in2_1 = Pin(13, Pin.OUT) # back right -
in2_2 = Pin(12, Pin.OUT) # back right +
in2_3 = Pin(11, Pin.OUT) # back left -
in2_4 = Pin(10, Pin.OUT) # back left +

def set_motor(direction, pin1, pin2):
    if direction == 0x00:  # off
        pin1.value(0)
        pin2.value(0)
    elif direction == 0x01:  # forward
        pin1.value(1)
        pin2.value(0)
    elif direction == 0x02:  # backward
        pin1.value(0)
        pin2.value(1)

class MotorHandler(I2CSlaveHandler):
    def process_packet(self, packet: Packet):
        """
        Example: Override process_packet to perform actions based on packet command/type.
        """
        if packet[0] == 0x01:
            # Motor directions
            fl, fr, bl, br = packet[2], packet[3], packet[4], packet[5]

            # Map motors to their control pins
            motors = {
                'fl': (fl, in1_3, in1_4),
                'fr': (fr, in1_1, in1_2),
                'bl': (bl, in2_3, in2_4),
                'br': (br, in2_1, in2_2)
            }

            # Set each motor
            for name, (direction, pin1, pin2) in motors.items():
                set_motor(direction, pin1, pin2)

        else:
            print("Unknown command")

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

if __name__ == "__main__":
    main()