# ----------------------------------------------------------------------------
# Kinisi motr controller commands.
# This file is auto generated by the commands generator from JSON file.
# Do not edit this file manually.
# Timestamp: 2024-02-20 23:13:00
# Version: 1.0.4
# ----------------------------------------------------------------------------

import struct

# The state of the controller for the specified motor.
# Properties:
#    motor_index: Index of the motor with the controller.
#    kp: Proportional constant of PID
#    ki: Integral constant of PID
#    kd: Derivative constant of PID
#    target_speed: The target speed of the motor.
#    current_speed: The current speed of the motor.
#    error: The error of the motor.
#    output: The output of the motor.
class MotorControllerState:
    def __init__(self, motor_index:int, kp:float, ki:float, kd:float, target_speed:float, current_speed:float, error:float, output:float):
        self.motor_index = motor_index
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.target_speed = target_speed
        self.current_speed = current_speed
        self.error = error
        self.output = output

    @staticmethod
    def get_size() -> int:
        return 57

    def encode(self) -> bytearray:
        msg = bytearray()
        msg += self.motor_index.to_bytes(1, 'little', signed=True)
        msg += bytearray(struct.pack('d', self.kp))
        msg += bytearray(struct.pack('d', self.ki))
        msg += bytearray(struct.pack('d', self.kd))
        msg += bytearray(struct.pack('d', self.target_speed))
        msg += bytearray(struct.pack('d', self.current_speed))
        msg += bytearray(struct.pack('d', self.error))
        msg += bytearray(struct.pack('d', self.output))
        return msg

    @staticmethod
    def decode(msg: bytearray) -> object:
        result = MotorControllerState(
            motor_index=int.from_bytes(msg[0:1], 'little'),
            kp=struct.unpack('<d', msg[1:9])[0],
            ki=struct.unpack('<d', msg[9:17])[0],
            kd=struct.unpack('<d', msg[17:25])[0],
            target_speed=struct.unpack('<d', msg[25:33])[0],
            current_speed=struct.unpack('<d', msg[33:41])[0],
            error=struct.unpack('<d', msg[41:49])[0],
            output=struct.unpack('<d', msg[49:57])[0])
        return result

# The velocity of the platform in meters per second.
# Properties:
#    x: X component of platform velocity in meters per second
#    y: Y component of platform velocity in meters per second
#    t: Theta component of platform velocity in radians per second
class PlatformVelocity:
    def __init__(self, x:float, y:float, t:float):
        self.x = x
        self.y = y
        self.t = t

    @staticmethod
    def get_size() -> int:
        return 24

    def encode(self) -> bytearray:
        msg = bytearray()
        msg += bytearray(struct.pack('d', self.x))
        msg += bytearray(struct.pack('d', self.y))
        msg += bytearray(struct.pack('d', self.t))
        return msg

    @staticmethod
    def decode(msg: bytearray) -> object:
        result = PlatformVelocity(
            x=struct.unpack('<d', msg[0:8])[0],
            y=struct.unpack('<d', msg[8:16])[0],
            t=struct.unpack('<d', msg[16:24])[0])
        return result

# The odometry of the platform in meters and radians.
# Properties:
#    x: X component of platform odometry in meters
#    y: Y component of platform odometry in meters
#    t: Theta component of platform odometry in radians
class PlatformOdometry:
    def __init__(self, x:float, y:float, t:float):
        self.x = x
        self.y = y
        self.t = t

    @staticmethod
    def get_size() -> int:
        return 24

    def encode(self) -> bytearray:
        msg = bytearray()
        msg += bytearray(struct.pack('d', self.x))
        msg += bytearray(struct.pack('d', self.y))
        msg += bytearray(struct.pack('d', self.t))
        return msg

    @staticmethod
    def decode(msg: bytearray) -> object:
        result = PlatformOdometry(
            x=struct.unpack('<d', msg[0:8])[0],
            y=struct.unpack('<d', msg[8:16])[0],
            t=struct.unpack('<d', msg[16:24])[0])
        return result


