import time
from .NanotecBase import NanotecBase
from eip.client import EIPBaseClient


class NanotecC5E11UDP(NanotecBase):
    def __init__(
        self,
        deviceIpAddress: str,
        originatorUdpPort: int = 2222,
        lockUdpPort: bool = True,
        inputId: int = 0x68,
        inputSize: int = 8,
        inputRPI: int = 10000,
        outputId: int = 0x69,
        outputSize: int = 8,
        outputRPI: int = 10000,
    ):
        super().__init__(deviceIpAddress)
        try:
            self.eeipClient = EIPBaseClient(originator_udp_port=originatorUdpPort, lock_udp_port=lockUdpPort)
            self.eeipClient.list_identity()
            self.eeipClient.register_session(self.deviceIpAddress)
        except Exception as e:
            raise Exception(f"UDP connection failed: {e}")

        self.eeipClient.output_assembly.output_assembly = outputId
        self.eeipClient.output_assembly.output_size = outputSize
        self.eeipClient.output_assembly.output_rpi = outputRPI

        self.eeipClient.input_assembly.input_assembly = inputId
        self.eeipClient.input_assembly.input_size = inputSize
        self.eeipClient.input_assembly.input_rpi = inputRPI
        try:
            self.eeipClient.forward_open()
        except Exception as e:
            raise Exception(f"UDP forward open failed: {e}")

        self.stableCount = 0
        self.lastPdiStatus = None
        self.pdiStatus = None

    ##########################
    ## Public Class Methods ##
    ##########################

    ####################
    ## Public Getters ##
    ####################
    def get_od(self, index: int, subindex: int, signed: bool = False):
        """
        Read data from the Object Dictionary (OD) using the PDI interface.

        Parameters:
        index (int): The index to read from in the object dictionary.
        subindex (int): The subindex to read from in the object dictionary.
        signed (bool): Whether the value should be interpreted as signed. Default is False.

        Returns:
        int: The value read from the object dictionary.

        Notes:
        - Writes the index and subindex to PDI-SetValue2 (2291h:02h) and PDI-SetValue3 (2291h:03h).
        - Sends the OD-Read command using PDI-Cmd (2291h:04h).
        - Uses ToggleCmd to ensure the command is recognized.
        - Introduces a short delay (0.02s) to ensure the response is ready.
        - Retrieves the result from PDI-ReturnValue (2292h:02h).
        """
        self.toggle = not self.toggle
        readData = bytearray(8)

        # Step 1: Set index in PDI-SetValue2 (2291h:02h)
        readData[4:6] = index.to_bytes(2, byteorder="little", signed=signed)

        # Step 2: Set subindex in PDI-SetValue3 (2291h:03h)
        readData[6] = subindex

        # Step 3: Set OD-Read command in PDI-Cmd (2291h:04h)
        command = self._pdiCmd["OD-Read"]
        readData[7] = command if self.toggle else command + 128

        # Send data to initiate the read process
        self.eeipClient.output_assembly.iodata = readData

        # Short delay to ensure the response is ready
        time.sleep(0.05)

        # Retrieve the result from PDI-ReturnValue (2292h:02h)
        response = self.eeipClient.input_assembly.iodata
        return response

    def get_pole_pair(self):
        """
        Read the pole pair count from the motor controller.

        Returns:
        int: The current pole pair count read from the motor controller.

        Notes:
        - Reads from index 0x2030, subindex 0x00 in the object dictionary.
        - The pole pair count determines the relationship between the motor's electrical input and mechanical output.
        - This information is useful for ensuring proper motor operation and configuration.
        - The returned value is converted from bytes with a data size of 32 bits and unsigned format.
        """
        statusWord = self.get_od(index=0x2030, subindex=0x00, signed=False)
        return self.convert_bytes(value=statusWord, dataSize=32, signed=False)

    def get_is_open_loop(self):
        """
        Determine if the motor controller is configured for open-loop operation.

        Returns:
        bool: True if the controller is in open-loop mode, False if it is in closed-loop mode.

        Notes:
        - Reads from index 0x3202, subindex 0x00 in the object dictionary.
        - Checks bit position 0 to determine if the controller is operating in open-loop mode.
        - If the bit is 0, the controller is in open-loop mode; otherwise, it is in closed-loop mode.
        """
        statusWord = self.get_od(index=0x3202, subindex=0x00)
        return self._check_bit(
            decimalValue=statusWord[4],
            bitPosition=0,
            expectedValue=0,
        )

    def get_is_closed_loop(self):
        """
        Determine if the motor controller is configured for closed-loop operation.

        Returns:
        bool: True if the controller is in closed-loop mode, False if it is in open-loop mode.

        Notes:
        - Reads from index 0x3202, subindex 0x00 in the object dictionary.
        - Checks bit position 0 to determine if the controller is operating in closed-loop mode.
        - If the bit is 1, the controller is in closed-loop mode; otherwise, it is in open-loop mode.
        """
        statusWord = self.get_od(index=0x3202, subindex=0x00)
        return self._check_bit(
            decimalValue=statusWord[4],
            bitPosition=0,
            expectedValue=1,
        )

    def get_current_reduction_settings(self):
        """
        Retrieve the current reduction configuration from the motor controller.

        Returns:
        dict: A dictionary containing:
            - "Current Reduction Enabled" (bool): Whether current reduction is enabled.
            - "Idle Timeout" (int): The idle time (in ms) before current reduction activates.
            - "Reduction Percent" (int): The percentage of current reduction applied when idle.

        Notes:
        - Reads the idle timeout from index `0x2036`, subindex `0x00` as UNSIGNED32.
        - Reads the reduction percentage from index `0x2037`, subindex `0x00` as INTEGER32.
        - Reads the motor drive mode from index `0x3202`, subindex `0x00` as raw bytes.
        - Uses `__check_bit()` on bit 3 to determine whether current reduction is enabled.
        - Provides a comprehensive snapshot of the current reduction configuration for diagnostics or tuning.
        """
        idleTime = self._get_current_reduction_idle_time()
        reductionValue = self._get_current_reduction_value()
        motorDriveMode = self._get_current_reduction_status()

        return {"Current Reduction Enabled": motorDriveMode, "Idle Timeout": idleTime, "Reduction Percent": reductionValue}

    def get_analog_config(self):
        """
        Retrieve the configuration parameters for Analog Input 1 and Analog Input 2.

        Returns:
        dict: A dictionary containing:
            - "Analog 1 Offset" (int): Offset for Analog Input 1.
            - "Analog 1 Upper Limit" (int): Maximum current in microamps for Analog Input 1.
            - "Analog 1 ADC Resolution" (int): ADC resolution for Analog Input 1.
            - "Analog 2 Offset" (int): Offset for Analog Input 2.
            - "Analog 2 Upper Limit" (int): Maximum current in microamps for Analog Input 2.
            - "Analog 2 ADC Resolution" (int): ADC resolution for Analog Input 2.

        Notes:
        - Reads configuration values using the `get_od()` method from the following OD indexes:
            - 0x3321: Offset
            - 0x3322: Upper current limit in µA
            - 0x3323: ADC resolution
        - Subindex `0x01` is used for Analog Input 1, and `0x02` for Analog Input 2.
        - All values are read as 16-bit signed integers.
        """
        # Read Offset for Analog Input 1
        # a1Offset = self.get_od(index=0x3321, subindex=0x01, valueType="integer16")
        a1Offset = self.convert_bytes(value=self.get_od(index=0x3321, subindex=0x01, signed=True), dataSize=16, signed=True)
        # Read upper limit current in microamps
        # a1UpperLimit = self.get_od(index=0x3322, subindex=0x01, valueType="integer16")
        a1UpperLimit = self.convert_bytes(value=self.get_od(index=0x3322, subindex=0x01, signed=True), dataSize=16, signed=True)
        # Read ADC resolution value
        # a1Resolution = self.get_od(index=0x3322, subindex=0x01, valueType="integer16")
        a1Resolution = self.convert_bytes(value=self.get_od(index=0x3323, subindex=0x01, signed=True), dataSize=16, signed=True)
        # Read Offset for Analog Input 2
        # a2Offset = self.get_od(index=0x3321, subindex=0x02, valueType="integer16")
        a2Offset = self.convert_bytes(value=self.get_od(index=0x3321, subindex=0x02, signed=True), dataSize=16, signed=True)
        # Read upper limit current in microamps
        # a2UpperLimit = self.get_od(index=0x3322, subindex=0x02, valueType="integer16")
        a2UpperLimit = self.convert_bytes(value=self.get_od(index=0x3322, subindex=0x02, signed=True), dataSize=16, signed=True)
        # Read ADC resolution value
        # a2Resolution = self.get_od(index=0x3323, subindex=0x02, valueType="integer16")
        a2Resolution = self.convert_bytes(value=self.get_od(index=0x3323, subindex=0x02, signed=True), dataSize=16, signed=True)
        return {
            "Analog 1 Offset": a1Offset,
            "Analog 1 Upper Limit": a1UpperLimit,
            "Analog 1 ADC Resolution": a1Resolution,
            "Analog 2 Offset": a2Offset,
            "Analog 2 Upper Limit": a2UpperLimit,
            "Analog 2 ADC Resolution": a2Resolution,
        }

    def get_analog_1_mode(self) -> str:
        """
        Read the operating mode of Analog Input 1.

        Returns:
        str: "current" if Analog Input 1 is in current mode, "voltage" if in voltage mode.

        Notes:
        - Reads from index 0x3221, subindex 0x00 in the object dictionary.
        - Checks bit position 0 of the returned data to determine the mode.
        - If the bit is set (1), the mode is "current"; otherwise, it is "voltage".
        """
        statusWord = self.get_od(index=0x3221, subindex=0x00)
        isCurrent = self._check_bit(
            decimalValue=statusWord[4],
            bitPosition=0,
            expectedValue=1,
        )

        return "current" if isCurrent else "voltage"

    def get_analog_2_mode(self) -> str:
        """
        Read the operating mode of Analog Input 2.

        Returns:
        str: "current" if Analog Input 2 is in current mode, "voltage" if in voltage mode.

        Notes:
        - Reads from index 0x3221, subindex 0x00 in the object dictionary.
        - Checks bit position 1 of the returned data to determine the mode.
        - If the bit is set (1), the mode is "current"; otherwise, it is "voltage".
        """
        statusWord = self.get_od(index=0x3221, subindex=0x00)
        isCurrent = self._check_bit(
            decimalValue=statusWord[4],
            bitPosition=1,
            expectedValue=1,
        )

        return "current" if isCurrent else "voltage"

    def get_analog_1_input(self) -> int:
        """
        Read the value of Analog Input 1 from the motor controller.

        Returns:
        int: The analog input value, interpreted as a signed 32-bit integer.

        Notes:
        - Reads from index 0x3320, subindex 0x01 in the object dictionary.
        - The value is converted from bytes using a 32-bit signed format.
        - This input can be used for monitoring external sensor data or control signals.
        """
        statusWord = self.get_od(index=0x3320, subindex=0x01)
        return self.convert_bytes(value=statusWord, dataSize=32, signed=True)

    def get_analog_2_input(self) -> int:
        """
        Read the value of Analog Input 2 from the motor controller.

        Returns:
        int: The analog input value, interpreted as a signed 32-bit integer.

        Notes:
        - Reads from index 0x3320, subindex 0x02 in the object dictionary.
        - The value is converted from bytes using a 32-bit signed format.
        - This input can be used for monitoring external sensor data or control signals.
        """
        statusWord = self.get_od(index=0x3320, subindex=0x02)
        return self.convert_bytes(value=statusWord, dataSize=32, signed=True)

    def get_digital_output(self, outputIndex: int) -> bool:
        """
        Read the state of a digital output from the motor controller.

        Parameters:
        outputIndex (int): The index of the digital output to read (valid values: 1 to 5).

        Returns:
        bool: True if the digital output is high, False if it is low.

        Raises:
        ValueError: If `outputIndex` is not between 1 and 5.

        Notes:
        - Reads from index 0x60FE, subindex 0x01 in the object dictionary.
        - Uses bitwise checking to determine if the specified output is high or low.
        - The bit position is determined by `outputIndex - 1` for correct mapping.
        """
        if outputIndex not in [1, 2, 3, 4, 5]:
            raise ValueError("outputIndex must be between 1 and 5")

        statusWord = self.get_od(index=0x60FE, subindex=0x01)

        # Return False if the output is low, True if the output is high
        return self._check_bit(
            decimalValue=statusWord[6],
            bitPosition=outputIndex - 1,  # Adjust index for zero-based bit position
            expectedValue=1,
        )

    def get_torque_forming_current(self):
        """
        Read the torque-forming current of the motor.

        This current directly produces torque to drive the motor. When the load increases (e.g., due to a seizing valve),
        this value will rise to compensate.

        Returns:
        int: The torque-forming current value in mA.

        Notes:
        - Reads from index 0x2039, subindex 0x02 in the object dictionary.
        - The returned value is converted from bytes with a data size of 32 bits and signed format.
        """
        statusWord = self.get_od(index=0x2039, subindex=0x02)
        return self.convert_bytes(value=statusWord, dataSize=32, signed=True)

    def get_max_voltage(self):
        """
        Read the maximum motor voltage setting from the motor controller.

        Returns:
        int: The maximum motor voltage in millivolts.

        Notes:
        - Reads from index 0x321E, subindex 0x00 in the object dictionary.
        - The value is returned as an unsigned 32-bit integer.
        """
        statusWord = self.get_od(index=0x321E, subindex=0x00)
        return self.convert_bytes(value=statusWord, dataSize=32, byteOrder="little", signed=True)

    def get_max_current(self):
        """
        Read the maximum motor current setting from the motor controller.

        Returns:
        int: The maximum motor current in milliamps.

        Notes:
        - Reads from index 0x2031, subindex 0x00 in the object dictionary.
        - The value is returned as an unsigned 32-bit integer.
        """
        statusWord = self.get_od(index=0x2031, subindex=0x00)
        return self.convert_bytes(value=statusWord, dataSize=32)

    def get_acceleration(self):
        """
        Read the profile acceleration setting from the motor controller.

        Returns:
        int: The profile acceleration value.

        Notes:
        - Reads from index 0x6083, subindex 0x00 in the object dictionary.
        - The value is returned as an unsigned 32-bit integer.
        """
        statusWord = self.get_od(index=0x6083, subindex=0x00)
        return self.convert_bytes(value=statusWord, dataSize=32)

    def get_deceleration(self):
        """
        Read the profile deceleration setting from the motor controller.

        Returns:
        int: The profile deceleration value.

        Notes:
        - Reads from index 0x6084, subindex 0x00 in the object dictionary.
        - The value is returned as an unsigned 32-bit integer.
        """
        statusWord = self.get_od(index=0x6084, subindex=0x00)
        return self.convert_bytes(value=statusWord, dataSize=32)

    ####################
    ## Public Setters ##
    ####################
    def set_od(self, index: int, subindex: int, value: int, signed: bool = False, pdiCmd: str = None):
        """
        Write data to the Object Dictionary (OD) with ToggleCmd.

        Parameters:
        index (int): The index to write to in the object dictionary.
        subindex (int): The subindex to write to in the object dictionary.
        value (int): The value to write.
        signed (bool): Whether the value should be interpreted as signed. Default is False.
        pdiCmd (str, optional): A specific PDI command to use instead of the default OD-Write command.

        Returns:
        None

        Notes:
        - Writes to PDI-SetValue1 (2291h:01h), PDI-SetValue2 (2291h:02h), and PDI-SetValue3 (2291h:03h).
        - Uses ToggleCmd to ensure the command is recognized.
        - If `pdiCmd` is provided, it will override the default OD-Write command.
        - The value is written in little-endian byte order.
        """
        self.toggle = not self.toggle
        writeData = bytearray(8)

        # Step 1: Set value in PDI-SetValue1 (2291h:01h)
        writeData[0:4] = value.to_bytes(4, byteorder="little", signed=signed)

        # Step 2: Set index in PDI-SetValue2 (2291h:02h)
        writeData[4:6] = index.to_bytes(2, byteorder="little", signed=signed)

        # Step 3: Set subindex in PDI-SetValue3 (2291h:03h)
        writeData[6] = subindex

        # Step 4: Set OD-Write command in PDI-Cmd (2291h:04h)
        command = self._pdiCmd.get(pdiCmd, self._pdiCmd["OD-Write"])  # Use provided pdiCmd or default OD-Write
        writeData[7] = command if self.toggle else command + 128

        # Send data
        self.eeipClient.output_assembly.iodata = writeData
        time.sleep(0.05)

    def set_move(
        self,
        moveType: str = "relative",
        target: int = 5000,
        maxSpeed: int = 200,
        bypassError: bool = False,
    ):
        """
        Move the motor to a specified position.

        Parameters:
        moveType (str): Type of move to perform. Must be "relative" or "absolute". Default is "relative".
        target (int): The target position in encoder counts. Default is 5000.
        maxSpeed (int): The maximum speed for the movement. Default is 200.
        bypassError (bool): If True, allows the move to continue without raising an error when the timer expires.

        Returns:
        None

        Notes:
        - Uses `_write_move_command` to initiate the move.
        - Monitors position completion with `_get_target_reached()`.
        - Uses `_start_timer()` to track movement duration.
        - If `bypassError` is False, raises a `RuntimeError` if the move exceeds the allowed time.
        """
        if moveType.lower() not in ["relative", "absolute"]:
            raise ValueError("moveType must be 'relative' or 'absolute'")
        if moveType.lower() == "relative":
            pdiCmd = "ProfilePosRel"
        else:
            pdiCmd = "ProfilePosAbs"
        # Execute the move command
        self._set_move_command(pdiCmd=pdiCmd, target=target, maxSpeed=maxSpeed)
        self._start_timer()

        # Monitor until position is reached or timeout occurs
        while not self._get_target_reached():
            time.sleep(0.0001)

            # Handle timeout conditions
            if bypassError:
                if self._timer_is_done():
                    break
            else:
                if self._timer_is_done():
                    raise RuntimeError(f"Motor failed to reach position in {self.timerDuration} seconds")

    def set_move_torque_current_avg(
        self,
        moveType: str = "relative",
        target: int = 5000,
        maxSpeed: int = 200,
        filterBelow: int = None,
        bypassError: bool = False,
    ) -> dict:
        """
        Execute a move operation while monitoring and averaging torque current.

        Parameters:
        moveType (str): Type of move to perform. Must be "relative" or "absolute". Default is "relative".
        target (int): The target position in encoder counts. Default is 5000.
        maxSpeed (int): The maximum speed for the movement. Default is 200.
        filterBelow (int, optional): Removes values below this threshold from the current dataset before averaging. Default is None.
        bypassError (bool): If True, allows the move to continue without raising an error when the timer expires.

        Returns:
        dict: A dictionary containing:
            - "average" (int): The average torque current measured during the move.
            - "data" (list): The filtered set of torque current measurements.

        Notes:
        - Inverts torque current readings for negative target positions to maintain consistency in measurement.
        - Uses `_write_move_command` to initiate the move.
        - Monitors position completion with `_get_target_reached()`.
        - Logs torque current readings during movement.
        - Uses `_timer_is_done()` to determine if the move exceeds the allowed time.
        - Filters the dataset using `_filter_data()` before computing the average.
        """
        currentData = []

        # Determine if movement is negative to adjust current readings
        isNegative = True if target > 0 else False
        if moveType.lower() not in ["relative", "absolute"]:
            raise ValueError("moveType must be 'relative' or 'absolute'")
        if moveType.lower() == "relative":
            pdiCmd = "ProfilePosRel"
        else:
            pdiCmd = "ProfilePosAbs"
        # Execute the move command
        self._set_move_command(pdiCmd=pdiCmd, target=target, maxSpeed=maxSpeed)
        self._start_timer()

        # Monitor until position is reached
        while not self._get_target_reached():
            time.sleep(0.0001)

            # Capture torque current values, adjusting for negative moves
            if isNegative:
                currentData.append(-1 * (self.get_torque_forming_current()))
            else:
                currentData.append(self.get_torque_forming_current())

            # Handle timeout conditions
            if bypassError:
                if self._timer_is_done():
                    break
            else:
                if self._timer_is_done():
                    raise RuntimeError(f"Motor failed to reach position in {self.timerDuration} seconds")

        # Filter and compute average torque current
        filteredDataSet = self._filter_data(below=True, threshold=filterBelow, dataSet=currentData)
        if len(filteredDataSet) <= 0:
            avgCurrent = 0
        else:
            avgCurrent = sum(filteredDataSet) / len(filteredDataSet)
            avgCurrent = round(avgCurrent)

        return {"average": avgCurrent, "data": filteredDataSet}

    def set_failure_timer_duration(self, duration: float):
        """
        Set the failure timer duration for monitoring motor movement.

        Parameters:
        duration (float): The duration in seconds before a movement operation is considered failed.

        Returns:
        None

        Notes:
        - This value is used by `_start_timer()` and `_timer_is_done()` to track movement timeout conditions.
        """
        self.timerDuration = duration

    def set_pole_pair(self, stepAngle: float = 1.8):
        """
        Set the pole pair number using an Object Dictionary (OD) write.

        Parameters:
        stepAngle (float): The step angle of the motor. Valid values are 0.9 or 1.8. Default is 1.8.

        Returns:
        None

        Raises:
        ValueError: If the provided step angle is not 0.9 or 1.8.

        Notes:
        - Writes to index 0x2030, subindex 0x00 in the object dictionary.
        - Sets the value to 50 for a step angle of 1.8.
        - Sets the value to 100 for a step angle of 0.9.
        """
        if stepAngle in [1.8]:
            self.set_od(index=0x2030, subindex=0x00, value=50)
            return True
        elif stepAngle in [0.9]:
            self.set_od(index=0x2030, subindex=0x00, value=100)
            return True
        else:
            raise ValueError(f"Step Angle {stepAngle} is not a valid value")

    def set_open_loop(self):
        """
        Set the Motor Drive Submode Select to Open-Loop mode. Check to see if we already in Open-Loop, if so return.

        Parameters:
        None

        Returns:
        None

        Notes:
        - Writes to index 0x3202 in the object dictionary, which controls the motor sub mode functions.
        - Toggling this value is not possible in the Operation Enable State.
        - The bit for Open/Closed Loop (CL/OL) is bit 0 in  3202
        """
        statusWord = self.get_od(index=0x3202, subindex=0x00)
        isOpenLoop = self._check_bit(
            decimalValue=statusWord[4],
            bitPosition=0,
            expectedValue=0,
        )
        if isOpenLoop:
            return
        # Modify the specified output bit
        modifiedByte = self._modify_bit(bitIndex=0, value=0, byteToModify=statusWord[4])
        statusWord[4] = modifiedByte
        # Convert the modified byte array to an integer
        modifiedValue = int.from_bytes(statusWord[4:8], byteorder="little", signed=False)
        self.set_od(index=0x3202, subindex=0x00, value=modifiedValue, signed=False)

    def set_closed_loop(self):
        """
        Set the Motor Drive Submode Select to Closed-Loop mode. Check to see if we already in Closed-Loop, if so return.

        Parameters:
        None

        Returns:
        None

        Notes:
        - Writes to index 0x3202 in the object dictionary, which controls the motor sub mode functions.
        - Toggling this value is not possible in the Operation Enable State.
        - The bit for Open/Closed Loop (CL/OL) is bit 0 in 0x3202
        """
        statusWord = self.get_od(index=0x3202, subindex=0x00)
        isClosedLoop = self._check_bit(
            decimalValue=statusWord[4],
            bitPosition=0,
            expectedValue=1,
        )
        if isClosedLoop:
            return
        # Modify the specified output bit
        modifiedByte = self._modify_bit(bitIndex=0, value=1, byteToModify=statusWord[4])
        statusWord[4] = modifiedByte
        # Convert the modified byte array to an integer
        modifiedValue = int.from_bytes(statusWord[4:8], byteorder="little", signed=False)
        self.set_od(index=0x3202, subindex=0x00, value=modifiedValue, signed=False)

    def set_enable_current_reduction(self, idleTimeout: int = 500, currRedPercent: int = 90):
        """
        Enable current reduction on the motor controller, which involves multiple steps (see notes).
        When them motor is at a stand still, reduce the current in order to reduce heat build up and power loss.
        This option is only available to motors in open loop mode.

        Parameters:
        idleTimeout (int): The number of Milliseconds the motor must stand still before current reduction activates.
        currRedPercent (int): Percentage of current that will be reduced, i.e. 90 will result in a 90% reduction in allowed current from the rated current value.

        Returns:
        None

        Notes:
        - In object 3202h (Motor Drive Submode Select), set bit 3 (CurRed) to "1".
        - In object 2036h (open-loop current reduction idle time), the time in milliseconds is specified that the motor must be at a standstill (set value is checked) before current reduction is activated.
        - In object 2037h (open-loop current reduction value/factor), the root mean square is specified to which the rated current is to be reduced if current reduction is activated in open loop and the motor is at a standstill.
        """
        if currRedPercent > 100 or currRedPercent < 0:
            raise ValueError(f"currRedPercent must be between 0 and 100, given value: {currRedPercent}")
        else:
            currRedPercent = -abs(currRedPercent)
        statusWord = self.get_od(index=0x3202, subindex=0x00)
        isOpenLoop = self._check_bit(
            decimalValue=statusWord[4],
            bitPosition=0,
            expectedValue=0,
        )
        if not isOpenLoop:  # Move to open loop mode if we are not in that mode
            self.set_open_loop()
        # Modify the specified output bit
        modifiedByte = self._modify_bit(bitIndex=3, value=1, byteToModify=statusWord[4])
        statusWord[4] = modifiedByte
        # Convert the modified byte array to an integer
        modifiedValue = int.from_bytes(statusWord[4:8], byteorder="little", signed=False)
        # Write the current reduction bit to enable
        self.set_od(index=0x3202, subindex=0x00, value=modifiedValue, signed=False)
        # Write the current reduction idle time
        self.set_od(index=0x2036, subindex=0x00, value=idleTimeout, signed=False)
        # Wite the current reduction factor
        self.set_od(index=0x2037, subindex=0x00, value=currRedPercent, signed=True)

    def set_disable_current_reduction(self):
        """
        Disable the automatic current reduction feature on the motor controller.

        Returns:
        None

        Notes:
        - Reads the control word from index `0x3202`, subindex `0x00`.
        - Modifies bit 3 to disable current reduction.
        - Writes the updated value back to the object dictionary.
        - Uses `_modify_bit()` to manipulate the bit safely.
        """
        statusWord = self.get_od(index=0x3202, subindex=0x00)

        # Modify the specified output bit (bit 3) to disable current reduction
        modifiedByte = self._modify_bit(bitIndex=3, value=0, byteToModify=statusWord[4])
        statusWord[4] = modifiedByte

        # Convert the modified byte array to an integer
        modifiedValue = int.from_bytes(statusWord[4:8], byteorder="little", signed=False)

        # Write the updated value back to disable current reduction
        self.set_od(index=0x3202, subindex=0x00, value=modifiedValue, signed=False)

    def set_auto_setup(self):
        """
        Initiate the auto setup process on the motor controller.

        Returns:
        None

        Notes:
        - Sends the "Auto Setup" command via PDI-Cmd (2291h:04h).
        - Uses the `pdiCmd` mapping to set the appropriate command value.
        - Introduces a short delay (0.02s) after sending the command.
        - This function is currently incomplete and may require additional implementation.
        """
        if self._get_auto_setup_complete():
            return True
        else:
            writeData = bytearray(8)

            # Step 1: Set Auto Setup command in PDI-Cmd (2291h:04h)
            writeData[7] = self._pdiCmd["Auto Setup"]  # Set the PDI command

            # Send data to the motor controller
            self.eeipClient.output_assembly.iodata = writeData
            self._start_timer()
            self.timerDuration(30)
            while not self._get_auto_setup_complete():
                time.sleep(0.01)
                if self._timer_is_done():
                    raise Exception(f"Auto Setup failed to complete in {self.timerDuration}")
            self.timerDuration(5)
            return True

    def set_analog_config(self):
        """
        Configure the analog inputs for current measurement.

        This function sets the offset, upper limit, and ADC resolution for both Analog Input 1 and Analog Input 2.

        Returns:
        None

        Notes:
        - Writes to the following object dictionary indices and subindices:
            - Index 0x3321: Offset (subindex 0x01 for Analog 1, subindex 0x02 for Analog 2).
            - Index 0x3322: Upper limit current in microamps (subindex 0x01 for Analog 1, subindex 0x02 for Analog 2).
            - Index 0x3323: ADC resolution (subindex 0x01 for Analog 1, subindex 0x02 for Analog 2).
        - The upper limit is set to **20000 µA** (20mA).
        - The ADC resolution is set to **1023**, assuming a 10-bit ADC.
        - All values are written as signed integers.
        """
        # Write the offset
        self.set_od(index=0x3321, subindex=0x01, value=0, signed=True)
        # Write the upper limit current in microamps
        self.set_od(index=0x3322, subindex=0x01, value=20000, signed=True)
        # Write the ADC resolution value
        self.set_od(index=0x3323, subindex=0x01, value=1023, signed=True)

        # Write the offset for Analog Input 2
        self.set_od(index=0x3321, subindex=0x02, value=0, signed=True)
        # Write the upper limit current in microamps for Analog Input 2
        self.set_od(index=0x3322, subindex=0x02, value=20000, signed=True)
        # Write the ADC resolution value for Analog Input 2
        self.set_od(index=0x3323, subindex=0x02, value=1023, signed=True)

    def set_analog_1_mode(self, mode: str = "current"):
        """
        Set the operating mode for Analog Input 1.

        Parameters:
        mode (str): The desired mode for Analog Input 1. Acceptable values are "current" or "voltage". Default is "current".

        Returns:
        None

        Raises:
        ValueError: If the provided mode is not "current" or "voltage".

        Notes:
        - Reads the current mode of Analog Input 2 using `read_analog_2_mode()`.
        - Determines the appropriate setting based on the combination of Analog Input 1 and Analog Input 2 modes.
        - Writes the mode configuration to index 0x3221, subindex 0x00 in the object dictionary.
        - Uses an encoded integer value to represent the mode settings:
            - 3: Both inputs in current mode.
            - 2: Analog 1 in voltage mode, Analog 2 in current mode.
            - 1: Analog 1 in current mode, Analog 2 in voltage mode.
            - 0: Both inputs in voltage mode.
        """
        if mode.lower() not in ["current", "voltage"]:
            raise ValueError("Mode must be 'current' or 'voltage'")

        mode2 = self.get_analog_2_mode()

        if mode.lower() == "current" and mode2.lower() == "current":
            setValue = 3
        elif mode.lower() == "voltage" and mode2.lower() == "current":
            setValue = 2
        elif mode.lower() == "current" and mode2.lower() == "voltage":
            setValue = 1
        elif mode.lower() == "voltage" and mode2.lower() == "voltage":
            setValue = 0

        self.set_od(index=0x3221, subindex=0x00, value=setValue, signed=True)

    def set_analog_2_mode(self, mode: str = "current"):
        """
        Set the operating mode for Analog Input 2.

        Parameters:
        mode (str): The desired mode for Analog Input 2. Acceptable values are "current" or "voltage". Default is "current".

        Returns:
        None

        Raises:
        ValueError: If the provided mode is not "current" or "voltage".

        Notes:
        - Reads the current mode of Analog Input 1 using `read_analog_2_mode()`.
        - Determines the appropriate setting based on the combination of Analog Input 1 and Analog Input 2 modes.
        - Writes the mode configuration to index 0x3221, subindex 0x00 in the object dictionary.
        - Uses an encoded integer value to represent the mode settings:
            - 3: Both inputs in current mode.
            - 2: Analog 1 in voltage mode, Analog 2 in current mode.
            - 1: Analog 1 in current mode, Analog 2 in voltage mode.
            - 0: Both inputs in voltage mode.
        """
        if mode.lower() not in ["current", "voltage"]:
            raise ValueError("Mode must be 'current' or 'voltage'")

        mode1 = self.get_analog_1_mode()

        if mode.lower() == "current" and mode1.lower() == "current":
            setValue = 3
        elif mode.lower() == "voltage" and mode1.lower() == "current":
            setValue = 1
        elif mode.lower() == "current" and mode1.lower() == "voltage":
            setValue = 2
        elif mode.lower() == "voltage" and mode1.lower() == "voltage":
            setValue = 0

        self.set_od(index=0x3221, subindex=0x00, value=setValue, signed=True)

    def set_digital_output(self, outputIndex: int, outputValue: int):
        """
        Set the state of a digital output on the motor controller.

        Parameters:
        outputIndex (int): The index of the digital output to modify (valid values: 1 to 5).
        outputValue (int): The desired state of the output (0 for low, 1 for high).

        Returns:
        None

        Raises:
        ValueError: If `outputIndex` is not between 1 and 5.
        ValueError: If `outputValue` is not 0 or 1.

        Notes:
        - Reads the current digital output status from index 0x60FE, subindex 0x01.
        - Modifies the specified output bit using `_modify_bit`.
        - Writes the modified status back to the object dictionary.
        - Uses little-endian byte order for conversion.
        """
        if outputIndex not in [1, 2, 3, 4, 5]:
            raise ValueError("outputIndex must be between 1 and 5")
        if outputValue not in [0, 1]:
            raise ValueError("outputValue must be 0 or 1")

        statusWord = self.get_od(index=0x60FE, subindex=0x01)

        # Modify the specified output bit
        modifiedByte = self._modify_bit(bitIndex=outputIndex - 1, value=outputValue, byteToModify=statusWord[6])
        statusWord[6] = modifiedByte

        # Convert the modified byte array to an integer
        modifiedValue = int.from_bytes(statusWord[4:8], byteorder="little", signed=False)

        # Write the updated value back to the object dictionary
        self.set_od(index=0x60FE, subindex=0x01, value=modifiedValue)

    def set_max_voltage(self, voltageValue: int = 4000):
        """
        Set the maximum motor voltage using an Object Dictionary (OD) write.

        Parameters:
        voltageValue (int): The maximum voltage value in millivolts. Default is 4000.
            - The value should be within the motor's voltage handling capacity.

        Returns:
        None

        Notes:
        - Writes to index 0x321E, subindex 0x00 in the object dictionary.
        - Exceeding the motor's rated voltage may cause overheating or damage.
        - The value is written as an unsigned integer.
        - Values <= 1000 are used as percentage of supplied voltage, so 995 would 99.5% of the supplied voltage
        - Values > 1000 are used mV readings, so 12000 would be 12V
        """
        self.set_od(index=0x321E, subindex=0x00, value=voltageValue, signed=False)

    def set_max_current(self, currentValue: int = 6000):
        """
        Set the maximum motor current using an Object Dictionary (OD) write.

        Parameters:
        currentValue (int): The maximum current value in milliamps. Default is 6000.
            - The value should be within the motor's current handling capacity.

        Returns:
        None

        Notes:
        - Writes to index 0x2031, subindex 0x00 in the object dictionary.
        - Exceeding the motor's rated current may cause overheating or damage.
        - The value is written as an unsigned integer.
        """
        self.set_od(index=0x2031, subindex=0x00, value=currentValue, signed=False)

    def set_acceleration(self, accelValue: int, index: int = 0x6083, subindex: int = 0x00):
        """
        Set the profile acceleration using an Object Dictionary (OD) write.

        Parameters:
        accelValue (int): The acceleration value to write.
        index (int): The index in the object dictionary. Default is 0x6083.
        subindex (int): The subindex in the object dictionary. Default is 0x00.

        Returns:
        None

        Notes:
        - Writes to index 0x6083 in the object dictionary, which controls the acceleration profile.
        - The acceleration value should be within the motor's supported range.
        """
        self.set_od(index=index, subindex=subindex, value=accelValue)

    def set_deceleration(self, decelValue: int, index: int = 0x6084, subindex: int = 0x00):
        """
        Set the profile deceleration using an Object Dictionary (OD) write.

        Parameters:
        decelValue (int): The deceleration value to write.
        index (int): The index in the object dictionary. Default is 0x6084.
        subindex (int): The subindex in the object dictionary. Default is 0x00.

        Returns:
        None

        Notes:
        - Writes to index 0x6084 in the object dictionary, which controls the deceleration profile.
        - The deceleration value should be within the motor's supported range.
        """
        self.set_od(index=index, subindex=subindex, value=decelValue)

    def set_reset_fault(self):
        """
        Send a fault-clear command to the motor controller to reset any active errors.

        Returns:
        None

        Notes:
        - Sends the `Clear Error` command using the PDI interface (command code 2).
        - Uses toggle logic (adds 128 to the command code on alternating calls) to ensure the command is recognized as new.
        - Command is written to the 8th byte of the output assembly (`ioData[7]`).
        - A short delay (`time.sleep(0.02)`) allows the controller time to process the command.
        - This is typically used after a fault or safety stop has occurred and the drive is ready to resume operation.
        """
        self.toggle = not self.toggle
        ioData = bytearray(8)  # Initialize the data array
        if self.toggle:
            ioData[7] = self._pdiCmd["Clear Error"]  # Set the PDI command based on the passed-in string
        else:
            ioData[7] = self._pdiCmd["Clear Error"] + 128  # Set the PDI command with ToggleCmd bit

        # Send data to the motor controller
        self.eeipClient.output_assembly.iodata = ioData
        time.sleep(0.02)  # Allow time for the command to be processed

    ####################
    ## Public Helpers ##
    ####################
    def convert_bytes(self, value: bytearray, startReg: int = 4, dataSize: int = 32, signed: bool = False, byteOrder: str = "little"):
        """
        Convert a byte sequence to an integer, interpreting only the relevant bytes.

        Parameters:
        value (bytearray): Bytearray representing the register data.
        startReg (int): The starting register (byte index) to read from. Default is 4.
        dataSize (int): The size of the data in bits (16 or 32). Default is 32.
        signed (bool): Whether the value should be interpreted as a signed integer. Default is False.

        Returns:
        int: The interpreted integer value.

        Notes:
        - Reads 2 bytes for 16-bit values and 4 bytes for 32-bit values.
        - Uses little-endian byte order for conversion.
        - Ensures that only the specified byte range is interpreted.
        """
        # Determine the number of bytes to read based on data size
        if dataSize == 16:
            endReg = startReg + 2  # Only read 2 bytes for 16-bit values
        elif dataSize == 32:
            endReg = startReg + 8  # Read 4 bytes for 32-bit values
        # Convert the specified byte range to an integer
        return int.from_bytes(value[startReg:endReg], byteorder=byteOrder, signed=signed)

    ###########################
    ## Private Class Methods ##
    ###########################

    #####################
    ## Private Getters ##
    #####################

    def _get_auto_setup_complete(self):
        """
        Check if the auto setup process has completed successfully.

        Returns:
        bool: True if the auto setup process is complete, False otherwise.

        Notes:
        - Calls `_get_pdi_status` to update the current PDI status.
        - Checks if the "PdiStatusAutosetupDone" bit is set in the PDI status.
        """
        self._get_pdi_status()
        return self._check_bit(
            decimalValue=self.pdiStatus[1],
            bitPosition=self._pdiStatusMapping["PdiStatusAutosetupDone"],
            expectedValue=1,
        )

    def _get_pdi_status(self):
        """
        Gather the current PDI status from the input assembly.

        Returns:
        None

        Notes:
        - Retrieves the latest PDI status from the motor controller.
        - Updates the `pdiStatus` attribute with the input assembly data.
        """
        self.pdiStatus = self.eeipClient.input_assembly.iodata

    def _get_target_reached(self) -> bool:
        """
        Monitor whether the target position has been reached.

        Returns:
        bool: True if the target position is reached, False otherwise.

        Notes:
        - Calls `_get_pdi_status` to update the current PDI status.
        - Checks if the "PdiStatusTargetReached" bit is set in the PDI status.
        - Uses `_check_bit` to determine if the target position has been successfully reached.
        """
        self._get_pdi_status()
        return self._check_bit(
            decimalValue=self.pdiStatus[0],
            bitPosition=self._pdiStatusMapping["PdiStatusTargetReached"],
            expectedValue=1,
        )

    def _get_current_reduction_idle_time(self) -> int:
        """
        Read the idle time before current reduction activates.

        Returns:
        int: The idle time value in milliseconds.

        Notes:
        - Reads the idle time for current reduction from index `0x2036`, subindex `0x00`.
        - Converts the retrieved byte data into a 32-bit unsigned integer.
        - This value determines how long the motor remains idle before reducing current to save energy.
        """
        statusWord = self.get_od(index=0x2036, subindex=0x00)
        return self.convert_bytes(value=statusWord, dataSize=32, signed=False)

    def _get_current_reduction_value(self):
        """
        Read the current reduction value.

        Returns:
        int: The current reduction percentage.

        Notes:
        - Reads the current reduction setting from index `0x2037`, subindex `0x00`.
        - Converts the retrieved byte data into a 32-bit signed integer.
        - This value represents the percentage of current reduction applied when the motor is idle.
        """
        statusWord = self.get_od(index=0x2037, subindex=0x00)
        return self.convert_bytes(value=statusWord, dataSize=32, signed=True)

    def _get_current_reduction_status(self):
        """
        Check if current reduction is enabled.

        Returns:
        bool: True if current reduction is enabled, False otherwise.

        Notes:
        - Reads the control word from index `0x3202`, subindex `0x00`.
        - Checks bit position `3` to determine if current reduction is active.
        - Uses `_check_bit()` to extract and evaluate the specific bit.
        """
        statusWord = self.get_od(index=0x3202, subindex=0x00)
        return self._check_bit(
            decimalValue=statusWord[4],
            bitPosition=3,
            expectedValue=1,
        )

    #####################s
    ## Private Setters ##
    #####################

    def _set_move_command(
        self,
        pdiCmd: str,
        target: int = 5000,
        maxSpeed: int = 200,
    ):
        """
        Write a movement command to the motor controller.

        Parameters:
        pdiCmd (str): The PDI command to execute (e.g., "ProfilePosAbs", "ProfilePosRel").
        target (int): The target position in encoder counts. Default is 5000.
        maxSpeed (int): The maximum speed for the movement. Default is 200.

        Returns:
        None

        Notes:
        - Writes the target position and maximum speed to the motor controller.
        - Uses ToggleCmd to ensure the command is recognized.
        - The position value is written as a signed 32-bit integer.
        - The speed value is written as a signed 16-bit integer.
        - Introduces a short delay (0.02s) after writing to ensure the command is processed.
        """
        self.toggle = not self.toggle
        target = int(target)  # Cast the target position as int
        positionData = bytearray(8)  # Initialize the data array
        positionData[0:4] = target.to_bytes(4, byteorder="little", signed=True)  # Convert pos value to bytes
        positionData[4:6] = maxSpeed.to_bytes(2, byteorder="little", signed=True)  # Convert speed value to bytes
        if self.toggle:
            positionData[7] = self._pdiCmd[pdiCmd]  # Set the PDI command based on the passed-in string
        else:
            positionData[7] = self._pdiCmd[pdiCmd] + 128  # Set the PDI command with ToggleCmd bit

        # Send data to the motor controller
        self.eeipClient.output_assembly.iodata = positionData
        time.sleep(0.02)  # Allow time for the command to be processed

    ####################
    ## Private Helpers ##
    ####################
    def _modify_bit(self, bitIndex: int, value: int, byteToModify: int) -> int:
        """
        Modify a specific bit in a byte.

        Parameters:
        bitIndex (int): The position of the bit to modify (0-7).
        value (int): The new bit value (0 or 1).
        byteToModify (int): The byte in which the bit should be modified.

        Returns:
        int: The modified byte value.

        Notes:
        - If `value` is 1, the specified bit is set to 1.
        - If `value` is 0, the specified bit is cleared to 0.
        - Uses bitwise operations to modify only the specified bit.
        """
        if value == 1:
            new_value = byteToModify | (1 << bitIndex)  # Set bit to 1
        else:
            new_value = byteToModify & ~(1 << bitIndex)  # Clear bit to 0
        return new_value  # Return the modified decimal value

    def _check_bit(self, decimalValue: int, bitPosition: int, expectedValue: int) -> bool:
        """
        Check if a specific bit in a decimal value matches the expected value.

        Parameters:
        decimalValue (int): The decimal value representing a byte (0-255).
        bitPosition (int): The bit position to check (0-7).
        expectedValue (int): The expected value of the bit (0 or 1).

        Returns:
        bool: True if the bit matches the expected value, False otherwise.

        Notes:
        - Validates that `decimalValue` is within the range of a byte (0-255).
        - Ensures `bitPosition` is between 0 and 7.
        - Confirms `expectedValue` is either 0 or 1.
        - Uses bitwise shifting to extract and compare the target bit.
        """
        # Validate inputs
        if not (0 <= decimalValue <= 255):
            raise ValueError("decimalValue must be between 0 and 255.")
        if not (0 <= bitPosition <= 7):
            raise ValueError("bitPosition must be between 0 and 7.")
        if expectedValue not in (0, 1):
            raise ValueError("expectedValue must be 0 or 1.")

        # Extract the bit at the specified position
        bitValue = (decimalValue >> bitPosition) & 1

        # Compare the extracted bit with the expected value
        return bitValue == expectedValue

    def _start_timer(self):
        """
        Start the failure timer to track movement duration.

        Returns:
        None

        Notes:
        - Sets `failureTimer` to the current high-precision time using `time.perf_counter()`.
        - This timer is used in `_timer_is_done()` to determine if a movement operation has exceeded the allowed duration.
        """
        self.failureTimer = time.perf_counter()

    def _timer_is_done(self) -> bool:
        """
        Check if the failure timer has expired.

        Returns:
        bool: True if the timer duration has elapsed, False otherwise.

        Notes:
        - Uses `time.perf_counter()` to measure elapsed time since `failureTimer` was started.
        - Compares the elapsed time against `timerDuration` to determine if the timer has expired.
        - This function is used to enforce movement timeouts in motor operations.
        """
        return (time.perf_counter() - self.failureTimer) > self.timerDuration

    def _filter_data(self, below: bool = True, threshold: int = None, dataSet: list = []) -> list:
        """
        Filter integers out of a list based on a threshold.

        This function is used to clean up ambient values in current monitoring by removing values
        that are either below or above a specified threshold.

        Parameters:
        below (bool): If True, filters out values below the threshold. If False, filters out values above the threshold.
        threshold (int, optional): The threshold value to filter against. If None, no filtering is applied.
        dataSet (list): The list of integer values to be filtered.

        Returns:
        list: A filtered list of values.

        Notes:
        - The threshold is non-inclusive (values equal to the threshold are kept).
        - If `threshold` is None, the original dataset is returned unmodified.
        - Default `dataSet` should not be defined as a mutable list in the function signature (use `None` instead).
        """
        # Avoid mutable default argument issue
        if dataSet is None:
            dataSet = []

        returnList = []
        if threshold is None:
            return dataSet

        if below:
            returnList = [dataPoint for dataPoint in dataSet if dataPoint >= threshold]
        else:
            returnList = [dataPoint for dataPoint in dataSet if dataPoint <= threshold]

        return returnList
