# ----------------------------------------------------------------------------
# Kinisi motr controller commands.
# This file is auto generated by the commands generator from JSON file.
# Do not edit this file manually.
# Timestamp: 2024-01-15 11:54:00
# Version: 1.0.2
# ----------------------------------------------------------------------------

import struct

INITIALIZE_MOTOR = 0x01
SET_MOTOR_SPEED = 0x02
STOP_MOTOR = 0x03
BRAKE_MOTOR = 0x04
INITIALIZE_MOTOR_CONTROLLER = 0x05
SET_MOTOR_TARGET_VELOCITY = 0x06
DELETE_MOTOR_CONTROLLER = 0x07
INITIALIZE_ENCODER = 0x11
GET_ENCODER_VALUE = 0x12
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_INPUT = 0x40
SET_PLATFORM_CONTROLLER = 0x41


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.
    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.
    def set_motor_speed(self, motor_index:int, direction:int, speed:int):
        msg = SET_MOTOR_SPEED.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little') + direction.to_bytes(1, 'little') + speed.to_bytes(2, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command stops motor by setting its speed to 0.
    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.
    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.
    def initialize_motor_controller(self, motor_index:int, kp:float, ki:float, kd:float):
        msg = INITIALIZE_MOTOR_CONTROLLER.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little') + bytearray(struct.pack('d', kp)) + bytearray(struct.pack('d', ki)) + bytearray(struct.pack('d', kd))
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the target velocity for the specified motor.
    def set_motor_target_velocity(self, motor_index:int, direction:int, speed:int):
        msg = SET_MOTOR_TARGET_VELOCITY.to_bytes(1, 'little') + motor_index.to_bytes(1, 'little') + direction.to_bytes(1, 'little') + speed.to_bytes(2, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command deletes the controller for the specified motor.
    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.
    def initialize_encoder(self, encoder_index:int):
        msg = INITIALIZE_ENCODER.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 current value of the encoder.
    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)
        result =  self.read(4)
        return int.from_bytes(result, 'little')

    # This command initializes a digital pin and prepares it for use.
    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.
    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.
    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)
        result =  self.read(1)
        return int.from_bytes(result, 'little')

    # This command toggles the specified pin.
    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.
    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.
    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.
    def initialize_mecanum_platform(self, is_reversed_0:bool, is_reversed_1:bool, is_reversed_2:bool, is_reversed_3:bool):
        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')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command initializes a omni platform and prepares it for use.
    def initialize_omni_platform(self, is_reversed_0:bool, is_reversed_1:bool, is_reversed_2:bool, wheels_diameter:int, robot_radius:int):
        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') + wheels_diameter.to_bytes(2, 'little') + robot_radius.to_bytes(2, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the velocity input for the platform.
    def set_platform_velocity_input(self, x:int, y:int, t:int):
        msg = SET_PLATFORM_VELOCITY_INPUT.to_bytes(1, 'little') + x.to_bytes(1, 'little', signed=True) + y.to_bytes(1, 'little', signed=True) + t.to_bytes(1, 'little', signed=True)
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

    # This command sets the controller for the platform.
    def set_platform_controller(self):
        msg = SET_PLATFORM_CONTROLLER.to_bytes(1, 'little')
        length = len(msg)
        msg = length.to_bytes(1, 'little') + msg
        self.write(msg)

