import serial
import sys
import time
import numpy as np
import math
from raex.device.challenge import AbstractChallengeWrapper, DummyChallengeWrapper


"""
Contact Technical Questions JVL.dk
Christian Peitzmeyer 02942 7998244
christian.peitzmeyer@jvl.dk
Manual: https://www.jvl.dk/files/pdf-1/user%20manuals/lb0058gb.pdf#G4.1557055
"""


class JvlControl(AbstractChallengeWrapper):
    def __init__(self, **kwargs):
        """
        Initialize motors. Example call:
        jvlcontrol_options = {'com_port': 'COM6',
                              'name': "Motors",
                              'move_shortest': False,
                              'motor_addresses': [1,2,3,4],
                              'home': [False] * 4,
                              'max_velocity': 3000,
                              'acceleration': 5000,
                              'deceleration': 5000}
        motors = JvlControl(**jvlcontrol_options)

        All connected motors are addressable under the same com_port, distinguishable by their addresses
        :param kwargs: com_port, name (some name), move_shortest (if rotation should take place in 360°),
                       motor_addresses (addresses of motors connected via BUS), max_velocity, acceleration,
                       deceleration, home (motors to home when init)
        :type kwargs: com_port: string, name: string, move_shortest: boolean, motor_addresses: list,
                      max_velocity: int, acceleration: int, deceleration: int, home: list of bool
        """
        super().__init__(**kwargs)
        self.ser = None
        self.sleep_after_write = 0.05
        self.open_serial_connection(kwargs["com_port"])
        self.motor_addresses = kwargs["motor_addresses"]
        self.name = kwargs["name"]

        self.max_velocity = kwargs["max_velocity"]
        self.acceleration = kwargs["acceleration"]
        self.deceleration = kwargs["deceleration"]
        self.set_current_mode(255, "position")

        self.force_acknowledgement = False
        self.shortest = kwargs["move_shortest"]
        self.to_home = kwargs["home"]
        self.init_homing()

    def __del__(self):
        self.close_serial_connection()

    def tohex(self, val: int) -> str:
        """
        Takes an integer and converts it into a 32 bit hex string. For example (10)_base10 -> (0000000a)_base16
        :param val: input value to convert, max val 0xFFFFFFFF
        :type val: integer
        :return: 32 bit hex string
        :rtype: hex string
        """
        nbits = 32
        if val < 0:
            return self.tohex2scomplement(abs(val))
        if val < -2**31 or val > 2**31-1:
            raise Exception(f"Input: {val} out of 32 bit range: [-2^31,2^31-1].")
        tmp = hex((val + (1 << nbits)) % (1 << nbits))
        return "{0:0>8}".format(tmp[2:])

    def tohex2scomplement(self, val: int) -> str:
        """
        Takes an integer and converts it into its two's complement hex representation. For example (10)_base10 -> (f6)_base16
        :param val: input value to convert, either positive or negative
        :type val: integer
        :return: hex string
        """
        binary_string = "{0:032b}".format(val)
        twos_complement = self.findTwoscomplement(binary_string)
        integer = int(twos_complement, 2)
        hexadecimal = hex(integer)
        # verify = self.twos_complement(hexadecimal, 32)
        return hexadecimal[2:]

    @staticmethod
    def findTwoscomplement(string: str) -> str:
        """
        Calculates the two's complement of an hex string
        :param string: value to convert
        :type string: string
        :return: two's complement as string
        """
        n = len(string)
        i = n - 1
        while (i >= 0):
            if (string[i] == '1'):
                break
            i -= 1
        if (i == -1):
            return '1' + string
        k = i - 1
        while (k >= 0):
            if (string[k] == '1'):
                string = list(string)
                string[k] = '0'
                string = ''.join(string)
            else:
                string = list(string)
                string[k] = '1'
                string = ''.join(string)
            k -= 1
        return string

    @staticmethod
    def twos_complement(hexstr, bits):
        value = int(hexstr, 16)
        if value & (1 << (bits - 1)):
            value -= 1 << bits
        return value

    @staticmethod
    def toint(val: str) -> int:
        """
        Takes a 32 bit hex value and converts it into an integer. For example (0000000a)_base16 -> (10)_base10
        :param val: input string to convert, max value "FFFFFFFF"
        :type val: hex string
        :return: conversion
        :rtype: integer
        """
        if len(val) > 8:
            raise Exception(f"Input: {val} out of 32 bit range: \"0xFFFFFFFF\".")
        tmp = bytes.fromhex(val)
        return int.from_bytes(tmp, byteorder='big', signed=True)

    def encode_address(self, address: int) -> str:
        """
        Takes an address and returns it as string, together with its one's complement. For example 1 -> 01 FE
        :param address: address to convert, max value 255
        :type address: integer
        :return: hex(address) and one's complement(address)
        :rtype: byte string
        """
        if address > 255 or address < 0:
            raise Exception(f"Address value {address} out of range of allowed addresses [0,255].")
        if address != 255 and address not in self.motor_addresses:
            raise Exception(f"Motor address {address} not valid: {self.motor_addresses}.")
        return "{0:0>2} ".format(hex(address)[2:]) + "{0:0>2} ".format(hex(address ^ 0xff)[2:])

    @staticmethod
    def command_to_bytes(command: str) -> bytes:
        """
        Takes the whole command string and encodes it as bytes
        :param command: command to encode, example command: "52 52 52 01 fe 03 fc 04 fb 00 ff 00 ff 00 ff 00 ff aa aa"
        :type command: byte string
        :return: command as byte
        :rtype: byte
        """
        command = command.split(" ")
        command = [int(i, 16) for i in command]
        return bytes(command)

    def get_response(self, bytes_to_read: int) -> str:
        """
        Get response from motor (serial connection) after a command was send.
        :param bytes_to_read: number of bytes to read from serial connection
        :type bytes_to_read: integer
        :return: response from motor
        :rtype: hex string
        """
        response = ""
        for i in range(bytes_to_read):
            byte = self.ser.read()
            response += byte.hex()
        return response

    def get_acknowledgement(self) -> str:
        """
        Checks if command reached the motors successfully. Motors will return "111111" if true.
        :return: "ACK" or "F" string
        """
        response = self.get_response(3)
        if response == "111111":
            return "ACK"
        else:
            return "F"

    def encode_write(self, address: int, reg: str, value: int) -> bytes:
        """
        Builds a string command with a given value, depending on which motor and register should be addressed
        :param address: motor that should be addressed
        :type address: integer
        :param reg: register to be written (see manual, 8.2 Internal Registers)
        :type reg: string
        :param value: value for that register
        :type value: int
        :return: command as string, for example: "52 52 52 01 fe 03 fc 04 fb 00 ff 00 ff 00 ff 00 ff aa aa"
        :rtype: byte string
        """
        val = self.tohex(value)
        if reg == "P_SOLL":
            command_1 = "52 52 52 " + self.encode_address(address) + "03 fc 04 fb "
        elif reg == "V_SOLL":
            command_1 = "52 52 52 " + self.encode_address(address) + "05 fa 04 fb "
        elif reg == "A_SOLL":
            command_1 = "52 52 52 " + self.encode_address(address) + "06 f9 04 fb "
        elif reg == "D_SOLL":
            command_1 = "52 52 52 " + self.encode_address(address) + "ae 51 04 fb "
        elif reg == "AC_WIN":
            command_1 = "52 52 52 " + self.encode_address(address) + "21 de 04 fb "
        elif reg == "AC_ATP":
            command_1 = "52 52 52 " + self.encode_address(address) + "22 dd 04 fb "
        elif reg == "S_TIME":
            command_1 = "52 52 52 " + self.encode_address(address) + "6e 91 04 fb "
        elif reg == "CMMND":
            command_1 = "52 52 52 " + self.encode_address(address) + "18 e7 04 fb "
        elif reg == "SETUP":
            command_1 = "52 52 52 " + self.encode_address(address) + "7c 83 04 fb "
        elif reg == "ERROR":
            command_1 = "52 52 52 " + self.encode_address(address) + "23 dc 04 fb "
        elif reg == "BAUDR":
            command_1 = "52 52 52 " + self.encode_address(address) + "92 6d 04 fb "
        elif reg == "M_ADR":
            command_1 = "52 52 52 " + self.encode_address(address) + "96 69 04 fb "
        else:
            raise Exception(f"Register parameter: {reg} does not exist.")
        command_2 = val[6:8] + " {0:0>2} ".format(hex(int(val[6:8], 16) ^ 0xff)[2:]) + \
                    val[4:6] + " {0:0>2} ".format(hex(int(val[4:6], 16) ^ 0xff)[2:]) + \
                    val[2:4] + " {0:0>2} ".format(hex(int(val[2:4], 16) ^ 0xff)[2:]) + \
                    val[0:2] + " {0:0>2} ".format(hex(int(val[0:2], 16) ^ 0xff)[2:])
        command_3 = "aa aa"
        command = self.command_to_bytes(command_1 + command_2 + command_3)
        return command

    def open_serial_connection(self, serial_port) -> str:
        """
        Opens the serial connection to connect to the motors
        :param serial_port: Serial port to connect to e.g. "COM7"
        :return: string if okay
        """
        self.ser = serial.Serial(serial_port, baudrate=19200, timeout=0.075, parity=serial.PARITY_NONE,
                                 bytesize=serial.EIGHTBITS, stopbits=serial.STOPBITS_ONE, xonxoff=False)
        return "Opened"

    def close_serial_connection(self) -> str:
        """
        Closes the serial connection to the motors
        :return: string if okay
        """
        self.ser.close()
        return "Closed"

    def get_startup_mode(self, address: int) -> str:
        """
        Returns the startup mode of a motor
        :param address: motor to get the startup mode from
        :type address: integer
        :return: startup mode as hex
        :rtype: string
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "25 da aa aa"
        self.ser.write(self.command_to_bytes(command))
        return self.get_response(19)[18:20]

    def set_startup_mode(self, address: int, mode: str):
        """
        Sets the startup mode of a motor to the given mode state, startup mode is the mode the motor is entering after power up
        :param address: motor to get startup mode from
        :type address: integer
        :param mode: the mode the motor should enter when powering up
        :type mode: string
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        if mode == "passive":
            command = "52 52 52 " + self.encode_address(address) + "25 da 02 fd 00 ff 00 ff aa aa"
        elif mode == "position":
            command = "52 52 52 " + self.encode_address(address) + "25 da 02 fd 02 fd 00 ff aa aa"
        elif mode == "stop":
            command = "52 52 52 " + self.encode_address(address) + "25 da 02 fd 0b f4 00 ff aa aa"
        else:
            raise Exception(f"Mode parameter {mode} does not exist.")
        self.ser.write(self.command_to_bytes(command))

        if self.get_acknowledgement() == "F": self.set_startup_mode(address, mode)

    def get_current_mode(self, address: int) -> str:
        """
        Returns the current mode of a motor. For example:
        - Mode 0: 00 ff 00 ff (Passive)
        - Mode 2: 02 fd 00 ff (Position)
        - Mode 11: 0b f4 00 ff (Stop)
        :param address: motor to get the current mode from
        :type address: integer
        :return: current mode as hex
        :rtype: string
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "02 fd aa aa"
        self.ser.write(self.command_to_bytes(command))
        return self.get_response(19)[18:20]

    def set_current_mode(self, address: int, mode: str):
        """
        Sets the current mode of a motor to the given mode state, current mode is the mode the motor is currently in
        :param address: motor to set current mode to
        :type address: integer
        :param mode: the mode the motor should enter for operation
        :type mode: string
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        if mode == "passive":
            command = "52 52 52 " + self.encode_address(address) + "02 fd 02 fd 00 ff 00 ff aa aa"
        elif mode == "position":
            command = "52 52 52 " + self.encode_address(address) + "02 fd 02 fd 02 fd 00 ff aa aa"
        elif mode == "stop":
            command = "52 52 52 " + self.encode_address(address) + "02 fd 02 fd 0b f4 00 ff aa aa"
        else:
            raise Exception(f"Mode parameter {mode} does not exist.")
        self.ser.write(self.command_to_bytes(command))

        if self.get_acknowledgement() == "F": self.set_current_mode(address, mode)

    def get_max_velocity(self, address: int) -> int:
        """
        Returns the current velocity setting of the motor
        :param address: motor to get current velocity setting from
        :type address: integer
        :return: velocity
        :rtype: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "05 fa aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20]) / 100)

    def set_max_velocity(self, address: int, velocity: int):
        """
        Sets the maximum possible velocity of a motor to the given value.
        Allowed value range is -3000 - 3000
        :param address: motor to set velocity to
        :type address: integer
        :param velocity: the velocity the motor should be set to
        :type velocity: integer
        """
        # -3000 - 3000 RPM
        self.ser.flushInput()
        self.ser.flushOutput()
        if velocity <= -3000 or velocity > 3000:
            raise Exception(f"Velocity value {velocity} out of range: [-3000,3000].")
        command = self.encode_write(address, "V_SOLL", velocity*100)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_max_velocity(address, velocity)

    def get_acceleration(self, address: int) -> int:
        """
        Returns the current acceleration setting of the motor
        :param address: motor to get current acceleration setting from
        :type address: integer
        :return: acceleration
        :rtype: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "06 f9 aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20]))

    def set_acceleration(self, address: int, acceleration: int):
        """
        Sets the acceleration of a motor to the given value.
        Allowed value range is 0 - 5000
        :param address: motor to set velocity to
        :type address: integer
        :param acceleration: the acceleration the motor should be set to
        :type acceleration: integer
        """
        # 0 - 5000
        self.ser.flushInput()
        self.ser.flushOutput()
        if 0 <= acceleration > 5000:
            raise Exception(f"Accerleration value {acceleration} out of range: [0,5000].")
        command = self.encode_write(address, "A_SOLL", acceleration)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_acceleration(address, acceleration)

    def get_deceleration(self, address: int):
        """
        Returns the current deceleration setting of the motor
        :param address: motor to get current deceleration setting from
        :type address: integer
        :return: deceleration
        :rtype: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "ae 51 aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20]))

    def set_deceleration(self, address: int, deceleration: int):
        """
        Sets the deceleration of a motor to the given value.
        Allowed value range is 0 - 5000, if = 0: acceleration is used for deceleration
        :param address: motor to set velocity to
        :type address: integer
        :param deceleration: the deceleration the motor should be set to
        :type deceleration: integer
        """
        # 0 - 5.000 if = 0 -> acceleration is used for deceleration
        self.ser.flushInput()
        self.ser.flushOutput()
        if 0 <= deceleration > 5000:
            raise Exception(f"Decerleration value {deceleration} out of range: [0,5000].")
        command = self.encode_write(address, "D_SOLL", deceleration)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_deceleration(address, deceleration)

    def get_current_position(self, address: int) -> list:
        """
        Returns the current position of either one motor or all motors of the system
        :param address: motor to get current position from. 0-254 for one motor, 255 for all motors
        :type address: integer
        :return: position as list of integers
        :rtype: list
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        if address == 255:
            position_ist = [0xffffffff] * len(self.motor_addresses)
            motor = 0
            for address in self.motor_addresses:
                command = "50 50 50 " + self.encode_address(address) + "0a f5 aa aa"
                self.ser.write(self.command_to_bytes(command))
                response = self.get_response(19)
                position_ist[motor] = self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20])
                motor += 1
        else:
            command = "50 50 50 " + self.encode_address(address) + "0a f5 aa aa"
            self.ser.write(self.command_to_bytes(command))
            # print(f'time send<->received: {time.time() - start}')
            position_ist = 0xffffffff
            response = self.get_response(19)
            position_ist = self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20])
        return position_ist

    def move_to_position(self, address: int, position: int):
        """
        Requested position a motor should move to.
        Valid value range is from -2^31-1 to 2^31 (-2147483649 to 2147483648)
        :param address: motor to position to
        :type address: integer
        :param position: the requested motor position
        :type deceleration: integer
        """
        # range from -2^31-1 to 2^31 (-2147483649 to 2147483648)
        self.ser.flushInput()
        self.ser.flushOutput()
        if position < -2147483649 or position > 2147483648:
            raise Exception("Position out of Interger Range")
        command = self.encode_write(address, "P_SOLL", position)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.move_to_position(address, position)

    def move_shortest_path(self, address: int, position: int):
        """
        Requested position a motor should move to. Here, the motor move within 360 degree choosing the
        shortest path to reach the requested position itself
        :param address: motor to position to
        :type address: integer
        :param position: the requested motor position
        :type deceleration: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        origin = self.get_current_position(address)

        if origin > position:
            rawDiff = origin - position
        else:
            rawDiff = position - origin
        modDiff = rawDiff % 409600
        if (modDiff > 409600 / 2):
            signedDiff = 409600 - modDiff
            if (position > origin):
                signedDiff = signedDiff * (-1)
        else:
            signedDiff = modDiff
            if (origin > position):
                signedDiff = signedDiff * (-1)

        command = self.encode_write(address, "P_SOLL", origin + signedDiff)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.move_shortest_path(address, position)

    def set_challenge(self, challenge):
        """
        Setting a challenge to all motors used within the system (controlled via self.motor_addresses)
        :param challenge: positions the motors should move to
        :type challenge: list of integers
        :return: 'OK' if positions are reached successfully
        :rtype: string
        """
        motor = 0
        if self.shortest == True:
            for address in self.motor_addresses:
                self.move_shortest_path(address, challenge[motor])
                motor += 1
            while True:
                positions = [self.get_current_position(255)[i] % 409600 for i in range(len(self.motor_addresses))]
                # print(positions)
                if positions == challenge:
                    break
        else:
            for address in self.motor_addresses:
                self.move_to_position(address, challenge[motor])
                motor += 1
            while True:
                positions = self.get_current_position(255)
                # print(positions)
                if positions == challenge:
                    break
        return 'OK'

    def create_random_challenges(self, number_of_challenges, steps_circle=None, rng=None, **kwargs):
        """
        Creates random challenge positions for the number of motors used within the setup
        :param number_of_challenges: number of challenges to generate
        :type number_of_challenges: integer
        :param steps_circle: the resolution of the challenges, e.g. 360 would create the motor postions
        in such way that each position equals a 1 degree step
        :type steps_circle: integer
        :param rng: which random number generator to use
        :param kwargs: unused
        :return: challenge positions for each motor
        :rtype: list of list of integers
        """
        if rng is None:
            rng = np.random.default_rng()
        if steps_circle != None:
            step_positions = [int(i) for i in np.linspace(0,409600, num=steps_circle, endpoint=True)]
            return rng.choice(step_positions, (number_of_challenges, len(self.motor_addresses))).tolist()
        else:
            return rng.integers(0, 409600, size=(number_of_challenges, len(self.motor_addresses))).tolist()

    def init_homing(self):
        """
        Homes all motors of the system to the zero position
        """
        store_shortest = self.shortest
        self.shortest = False

        self.set_max_velocity(255, 1000)
        self.set_acceleration(255, 500)
        self.set_deceleration(255, 500)
        motor = 0
        for home in self.to_home:
            if home:
                self.to_home[motor] = 0
            else:
                self.to_home[motor] = self.get_current_position(self.motor_addresses[motor])
            motor += 1
        self.set_challenge(self.to_home)

        self.set_max_velocity(255, self.max_velocity)
        self.set_acceleration(255, self.acceleration)
        self.set_deceleration(255, self.deceleration)

        self.shortest = store_shortest

    def get_autocorrection_window(self, address: int) -> int:
        """
        Returns the current autocorrection window setting of the motor aka the error tolerance in steps
        where no position correction is done
        :param address: motor to get current autocorrection window setting from
        :type address: integer
        :return: auto correction window
        :rtype: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "21 de aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20]))

    def set_autocorrection_window(self, address: int, window: int):
        """
        Sets the autocorrection window of a motor to the given value. Allowed value range is 0 - 2^31-1
        :param address: motor to set autocorrection window to
        :type address: integer
        :param window: the autocorrection window the motor should be set to
        :type window: integer
        """
        # in range 0 - 2^31-1
        self.ser.flushInput()
        self.ser.flushOutput()
        if window < 0 or window > (2**31-1):
            raise Exception("Autocorrection Value Out of Range, 0 to 2**31-1")
        command = self.encode_write(address, "AC_WIN", window)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_autocorrection_window(address, window)

    def get_autocorrection_attempts(self, address: int) -> int:
        """
        Returns the current autocorrection attempts setting of the motor aka how often the motor tries to
        reach a position if the autocorrection windows is exceeded
        :param address: motor to get current autocorrection attempts setting from
        :type address: integer
        :return: autocorrection attempts
        :rtype: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "22 dd aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20]))

    def set_autocorrection_attempts(self, address: int, attemps: int):
        """
        Sets the autocorrection attempts of a motor to the given value. Allowed value range is 0 - 100
        :param address: motor to set autocorrection attempts to
        :type address: integer
        :param attemps: the autocorrection attempts the motor should be set to
        :type attemps: integer
        """
        # in range 0 - 100
        self.ser.flushInput()
        self.ser.flushOutput()
        if attemps < 0 or attemps > 100:
            raise Exception("Attempts Value Out of Range, 0 to 100")
        command = self.encode_write(address, "AC_ATP", attemps)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_autocorrection_attempts(address, attemps)

    def get_settling_time(self, address: int) -> int:
        """
        Returns the current settling time setting of the motor aka long the motor will wait until a new
        autocorrection attempt is performed
        :param address: motor to get current settling time setting from
        :type address: integer
        :return: settling time
        :rtype: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "6e 91 aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20]))

    def set_settling_time(self, address: int, milliseconds: int):
        """
        Sets the settling time of a motor to the given value. Allowed value range is 0 - 32676 ms
        :param address: motor to set settling time to
        :type address: integer
        :param milliseconds: the settling time the motor should be set to
        :type milliseconds: integer
        """
        # in range 0 - 32676 ms
        self.ser.flushInput()
        self.ser.flushOutput()
        if milliseconds < 0 or milliseconds > 32676:
            raise Exception("Milliseconds Value Out of Range, 0 to 32676")
        command = self.encode_write(address, "S_TIME", milliseconds)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_settling_time(address, milliseconds)

    def save_in_flash_reset(self, address: int):
        """
        Saves the current motor settings in the flash memory of the specified motor to retain them after a power down
        Note, that uncontrolled loops can exceed the max R/W operations for the flash memory and therefore damage the memory
        The motor will restart
        :param address: motor to reconfigure flash
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = self.encode_write(address, "CMMND", 268)
        self.ser.write(command)
        self.__del__()

    def get_setup_bits(self, address: int) -> str:
        """
        Returns all user command bits
        :param address: motor to get current user command bits from
        :type address: integer
        :return: user command bits
        :rtype: string
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "7c 83 aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return "{0:b}".format(int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20])))

    def get_setup_bit(self, address: int, bit_position: int) -> int:
        """
        Returns one user command bit, valid bit range 0 - 31
        :param address: motor to get user command bit from
        :type address: integer
        :param bit_position: bit position of register to read
        :type bit_position: integer
        :return: user command bit
        :rtype: integer
        """
        if bit_position < 0 or bit_position >= 32:
            raise Exception(
                f"Bit postion {bit_position} not valid: valid range is 0 - 31")
        bits = self.get_setup_bits(address)
        return int(bits[bit_position])

    def set_setup_bit(self, address: int, bit_position: int):
        """
        Sets a bit of the user command register of a motor. Allowed bit value range is 0 - 31.
        If bit is 0 and gets set => 1, if bit is 1 and gets set => 0
        :param address: motor to set autocorrection attempts to
        :type address: integer
        :param bit_position: bit position of register to set
        :type bit_position: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        if bit_position < 0 or bit_position >= 32:
            raise Exception(
                f"Bit postion {bit_position} not valid: valid range is 0 - 31")
        command = self.encode_write(address, "SETUP", int(self.get_setup_bits(address)) ^ (1 << bit_position))
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_setup_bit(address, bit_position)

    def get_error_bits(self, address: int) -> (str, list):
        """
        Returns all user command bits
        :param address: motor to get current error bits from
        :type address: integer
        :return: error bits, none-zero bits
        :rtype: string, list of integers
        """
        # Bit 0: General error(always set together with another error bit)
        # Bit 1: Follow error
        # Bit 2: Output driver
        # Bit 3: Position Limit
        # Bit 4: Low bus voltage
        # Bit 5: Over voltage
        # Bit 6: Temperature > 90 °C
        # Bit 7: Internal(Self diagnostics failed)
        # Bit 8: Absolute multiturn encoder lost position
        # Bit 9: Absolute multiturn encoder sensor counting
        # Bit 10: No communication to absolute multiturn encoder
        # Bit 11: SSI encoder counting
        # Bit 12: Closed loop
        # Bit 13: External memory
        # Bit 14: Absolute single turn encoder
        # Bit 16: Zero search timeout
        # Bit 17: CVI unstable
        # Bit 18: Motor driver overload
        # Bit 27: Safe Torque Off(STO)

        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "23 dc aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        response = "{0:b}".format(int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20])))
        bits_set = []
        for bit in range(len(response)):
            if response[bit] == '1':
                bits_set.append(bit)
        return response, bits_set

    def get_error_bit(self, address: int, bit_position: int) -> int:
        """
        Returns one error bit, valid bit range 0 - 31
        :param address: motor to get error bit from
        :type address: integer
        :param bit_position: bit position of register to read
        :type bit_position: integer
        :return: error bit
        :rtype: integer
        """
        if bit_position < 0 or bit_position >= 32:
            raise Exception(
                f"Bit postion {bit_position} not valid: valid range is 0 - 31")
        bits = self.get_error_bits(address)[0]
        return int(bits[bit_position])

    def clear_errors(self, address: int):
        """
        Clear the whole error register aka clear all errors from a motor
        :param address: motor address to clear errors from
        :type address: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = self.encode_write(address, "CMMND", 354)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.clear_errors(address)
        command = self.encode_write(address, "ERROR", 0)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.clear_errors(address)

    def get_baud_rate(self, address: int) -> int:
        """
        Returns the current baud rate setting of the motor
        :param address: motor to get current baud rate setting from
        :type address: integer
        :return: baud rate
        :rtype: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "92 6d aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20]))

    def set_baud_rate(self, address: int, baud_rate: int):
        """
        Sets the baud rate of a motor to the given value. Allowed value range is 0 - 7
        :param address: motor to set settling time to
        :type address: integer
        :param baud_rate: the baud rate the motor should be set to
        :type baud_rate: integer
        """
        # 0: 9600
        # 1: 19200 (default)
        # 2: 38400
        # 3: 57600
        # 4: 115200
        # 5: 230400
        # 6: 460800
        # 7: 921600
        self.ser.flushInput()
        self.ser.flushOutput()
        if baud_rate not in range(0, 8):
            raise Exception(f"Baud rate {baud_rate} not valid: 0: 9600, 1: 19200, 2: 38400, 3: 57600, 4: 115200, 5: 230400, 6: 460800, 7: 921600")
        command = self.encode_write(address, "BAUDR", baud_rate)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_baud_rate(address, baud_rate)

    def get_motor_address(self, address: int):
        """
        Returns the motor address of the motor
        :param address: motor to get motor address from
        :type address: integer
        :return: motor address
        :rtype: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        command = "50 50 50 " + self.encode_address(address) + "96 69 aa aa"
        self.ser.write(self.command_to_bytes(command))
        response = self.get_response(19)
        return int(self.toint(response[30:32] + response[26:28] + response[22:24] + response[18:20]))

    def set_motor_address(self, address, new_address):
        """
        Sets the address of a motor to the given value. Allowed value range is 0 - 254
        :param address: motor to set settling time to
        :type address: integer
        :param new_address: the new address the motor should be set to
        :type new_address: integer
        """
        self.ser.flushInput()
        self.ser.flushOutput()
        if address == 255 or new_address == 255:
            raise Exception("Broadcast not possible.")
        if new_address < 0 or new_address >= 255:
            raise Exception(
                f"New address {new_address} not valid: valid range is 0 - 254")
        command = self.encode_write(address, "M_ADR", new_address)
        self.ser.write(command)
        if self.get_acknowledgement() == "F": self.set_motor_address(address, new_address)

    ### not evaluated ###
    # def stop_motor(self, address: int) -> int:
    #     command = b'\x61\x61\x61\xff\x00\x01\xfe\x02\xfd\xaa\xaa' #stop
    #     self.ser.write(command)
    #     command = b'\x52\x52\x52\xff\x00\x02\xfd\x02\xfd\x00\xff\x00\xff\xaa\xaa' #stop
    #     self.ser.write(command)
    #     return 0