INITIALIZE_MOTOR = 0x01
SET_MOTOR_SPEED = 0x02
STOP_MOTOR = 0x03
BRAKE_MOTOR = 0x04
INITIALIZE_MOTOR_CONTROLLER = 0x05
SET_MOTOR_TARGET_SPEED = 0x06
RESET_MOTOR_CONTROLLER = 0x07
GET_MOTOR_CONTROLLER_STATE = 0x08
DELETE_MOTOR_CONTROLLER = 0x09
INITIALIZE_ENCODER = 0x11
GET_ENCODER_VALUE = 0x12
START_ENCODER_ODOMETRY = 0x13
RESET_ENCODER_ODOMETRY = 0x14
STOP_ENCODER_ODOMETRY = 0x15
GET_ENCODER_ODOMETRY = 0x16
INITIALIZE_GPIO_PIN = 0x20
SET_GPIO_PIN_STATE = 0x21
GET_GPIO_PIN_STATE = 0x22
TOGGLE_GPIO_PIN_STATE = 0x23
SET_STATUS_LED_STATE = 0x25
TOGGLE_STATUS_LED_STATE = 0x26
INITIALIZE_MECANUM_PLATFORM = 0x30
INITIALIZE_OMNI_PLATFORM = 0x31
SET_PLATFORM_VELOCITY = 0x40
START_PLATFORM_CONTROLLER = 0x41
SET_PLATFORM_TARGET_VELOCITY = 0x42
GET_PLATFORM_CURRENT_VELOCITY = 0x43
STOP_PLATFORM_CONTROLLER = 0x44
START_PLATFORM_ODOMETRY = 0x45
RESET_PLATFORM_ODOMETRY = 0x46
STOP_PLATFORM_ODOMETRY = 0x47
GET_PLATFORM_ODOMETRY = 0x48


