# Filename: KinisiController.py
# Description: Kinisi controller class. This class implements serial communication with the Kinisi controller.
# and adds some additional functionality to autogenerated api.

import serial
from .KinisiCommands import *

SpeedResolution = 840

class MotorIndex:
    Motor0 = 0
    Motor1 = 1
    Motor2 = 2
    Motor3 = 3

class EncoderIndex:
    Encoder0 = 0
    Encoder1 = 1
    Encoder2 = 2
    Encoder3 = 3

class GPIOIndex:
    GPIO0 = 0
    GPIO1 = 1
    GPIO2 = 2
    GPIO3 = 3
    GPIO4 = 4
    GPIO5 = 5
    GPIO6 = 6
    GPIO7 = 7
    GPIO8 = 8
    GPIO9 = 9

class GPIOMode:
    INPUT = 0
    INPUT_PULLUP = 1
    INPUT_NOPULL = 2
    OUTPUT = 3

class State:
    LOW = 0
    HIGH = 1

class KinisiController(KinisiCommands):
    def connect(self, port):
        try:
            self.serial =  serial.Serial(port, 115200, timeout = 1)
            return True
        except serial.SerialException:
            return False
        except:
            return False

    def write(self, msg: bytearray):
        self.serial.write(msg)

    def read(self, length: int) -> bytearray:
        return self.serial.read(length)
    
    def set_motor_speed(self, motorIndex: MotorIndex, direction: bool, speed: int):
        speed = int(speed * SpeedResolution / 100)
        direction = int(direction)
        return super().set_motor_speed(motorIndex, direction, speed)
    
    def set_motor_target_velocity(self, motorIndex:MotorIndex, direction:int, speed:int):
        speed = int(speed * SpeedResolution / 100)
        direction = int(direction)
        return super().set_motor_target_velocity(motorIndex, direction, speed)

    # Motors
    def initialoze_motor_all(self, is_reversed:bool = False):
        self.initialize_motor(MotorIndex.Motor0, is_reversed)
        self.initialize_motor(MotorIndex.Motor1, is_reversed)
        self.initialize_motor(MotorIndex.Motor2, is_reversed)
        self.initialize_motor(MotorIndex.Motor3, is_reversed)

    def stop_motor(self, index):
        self.set_motor_speed(index, True, 0)

    def stop_motor_all(self):
        self.stop_motor(MotorIndex.Motor0)
        self.stop_motor(MotorIndex.Motor1)
        self.stop_motor(MotorIndex.Motor2)
        self.stop_motor(MotorIndex.Motor3)

    def __del__(self):
        if self.serial:
            self.serial.close()