"""
RH56F1 EtherCAT Hand Controller - Python API
Real-time per-finger control via C++ wrapper using ctypes
"""

import ctypes
import os
import time
from typing import List, Optional, Tuple, Dict, Union
from pathlib import Path
from dataclasses import dataclass
from feathersdk.utils.logger import warning, error
from enum import IntEnum

@dataclass
class FingerData:
    """Complete data for a single finger"""

    finger_id: int  # 1-6
    position: int  # Raw position value
    angle: int  # Angle in 0.1 degree units
    force: int  # Actual force
    current: int  # Current draw
    temperature: int  # Temperature reading
    error: int  # Error code
    status: int  # Status word

    def __str__(self):
        return "{:4} {:<8} {:<8} {:<8} {:<8} {:<6} {:<8} {:<8}".format(
            self.finger_id, self.position, self.angle, self.force, 
            self.current, self.temperature, self.error, self.status
        )


@dataclass
class TactileSensor:
    """Tactile sensor data for one sensor"""

    normal_force: int
    tangential_force: int
    direction: int
    proximity_low: int
    proximity_high: int

    def __str__(self):
        return "{:<10} {:<10} {:<8} {:<10} {:<10}".format(
            self.normal_force, self.tangential_force, self.direction, self.proximity_low, self.proximity_high
        )


@dataclass
class HandState:
    """Complete hand state"""

    fingers: List[FingerData]
    tactile_sensors: List[TactileSensor]
    timestamp: float


class FingerID(IntEnum):
    """Finger ID enumeration for RH56F1 hand"""
    LITTLE = 1
    RING = 2
    MIDDLE = 3
    INDEX = 4
    THUMB_BEND = 5
    THUMB_SIDESWAY = 6

class TactileSensorID(IntEnum):
    """Sensor ID enumeration for RH56F1 hand"""
    LITTLE = 1
    RING = 2
    MIDDLE = 3
    INDEX = 4
    THUMB = 5
    PALM_LEFT = 6
    PALM_MIDDLE = 7
    PALM_RIGHT = 8