class KinisiCommands:
    # Write command to serial interface
    def write(self, msg: bytearray):
        """Abstract method to write the byte message to the serial interface."""
        raise NotImplementedError("This method should be overridden by subclass.")
    
    # Read command from serial interface
    def read(self, length: int) -> bytearray:
        """Abstract method to read a specified number of bytes from the serial interface."""
        raise NotImplementedError("This method should be overridden by subclass.")
        # This command initializes a motor and prepares it for use.
    # Parameters:
    #   motor_index: The index of the motor to initialize.
    #   is_reversed: Whether or not the motor is reversed.
    def initialize_motor(self, motor_index:int, is_reversed:bool):
        msg = INITIALIZE_MOTOR.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little') + is_reversed.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the speed of the specified motor in PWM.
    # Parameters:
    #   motor_index: The index of the motor to set the speed for.
    #   pwm: The speed of the motor.
    def set_motor_speed(self, motor_index:int, pwm:float):
        msg = SET_MOTOR_SPEED.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little') + bytearray(struct.pack('d', pwm))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command stops motor by setting its speed to 0.
    # Parameters:
    #   motor_index: The index of the motor to set the speed for.
    def stop_motor(self, motor_index:int):
        msg = STOP_MOTOR.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command brakes motor.
    # Parameters:
    #   motor_index: The index of the motor to set the speed for.
    def brake_motor(self, motor_index:int):
        msg = BRAKE_MOTOR.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the controller for the specified motor.
    # Parameters:
    #   motor_index: The index of the motor to set the controller for.
    #   is_reversed: Whether or not the motor is reversed.
    #   encoder_index: The index of the encoder to use for the controller.
    #   encoder_resolution: Encoder resolution in ticks per revolution. The value can not be negative or zero.
    #   kp: Proportional constant of PID
    #   ki: Integral constant of PID
    #   kd: Derivative constant of PID
    #   integral_limit: Integral limit of PID controller. The value can not be negative or zero. If the value is zero or negative, the integral limit is disabled.
    def initialize_motor_controller(self, motor_index:int, is_reversed:bool, encoder_index:int, encoder_resolution:float, kp:float, ki:float, kd:float, integral_limit:float):
        msg = INITIALIZE_MOTOR_CONTROLLER.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little') + is_reversed.to_bytes(1, 'little') + encoder_index.to_bytes(1, 'little') + bytearray(struct.pack('d', encoder_resolution)) + bytearray(struct.pack('d', kp)) + bytearray(struct.pack('d', ki)) + bytearray(struct.pack('d', kd)) + bytearray(struct.pack('d', integral_limit))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the target speed for the specified motor in radians.
    # Parameters:
    #   motor_index: The index of the motor to set the target velocity for.
    #   speed: The speed of the motor.
    def set_motor_target_speed(self, motor_index:int, speed:float):
        msg = SET_MOTOR_TARGET_SPEED.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little') + bytearray(struct.pack('d', speed))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command resets the controller for the specified motor.
    # Parameters:
    #   motor_index: The index of the motor to reset the controller for.
    def reset_motor_controller(self, motor_index:int):
        msg = RESET_MOTOR_CONTROLLER.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command gets the state of the controller for the specified motor.
    # Parameters:
    #   motor_index: The index of the motor to get the state for.
    def get_motor_controller_state(self, motor_index:int):
        msg = GET_MOTOR_CONTROLLER_STATE.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)
        response_length = MotorControllerState.get_size()
        result =  self.read(response_length)
        return MotorControllerState.decode(result)

    # This command deletes the controller for the specified motor.
    # Parameters:
    #   motor_index: The index of the motor to delete the controller for.
    def delete_motor_controller(self, motor_index:int):
        msg = DELETE_MOTOR_CONTROLLER.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command initializes an encoder and prepares it for use.
    # Parameters:
    #   encoder_index: The index of the encoder to initialize.
    #   encoder_resolution: Encoder resolution in ticks per revolution. The value can not be negative or zero.
    #   is_reversed: Whether or not the encoder is reversed.
    def initialize_encoder(self, encoder_index:int, encoder_resolution:float, is_reversed:bool):
        msg = INITIALIZE_ENCODER.to_bytes(1, 'little') + encoder_index.to_bytes(1, 'little') + bytearray(struct.pack('d', encoder_resolution)) + is_reversed.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command retrieves the current value of the encoder.
    # Parameters:
    #   encoder_index: The index of the encoder to retrieve the value for.
    def get_encoder_value(self, encoder_index:int):
        msg = GET_ENCODER_VALUE.to_bytes(1, 'little') + encoder_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)
        response_length = 2
        result =  self.read(response_length)
        return int.from_bytes(result, 'little')

    # This command starts the odometry calculation for the specified encoder.
    # Parameters:
    #   encoder_index: The index of the encoder to start the odometry calculation for.
    def start_encoder_odometry(self, encoder_index:int):
        msg = START_ENCODER_ODOMETRY.to_bytes(1, 'little') + encoder_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command resets the odometry calculation for the specified encoder.
    # Parameters:
    #   encoder_index: The index of the encoder to reset the odometry calculation for.
    def reset_encoder_odometry(self, encoder_index:int):
        msg = RESET_ENCODER_ODOMETRY.to_bytes(1, 'little') + encoder_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command stops the odometry calculation for the specified encoder.
    # Parameters:
    #   encoder_index: The index of the encoder to stop the odometry calculation for.
    def stop_encoder_odometry(self, encoder_index:int):
        msg = STOP_ENCODER_ODOMETRY.to_bytes(1, 'little') + encoder_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command retrieves the odometry of the specified encoder.
    # Parameters:
    #   encoder_index: The index of the encoder to retrieve the odometry for.
    def get_encoder_odometry(self, encoder_index:int):
        msg = GET_ENCODER_ODOMETRY.to_bytes(1, 'little') + encoder_index.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)
        response_length = 8
        result =  self.read(response_length)
        return struct.unpack('<d', result)[0]

    # This command initializes a digital pin and prepares it for use.
    # Parameters:
    #   pin_number: The number of the pin to initialize.
    #   mode: Set digital pin as input or output. Modes: 0 = INPUT_PULLDOWN, 1 = INPUT_PULLUP, 2 = INPUT_NOPULL, 3 = OUTPUT.
    def initialize_gpio_pin(self, pin_number:int, mode:int):
        msg = INITIALIZE_GPIO_PIN.to_bytes(1, 'little') + pin_number.to_bytes(1, 'little') + mode.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the specified pin to a state.
    # Parameters:
    #   pin_number: The number of the pin to set to a state.
    #   state: The state of the pin. 0 = LOW, 1 = HIGH.
    def set_gpio_pin_state(self, pin_number:int, state:int):
        msg = SET_GPIO_PIN_STATE.to_bytes(1, 'little') + pin_number.to_bytes(1, 'little') + state.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command gets the state of the specified pin.
    # Parameters:
    #   pin_number: The number of the pin to get the state for.
    def get_gpio_pin_state(self, pin_number:int):
        msg = GET_GPIO_PIN_STATE.to_bytes(1, 'little') + pin_number.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)
        response_length = 1
        result =  self.read(response_length)
        return int.from_bytes(result, 'little')

    # This command toggles the specified pin.
    # Parameters:
    #   pin_number: The number of the pin to toggle.
    def toggle_gpio_pin_state(self, pin_number:int):
        msg = TOGGLE_GPIO_PIN_STATE.to_bytes(1, 'little') + pin_number.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the status LED to a state.
    # Parameters:
    #   state: The state of the status LED. 0 = OFF, 1 = ON.
    def set_status_led_state(self, state:int):
        msg = SET_STATUS_LED_STATE.to_bytes(1, 'little') + state.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command toggles the status LED.
    # Parameters:
    def toggle_status_led_state(self):
        msg = TOGGLE_STATUS_LED_STATE.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command initializes a mecanum platform and prepares it for use.
    # Parameters:
    #   is_reversed_0: Determins if motor 0 is reversed.
    #   is_reversed_1: Determins if motor 1 is reversed.
    #   is_reversed_2: Determins if motor 2 is reversed.
    #   is_reversed_3: Determins if motor 3 is reversed.
    #   length: Length of the platform in meters.
    #   width: Width of the platform in meters.
    #   wheels_diameter: Diameter of the robot wheels in meters.
    #   encoder_resolution: Encoder resolution in ticks per revolution. The value can not be negative. If platform does not have encoders, the value should be set to zero.
    def initialize_mecanum_platform(self, is_reversed_0:bool, is_reversed_1:bool, is_reversed_2:bool, is_reversed_3:bool, length:float, width:float, wheels_diameter:float, encoder_resolution:float):
        msg = INITIALIZE_MECANUM_PLATFORM.to_bytes(1, 'little') + is_reversed_0.to_bytes(1, 'little') + is_reversed_1.to_bytes(1, 'little') + is_reversed_2.to_bytes(1, 'little') + is_reversed_3.to_bytes(1, 'little') + bytearray(struct.pack('d', length)) + bytearray(struct.pack('d', width)) + bytearray(struct.pack('d', wheels_diameter)) + bytearray(struct.pack('d', encoder_resolution))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command initializes a omni platform and prepares it for use.
    # Parameters:
    #   is_reversed_0: Determins if motor 0 is reversed.
    #   is_reversed_1: Determins if motor 1 is reversed.
    #   is_reversed_2: Determins if motor 2 is reversed.
    #   wheels_diameter: Diameter of the robot wheels in millimeters.
    #   robot_radius: Distance berween the center of the robot and the center of the wheels in millimeters.
    #   encoder_resolution: Encoder resolution in ticks per revolution. The value can not be negative. If platform does not have encoders, the value should be set to zero.
    def initialize_omni_platform(self, is_reversed_0:bool, is_reversed_1:bool, is_reversed_2:bool, wheels_diameter:float, robot_radius:float, encoder_resolution:float):
        msg = INITIALIZE_OMNI_PLATFORM.to_bytes(1, 'little') + is_reversed_0.to_bytes(1, 'little') + is_reversed_1.to_bytes(1, 'little') + is_reversed_2.to_bytes(1, 'little') + bytearray(struct.pack('d', wheels_diameter)) + bytearray(struct.pack('d', robot_radius)) + bytearray(struct.pack('d', encoder_resolution))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the velocity for the platform in PWM.
    # Parameters:
    #   x: X component of platform velocity in PWM
    #   y: Y component of platform velocity in PWM
    #   t: Theta component of platform velocity in PWM
    def set_platform_velocity(self, x:float, y:float, t:float):
        msg = SET_PLATFORM_VELOCITY.to_bytes(1, 'little') + bytearray(struct.pack('d', x)) + bytearray(struct.pack('d', y)) + bytearray(struct.pack('d', t))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the controller for the platform.
    # Parameters:
    #   kp: Proportional constant of PID
    #   ki: Integral constant of PID
    #   kd: Derivative constant of PID
    #   integral_limit: Integral limit of PID controller. The value can not be negative or zero. If the value is zero or negative, the integral limit is disabled.
    def start_platform_controller(self, kp:float, ki:float, kd:float, integral_limit:float):
        msg = START_PLATFORM_CONTROLLER.to_bytes(1, 'little') + bytearray(struct.pack('d', kp)) + bytearray(struct.pack('d', ki)) + bytearray(struct.pack('d', kd)) + bytearray(struct.pack('d', integral_limit))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command set the target velocity for the platform in meters per second.
    # Parameters:
    #   x: X component of platform velocity in meters per second
    #   y: Y component of platform velocity in meters per second
    #   t: Theta component of platform velocity in radians per second
    def set_platform_target_velocity(self, x:float, y:float, t:float):
        msg = SET_PLATFORM_TARGET_VELOCITY.to_bytes(1, 'little') + bytearray(struct.pack('d', x)) + bytearray(struct.pack('d', y)) + bytearray(struct.pack('d', t))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command gets the current velocity of the platform in meters per second.
    # Parameters:
    def get_platform_current_velocity(self):
        msg = GET_PLATFORM_CURRENT_VELOCITY.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)
        response_length = PlatformVelocity.get_size()
        result =  self.read(response_length)
        return PlatformVelocity.decode(result)

    # This command stops the controller for the platform.
    # Parameters:
    def stop_platform_controller(self):
        msg = STOP_PLATFORM_CONTROLLER.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command starts the odometry calculation for the platform.
    # Parameters:
    def start_platform_odometry(self):
        msg = START_PLATFORM_ODOMETRY.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command resets the odometry calculation for the platform.
    # Parameters:
    def reset_platform_odometry(self):
        msg = RESET_PLATFORM_ODOMETRY.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command stops the odometry calculation for the platform.
    # Parameters:
    def stop_platform_odometry(self):
        msg = STOP_PLATFORM_ODOMETRY.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command retrieves the odometry of the platform in meters and radians.
    # Parameters:
    def get_platform_odometry(self):
        msg = GET_PLATFORM_ODOMETRY.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)
        response_length = PlatformOdometry.get_size()
        result =  self.read(response_length)
        return PlatformOdometry.decode(result)