class RH56F1Hand:
    """
    Python wrapper for RH56F1 EtherCAT hand controller with per-finger data access

    This class provides a Pythonic interface to the C++ EtherCAT library,
    giving you C++ real-time performance with Python convenience.

    Finger Numbering (from user manual):
        1: Little finger
        2: Ring finger
        3: Middle finger
        4: Index finger
        5: Thumb (bend)
        6: Thumb (sidesway)

    Parameter Ranges:
        - Angle: 0-3600 (0.1 degree units, e.g., 1700 = 170.0°)
        - Force: 0-1000 (device-specific units)
        - Speed: 0-5000 (device-specific units)
    """

    # Parameter range constants (from device manual)
    # Note: Individual fingers have different limits - these are the safe global bounds
    ANGLE_MIN = 600  # Thumb rotation minimum
    ANGLE_MAX = 1800  # Thumb rotation maximum
    FORCE_MIN = 0
    FORCE_MAX = 1000
    SPEED_MIN = 0
    SPEED_MAX = 5000

    # Per-finger angle limits (finger_id: (min, max))
    FINGER_ANGLE_LIMITS = {
        FingerID.LITTLE: (900, 1740),  # Little finger: 90° - 174°
        FingerID.RING: (900, 1740),  # Ring finger: 90° - 174°
        FingerID.MIDDLE: (900, 1740),  # Middle finger: 90° - 174°
        FingerID.INDEX: (900, 1740),  # Index finger: 90° - 174°
        FingerID.THUMB_BEND: (1100, 1450),  # Thumb bending: 110° - 145°
        FingerID.THUMB_SIDESWAY: (600, 1800),  # Thumb rotation: 60° - 180°
    }

    # PDO Input Mapping (from user_manual.md - 0x6000)
    # Array indices map to SubIndex values (SubIndex 1-76 -> array index 0-75)
    PDO_MAP = {
        "position": (0, 6),  # 0x6000:01-06 POSACT1-6 (array index 0-5)
        "angle": (6, 6),  # 0x6000:07-0C ANGLEACT1-6 (array index 6-11)
        "force": (12, 6),  # 0x6000:0D-12 FORCEACT1-6 (array index 12-17)
        "current": (18, 6),  # 0x6000:13-18 CURACT1-6 (array index 18-23)
        "error": (24, 6),  # 0x6000:19-1E ERROR1-6 (array index 24-29)
        "status": (30, 6),  # 0x6000:1F-24 STATUS1-6 (array index 30-35)
        "temperature": (36, 6),  # 0x6000:25-2A TEMP1-6 (array index 36-41)
        "tactile": 42,  # 0x6000:2B-4C Tactile/Proximity sensors (array index 42-75)
    }

    # PDO Output Mapping (from user_manual.md - 0x7000)
    # Array indices map to SubIndex values (SubIndex 1-19 -> array index 0-18)
    PDO_OUTPUT_MAP = {
        "enable_set": 0,  # 0x7000:01 ENABLE_SET (array index 0)
        "angle_cmd": (1, 6),  # 0x7000:02-07 ANGLESET1-6 (array index 1-6)
        "force_cmd": (7, 6),  # 0x7000:08-0D FORCESET1-6 (array index 7-12)
        "speed_cmd": (13, 6),  # 0x7000:0E-13 SPEEDSET1-6 (array index 13-18)
    }

    def __init__(self, lib_path: Optional[str] = None, master_index: int = 0, slave_position: int = 0):
        """
        Initialize the hand controller

        Args:
            lib_path: Path to librh56f1.so (auto-detected if None)
            master_index: EtherCAT master index (default: 0)
            slave_position: EtherCAT slave position (default: 0)
        """
        self.master_index = master_index
        self.slave_position = slave_position
        self._handle = None
        self._output_cache = [0] * 19  # Cache for output values (especially for enable bits)

        # Find library
        if lib_path is None:
            # Import here to avoid circular dependency
            from .recompilation import recompile_if_needed
            lib_path = recompile_if_needed()

        if not os.path.exists(lib_path):
            raise FileNotFoundError(f"Library not found: {lib_path}")

        # Load shared library
        self.lib = ctypes.CDLL(lib_path)

        # Define function prototypes
        self._setup_library_functions()



    def _setup_library_functions(self):
        # rh56f1_create
        self.lib.rh56f1_create.argtypes = [ctypes.c_uint32, ctypes.c_uint32]
        self.lib.rh56f1_create.restype = ctypes.c_void_p

        # rh56f1_start
        self.lib.rh56f1_start.argtypes = [ctypes.c_void_p]
        self.lib.rh56f1_start.restype = ctypes.c_int

        # rh56f1_stop
        self.lib.rh56f1_stop.argtypes = [ctypes.c_void_p]
        self.lib.rh56f1_stop.restype = ctypes.c_int

        # rh56f1_deactivate
        self.lib.rh56f1_deactivate.argtypes = [ctypes.c_void_p]
        self.lib.rh56f1_deactivate.restype = ctypes.c_int

        # rh56f1_activate
        self.lib.rh56f1_activate.argtypes = [ctypes.c_void_p]
        self.lib.rh56f1_activate.restype = ctypes.c_int

        # rh56f1_destroy
        self.lib.rh56f1_destroy.argtypes = [ctypes.c_void_p]
        self.lib.rh56f1_destroy.restype = None

        # rh56f1_get_all_inputs
        self.lib.rh56f1_get_all_inputs.argtypes = [ctypes.c_void_p, ctypes.POINTER(ctypes.c_int16), ctypes.c_int]
        self.lib.rh56f1_get_all_inputs.restype = ctypes.c_int

        # rh56f1_get_input
        self.lib.rh56f1_get_input.argtypes = [ctypes.c_void_p, ctypes.c_int, ctypes.POINTER(ctypes.c_int16)]
        self.lib.rh56f1_get_input.restype = ctypes.c_int

        # rh56f1_set_output
        self.lib.rh56f1_set_output.argtypes = [ctypes.c_void_p, ctypes.c_int, ctypes.c_uint16]
        self.lib.rh56f1_set_output.restype = ctypes.c_int

        # rh56f1_is_running
        self.lib.rh56f1_is_running.argtypes = [ctypes.c_void_p]
        self.lib.rh56f1_is_running.restype = ctypes.c_int

        # rh56f1_get_error
        self.lib.rh56f1_get_error.argtypes = [ctypes.c_void_p]
        self.lib.rh56f1_get_error.restype = ctypes.c_char_p

        # rh56f1_read_sdo
        self.lib.rh56f1_read_sdo.argtypes = [
            ctypes.c_void_p,
            ctypes.c_uint16,
            ctypes.c_uint8,
            ctypes.POINTER(ctypes.c_int16),
        ]
        self.lib.rh56f1_read_sdo.restype = ctypes.c_int

        # rh56f1_write_sdo
        self.lib.rh56f1_write_sdo.argtypes = [ctypes.c_void_p, ctypes.c_uint16, ctypes.c_uint8, ctypes.c_int16]
        self.lib.rh56f1_write_sdo.restype = ctypes.c_int

    # ========== Basic Control Methods ==========

    def initialize(self) -> bool:
        """Initialize the EtherCAT controller. Retries once after 2.5s if master is busy (e.g. after previous Robot aborted)."""
        if self._handle is not None:
            warning("Already initialized")
            return True

        self._handle = self.lib.rh56f1_create(self.master_index, self.slave_position)

        if not self._handle:
            err_msg = self.get_error()
            warning("Initialization failed (master may be busy): {}. Retrying in 2.5s...", err_msg if err_msg else "Unknown error")
            time.sleep(2.5)
            self._handle = self.lib.rh56f1_create(self.master_index, self.slave_position)

        if not self._handle:
            err_msg = self.get_error()
            error("Initialization failed: {}", err_msg if err_msg else "Unknown error")
            return False

        return True

    def start(self) -> bool:
        """Start the real-time control loop"""
        if self._handle is None:
            error("Not initialized. Call initialize() first.")
            return False

        if self.is_running():
            warning("Already running")
            return True

        result = self.lib.rh56f1_start(self._handle)

        if result != 0:
            err_msg = self.get_error()
            error("Start failed: {}", err_msg)
            return False

        return True

    def stop(self) -> bool:
        """Stop the real-time control loop and clear all outputs"""
        if self._handle is None:
            return True

        # Clear outputs before stopping to prevent twitching
        self.clear_all_outputs()

        result = self.lib.rh56f1_stop(self._handle)
        return result == 0

    @property
    def is_initialized(self) -> bool:
        """True if the controller has been initialized and not yet cleaned up."""
        return self._handle is not None

    def cleanup(self):
        """Stop the control loop and deactivate the master, but keep the reservation so start() can be called again without re-requesting the master (avoids 'Device or resource busy' on abort/restart)."""
        if self._handle is not None:
            self.stop()
            self.lib.rh56f1_deactivate(self._handle)

    def destroy(self):
        """Release the EtherCAT master and free resources. Call when the hand will not be used again (e.g. process exit)."""
        if self._handle is not None:
            self.stop()
            self.lib.rh56f1_destroy(self._handle)
            self._handle = None

    # ========== Per-Finger Data Access ==========

    def calculate_array_index(self, finger_id: FingerID, pdo_name: str) -> int:
        
        if not 1 <= finger_id <= 6:
            raise ValueError(f"finger_id must be 1-6, got {finger_id}")

        pdo_entry = self.PDO_MAP[pdo_name]
        # Handle both tuple (start, count) and single integer values
        if isinstance(pdo_entry, tuple):
            start_idx = pdo_entry[0]
        else:
            start_idx = pdo_entry
            
        array_index = start_idx + (finger_id - 1)
        return array_index

    def get_finger_position(self, finger_id: FingerID) -> Optional[int]:
        """
        Returns the actuator position for a specific finger (0x6000:01-06 POSACT)
        """
        array_index = self.calculate_array_index(finger_id, "position")
        value = ctypes.c_int16()
        result = self.lib.rh56f1_get_input(self._handle, array_index, ctypes.byref(value))
        return value.value if result == 0 else None

    def get_finger_angle(self, finger_id: FingerID) -> Optional[int]:
        """Returns the angle of actuator for a specific finger (0x6000:07-0C ANGLEACT) in 0.1 degree units (e.g., 1700 = 170.0 degrees)
        """
        array_index = self.calculate_array_index(finger_id, "angle")
        value = ctypes.c_int16()
        result = self.lib.rh56f1_get_input(self._handle, array_index, ctypes.byref(value))
        return value.value if result == 0 else None  

    def get_finger_force(self, finger_id: FingerID) -> Optional[int]:
        """
        Returns the force value for a specific finger (0x6000:0D-12 FORCEACT)
        """
        array_index = self.calculate_array_index(finger_id, "force")
        value = ctypes.c_int16()
        result = self.lib.rh56f1_get_input(self._handle, array_index, ctypes.byref(value))
        return value.value if result == 0 else None

    def get_finger_current(self, finger_id: FingerID) -> Optional[int]:
        """
        Returns the current for a specific finger (0x6000:13-18 CURACT)
        """
        array_index = self.calculate_array_index(finger_id, "current")
        value = ctypes.c_int16()
        result = self.lib.rh56f1_get_input(self._handle, array_index, ctypes.byref(value))
        return value.value if result == 0 else None

    def get_finger_temperature(self, finger_id: FingerID) -> Optional[int]:
        """
        Returns the temperature in °C for a specific finger (0x6000:25-2A TEMP)
        """
        array_index = self.calculate_array_index(finger_id, "temperature")
        value = ctypes.c_int16()
        result = self.lib.rh56f1_get_input(self._handle, array_index, ctypes.byref(value))
        return value.value if result == 0 else None

    def get_finger_error(self, finger_id: FingerID) -> Optional[int]:
        """
        Returns the error code for a specific finger (0x6000:19-1E ERROR), 0 = no error 
        """
        array_index = self.calculate_array_index(finger_id, "error")
        value = ctypes.c_int16()
        result = self.lib.rh56f1_get_input(self._handle, array_index, ctypes.byref(value))
        return value.value if result == 0 else None

    def get_finger_status(self, finger_id: FingerID) -> Optional[int]:
        """
        Returns the status for a specific finger (0x6000:1F-24 STATUS)
        """
        array_index = self.calculate_array_index(finger_id, "status")
        value = ctypes.c_int16()
        result = self.lib.rh56f1_get_input(self._handle, array_index, ctypes.byref(value))
        return value.value if result == 0 else None

    def get_finger_data(self, finger_id: FingerID) -> Optional[FingerData]:
        """
        Returns FingerData object for a specific finger
        """
        return FingerData(
            finger_id=finger_id,
            position=self.get_finger_position(finger_id) or 0,
            angle=self.get_finger_angle(finger_id) or 0,
            force=self.get_finger_force(finger_id) or 0,
            current=self.get_finger_current(finger_id) or 0,
            temperature=self.get_finger_temperature(finger_id) or 0,
            error=self.get_finger_error(finger_id) or 0,
            status=self.get_finger_status(finger_id) or 0,
        )

    def get_all_fingers_data(self) -> List[FingerData]:
        """
        Returns a list of FingerData objects for all fingers
        """
        return [self.get_finger_data(i) for i in range(1, 7)]

    # ========== Per-Finger Control ==========

    def set_finger_control(self, finger_id: FingerID, angle: int, force: int, speed: int, enable: bool = True) -> bool:
        """
        Control a specific finger

        Args:
            finger_id: FingerID (1-6)
            angle: Target angle (0-3600 = 0-360 degrees in 0.1 degree units)
            force: Force/torque limit (0-1000)
            speed: Speed limit (0-5000)
            enable: Enable motor (default: True)

        Returns:
            True if successful, False otherwise
        """
        if not 1 <= finger_id <= 6:
            raise ValueError(f"finger_id must be 1-6, got {finger_id}")

        if not self.is_running():
            error("Controller not running. Call start() first.")
            return False

        # Clamp values to valid ranges (use per-finger limits for angle)
        min_angle, max_angle = self.FINGER_ANGLE_LIMITS.get(finger_id, (self.ANGLE_MIN, self.ANGLE_MAX))
        angle = max(min_angle, min(max_angle, angle))
        force = max(self.FORCE_MIN, min(self.FORCE_MAX, force))
        speed = max(self.SPEED_MIN, min(self.SPEED_MAX, speed))

        # Calculate output indices for this finger based on actual PDO mapping
        angle_idx = self.PDO_OUTPUT_MAP["angle_cmd"][0] + (finger_id - 1)  # Indices 1-6
        force_idx = self.PDO_OUTPUT_MAP["force_cmd"][0] + (finger_id - 1)  # Indices 7-12
        speed_idx = self.PDO_OUTPUT_MAP["speed_cmd"][0] + (finger_id - 1)  # Indices 13-18

        # Set the values
        success = True
        success &= self.set_output(angle_idx, angle)
        success &= self.set_output(force_idx, force)
        success &= self.set_output(speed_idx, speed)

        # Handle enable - ENABLE_SET is a simple on/off flag (1=on, 0=off), NOT per-finger bits
        if enable:
            success &= self.set_output(self.PDO_OUTPUT_MAP["enable_set"], 1)
        else:
            success &= self.set_output(self.PDO_OUTPUT_MAP["enable_set"], 0)

        return success

    

    def set_all_fingers_control(
        self, angles: List[int], forces: List[int], speeds: List[int], enable: bool = True
    ) -> bool:
        """
        Control all 6 fingers at once

        Args:
            angles: List of 6 target angles [little, ring, middle, index, thumb_bend, thumb_sidesway]
            forces: List of 6 force limits (one per finger)
            speeds: List of 6 speed limits (one per finger)
            enable: Enable all motors (default: True)

        Returns:
            True if all successful, False otherwise
        """
        if len(angles) != 6 or len(forces) != 6 or len(speeds) != 6:
            error("angles, forces, and speeds must each have 6 values")
            return False

        success = True
        for i in range(1, 7):
            success &= self.set_finger_control(i, angles[i - 1], forces[i - 1], speeds[i - 1], enable)

        return success

    def move(
        self,
        open_percent: List[float],
        force: Optional[Union[int, List[int]]] = None,
        speed: Optional[Union[int, List[int]]] = None,
        enable: bool = True,
    ) -> bool:
        """
        Control all 6 fingers by open percentage (0 = fully closed, 1 = fully open).

        Args:
            open_percent: List of 6 values in [0, 1] for [little, ring, middle, index, thumb_bend, thumb_sidesway].
            force: Force limit(s). Single int (0-1000) for all fingers, or list of 6 ints. Default: FORCE_MAX.
            speed: Speed limit(s). Single int (0-5000) for all fingers, or list of 6 ints. Default: SPEED_MAX.
            enable: Enable all motors (default: True).

        Returns:
            True if all successful, False otherwise.
        """
        if len(open_percent) != 6:
            error("open_percent must have 6 values (one per finger)")
            return False

        # Convert percentages to angles using per-finger limits
        angles: List[int] = []
        for finger_id in range(1, 7):
            pct = max(0.0, min(1.0, float(open_percent[finger_id - 1])))
            min_a, max_a = self.FINGER_ANGLE_LIMITS.get(finger_id, (self.ANGLE_MIN, self.ANGLE_MAX))
            angle = int(min_a + pct * (max_a - min_a))
            angles.append(angle)

        # Normalize force: scalar -> list of 6
        if force is None:
            forces = [self.FORCE_MAX] * 6
        elif isinstance(force, (int, float)):
            v = max(self.FORCE_MIN, min(self.FORCE_MAX, int(force)))
            forces = [v] * 6
        else:
            if len(force) != 6:
                error("force must be a single value or a list of 6 values")
                return False
            forces = [max(self.FORCE_MIN, min(self.FORCE_MAX, int(f))) for f in force]

        # Normalize speed: scalar -> list of 6
        if speed is None:
            speeds = [self.SPEED_MAX] * 6
        elif isinstance(speed, (int, float)):
            v = max(self.SPEED_MIN, min(self.SPEED_MAX, int(speed)))
            speeds = [v] * 6
        else:
            if len(speed) != 6:
                error("speed must be a single value or a list of 6 values")
                return False
            speeds = [max(self.SPEED_MIN, min(self.SPEED_MAX, int(s))) for s in speed]

        return self.set_all_fingers_control(angles, forces, speeds, enable)

    def enable_hand(self) -> bool:
        """
        Enable motion control for all fingers. Returns True if successful, False otherwise.
        """

        # ENABLE_SET is a simple on/off flag (1=enabled, 0=disabled) for all fingers
        return self.set_output(self.PDO_OUTPUT_MAP["enable_set"], 1)

    def disable_hand(self) -> bool:
        """
        Disable hand motion control. Returns True if successful, False otherwise.
        """
        # ENABLE_SET is a simple on/off flag (1=enabled, 0=disabled) for all fingers
        return self.set_output(self.PDO_OUTPUT_MAP["enable_set"], 0)

    def clear_all_outputs(self) -> bool:
        """
        Clear all output values and disable motion

        This stops all movement by:
        1. Setting ENABLE_SET to 0 (disable)
        2. Zeroing all angle, force, and speed commands

        Use this when you want to stop the hand from moving.

        Returns:
            True if successful
        """
        success = True

        # Disable motion first
        success &= self.set_output(self.PDO_OUTPUT_MAP["enable_set"], 0)

        # Zero all angle commands (indices 1-6)
        for i in range(1, 7):
            success &= self.set_output(self.PDO_OUTPUT_MAP["angle_cmd"][0] + (i - 1), 0)

        # Zero all force commands (indices 7-12)
        for i in range(1, 7):
            success &= self.set_output(self.PDO_OUTPUT_MAP["force_cmd"][0] + (i - 1), 0)

        # Zero all speed commands (indices 13-18)
        for i in range(1, 7):
            success &= self.set_output(self.PDO_OUTPUT_MAP["speed_cmd"][0] + (i - 1), 0)

        return success

    # ========== Tactile Sensor Access ==========

    def get_tactile_sensor(self, sensor_id: TactileSensorID) -> Optional[TactileSensor]:
        """
        Get tactile sensor data (0x6000:2B-4C)

        Based on PDO table:
        - Finger sensors (1-5): 5 values each (Normal, Tangential, Direction, ProximityL, ProximityH)
        - Palm sensors (6-8): 3 values each (Normal, Tangential, Direction) - NO proximity values

        Args:
            sensor_id: Sensor ID (1-5 for finger sensors, 6-8 for palm sensors)

        Returns:
            TactileSensor object with NormalForce, TangentialForce, Direction, ProximityValue or None
        """
        if not 1 <= sensor_id <= 8:
            raise ValueError(f"sensor_id must be 1-8, got {sensor_id}")
        
        # Mapping: (base_array_index, num_values)
        TACTILE_SENSOR_MAP = {
            1: (42, 5),  # Finger 1: 0x2B-2F
            2: (47, 5),  # Finger 2: 0x30-34
            3: (52, 5),  # Finger 3: 0x35-39
            4: (57, 5),  # Finger 4: 0x3A-3E
            5: (62, 5),  # Finger 5: 0x3F-43
            6: (67, 3),  # Palm 1: 0x44-46
            7: (70, 3),  # Palm 2: 0x47-49
            8: (73, 3),  # Palm 3: 0x4A-4C
        }
        
        base_index, num_values = TACTILE_SENSOR_MAP[sensor_id]
        
        # Check bounds
        if base_index + num_values - 1 > 75:
            warning("Sensor {} is out of PDO bounds (needs indices {}-{}, but PDO only goes to 75).", 
                   sensor_id, base_index, base_index + num_values - 1)
            return None

        # Read values from PDO
        values = []
        for i in range(num_values):
            value = ctypes.c_int16()
            result = self.lib.rh56f1_get_input(self._handle, base_index + i, ctypes.byref(value))
            values.append(value.value if result == 0 else 0)
        
        # Palm sensors don't have proximity values, pad with zeros
        if num_values == 3:
            values.extend([0, 0])  # Add proximity_low=0, proximity_high=0

        return TactileSensor(
            normal_force=values[0],
            tangential_force=values[1],
            direction=values[2],
            proximity_low=values[3] if len(values) > 3 else 0,
            proximity_high=values[4] if len(values) > 4 else 0,
        )

    def get_all_tactile_sensors(self) -> List[TactileSensor]:
        """
        Returns a list of TactileSensor objects for all tactile sensors (5 finger sensors + 3 palm sensors)
        """
        return [self.get_tactile_sensor(i) for i in range(1, 9)]

    # ========== Hand State ==========

    def get_hand_state(self) -> HandState:
        """
        Returns a HandState object with all finger and sensor data
        """
        return HandState(
            fingers=self.get_all_fingers_data(),
            tactile_sensors=self.get_all_tactile_sensors(),
            timestamp=time.time(),
        )

    # ========== Convenience Methods ==========

    def get_all_angles(self) -> List[int]:
        """Get angles for all fingers"""
        return [self.get_finger_angle(i) or 0 for i in range(1, 7)]

    def get_all_forces(self) -> List[int]:
        """Get forces for all fingers"""
        return [self.get_finger_force(i) or 0 for i in range(1, 7)]

    def get_all_temperatures(self) -> List[int]:
        """Get temperatures for all fingers"""
        return [self.get_finger_temperature(i) or 0 for i in range(1, 7)]

    def get_all_currents(self) -> List[int]:
        """Get currents for all fingers"""
        return [self.get_finger_current(i) or 0 for i in range(1, 7)]

    def get_errors(self) -> Dict[int, int]:
        """Get error codes for all fingers (only non-zero errors)"""
        errors = {}
        for i in range(1, 7):
            err = self.get_finger_error(i)
            if err and err != 0:
                errors[i] = err
        return errors

    def print_hand_state(self):
        """Print a formatted view of the complete hand state"""
        state = self.get_hand_state()

        print("=" * 80)
        print(f"RH56F1 Hand State (Timestamp: {state.timestamp:.3f})")
        print("=" * 80)
        
        # Finger data table
        print("\nFinger Data:")
        print("{:4} {:<8} {:<8} {:<8} {:<8} {:<6} {:<8} {:<8}".format("ID", "Pos", "Angle", "Force", "Curr", "Temp", "Err", "Status"))
        print("-" * 80)

        for finger in state.fingers:
            print(str(finger))

        # Tactile sensors
        print("\nTactile Sensors:")
        print("{:<4} {:<10} {:<10} {:<8} {:<10} {:<10}".format("ID", "Normal", "Tangent", "Dir", "ProxL", "ProxH"))
        print("-" * 80)

        for i, sensor in enumerate(state.tactile_sensors, 1):
            sensor_type = "Finger" if i <= 5 else "Palm"
            sensor_str = f"{str(sensor)} ({sensor_type})"
            print("{:<4} {}".format(i, sensor_str))

        print("=" * 80)

    # ========== Raw PDO Access ==========

    def get_all_inputs(self) -> Optional[List[int]]:
        """Get all 76 input values (raw)"""
        values = (ctypes.c_int16 * 76)()
        result = self.lib.rh56f1_get_all_inputs(self._handle, values, 76)
        return list(values) if result > 0 else None

    def get_input(self, index: int) -> Optional[int]:
        """Get specific input value by index (0-75)"""
        if not 0 <= index < 76:
            return None

        value = ctypes.c_int16()
        result = self.lib.rh56f1_get_input(self._handle, index, ctypes.byref(value))
        return value.value if result == 0 else None

    def set_output(self, index: int, value: int) -> bool:
        """Set specific output value by index (0-18)"""
        if 0 <= index < 19:
            self._output_cache[index] = value
        if self._handle is None:
            return True  # Only update cache; avoid calling C library with NULL handle
        result = self.lib.rh56f1_set_output(self._handle, index, ctypes.c_uint16(value))
        return result == 0

    def get_output(self, index: int) -> int:
        """Get cached output value by index (0-18)"""
        if 0 <= index < 19:
            return self._output_cache[index]
        return 0

    def print_output_state(self):
        """
        Print current output values being sent to the device
        """
        print("=" * 80)
        print("Current Output State (values being sent to device):")
        print("=" * 80)

        enable = self.get_output(self.PDO_OUTPUT_MAP["enable_set"])
        enable_str = "ENABLED" if enable == 1 else "DISABLED"
        print("ENABLE_SET (index 0): {} ({})".format(enable, enable_str))
        print("")

        print("Angle Commands (ANGLESET1-6):")
        for i in range(1, 7):
            idx = self.PDO_OUTPUT_MAP["angle_cmd"][0] + (i - 1)
            angle = self.get_output(idx)
            print("  Finger {}: {} ({:.1f}°)".format(i, angle, angle/10))
        print("")

        print("Force Commands (FORCESET1-6):")
        for i in range(1, 7):
            idx = self.PDO_OUTPUT_MAP["force_cmd"][0] + (i - 1)
            force = self.get_output(idx)
            print("  Finger {}: {}".format(i, force))
        print("")

        print("Speed Commands (SPEEDSET1-6):")
        for i in range(1, 7):
            idx = self.PDO_OUTPUT_MAP["speed_cmd"][0] + (i - 1)
            speed = self.get_output(idx)
            print("  Finger {}: {}".format(i, speed))
        print("=" * 80)

    def is_running(self) -> bool:
        """Check if controller is running"""
        if self._handle is None:
            return False
        return self.lib.rh56f1_is_running(self._handle) == 1

    def get_error(self) -> Optional[str]:
        """Get last error message"""
        if self._handle is None:
            return None
        error_ptr = self.lib.rh56f1_get_error(self._handle)
        if error_ptr:
            return error_ptr.decode("utf-8")
        return None

    def read_sdo(self, index: int, subindex: int) -> Optional[int]:
        """
        Read an SDO (Service Data Object) from the device
        """
        value = ctypes.c_int16()
        result = self.lib.rh56f1_read_sdo(
            self._handle, ctypes.c_uint16(index), ctypes.c_uint8(subindex), ctypes.byref(value)
        )
        if result == 0:
            return value.value
        return None

    def write_sdo(self, index: int, subindex: int, value: int) -> bool:
        """
        Write an SDO (Service Data Object) to the device
        """
        result = self.lib.rh56f1_write_sdo(
            self._handle, ctypes.c_uint16(index), ctypes.c_uint8(subindex), ctypes.c_int16(value)
        )
        return result == 0

    def clear_errors(self) -> bool:
        """
        Clear all device errors (SDO 0x2000:03), returns True if successful, False otherwise.
        """
        return self.write_sdo(0x2000, 0x03, 1)

    def wait_for_operational(self, timeout: float = 5.0, check_interval: float = 0.1) -> bool:
        """
        Wait for the device to enter operational state and start providing data

        Args:
            timeout: Maximum time to wait in seconds (default: 5.0)
            check_interval: How often to check for data in seconds (default: 0.1)

        Returns:
            True if device is operational (providing non-zero data), False if timeout

        Note:
            The EtherCAT device typically takes 2-3 seconds to transition through
            INIT → PREOP → SAFEOP → OPERATIONAL states after starting.
        """
        import time

        if not self.is_running():
            return False

        start_time = time.time()

        while time.time() - start_time < timeout:
            # Check if we're getting any non-zero data
            all_inputs = self.get_all_inputs()
            if all_inputs and any(v != 0 for v in all_inputs[:42]):  # Check first 42 values (all finger data)
                return True
            time.sleep(check_interval)

        return False

    # ========== Context Manager ==========

    def __enter__(self):
        """Context manager entry"""
        if not self.initialize():
            raise RuntimeError("Failed to initialize hand controller")
        if not self.start():
            raise RuntimeError("Failed to start hand controller")
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        """Context manager exit: release master when leaving the with block."""
        self.destroy()

    def __del__(self):
        """Destructor: release master when the hand object is garbage-collected."""
        if self._handle is not None:
            self.destroy()
