#!/usr/bin/env python3
"""
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
from pathlib import Path
from dataclasses import dataclass


@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


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

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


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

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


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 = {
        1: (900, 1740),  # Little finger: 90° - 174°
        2: (900, 1740),  # Ring finger: 90° - 174°
        3: (900, 1740),  # Middle finger: 90° - 174°
        4: (900, 1740),  # Index finger: 90° - 174°
        5: (1100, 1450),  # Thumb bending: 110° - 145°
        6: (600, 1800),  # Thumb rotation: 60° - 180°
    }

    # Finger ID constants for readability
    FINGER_LITTLE = 1
    FINGER_RING = 2
    FINGER_MIDDLE = 3
    FINGER_INDEX = 4
    FINGER_THUMB_BEND = 5
    FINGER_THUMB_SIDESWAY = 6

    # 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:
            lib_path = self._find_library()

        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 _find_library(self) -> str:
        """Auto-detect library path"""
        possible_paths = [
            Path(__file__).parent / "librh56f1.so",
            Path(__file__).parent / "build" / "librh56f1.so",
            "/usr/local/lib/librh56f1.so",
            "/usr/lib/librh56f1.so",
        ]

        for path in possible_paths:
            if path.exists():
                return str(path)

        raise FileNotFoundError("librh56f1.so not found. Please build the library first using 'make'")

    def _setup_library_functions(self):
        """Setup ctypes function prototypes"""
        # 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_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"""
        if self._handle is not None:
            print("Warning: Already initialized")
            return True

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

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

        return True

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

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

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

        if result != 0:
            error = self.get_error()
            print(f"Start failed: {error}")
            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

    def cleanup(self):
        """Clean up and release resources"""
        if self._handle is not None:
            self.lib.rh56f1_destroy(self._handle)
            self._handle = None

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

    def get_finger_position(self, finger_id: int) -> Optional[int]:
        """
        Get actual actuator position for a specific finger (0x6000:01-06 POSACT)

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            Position value or None
        """
        if not 1 <= finger_id <= 6:
            return None

        start_idx, count = self.PDO_MAP["position"]
        array_index = start_idx + (finger_id - 1)

        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: int) -> Optional[int]:
        """
        Get actual angle of actuator for a specific finger (0x6000:07-0C ANGLEACT)

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            Angle in 0.1 degree units (e.g., 1700 = 170.0 degrees)
        """
        if not 1 <= finger_id <= 6:
            return None

        start_idx, count = self.PDO_MAP["angle"]
        array_index = start_idx + (finger_id - 1)

        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: int) -> Optional[int]:
        """
        Get actual force for a specific finger (0x6000:0D-12 FORCEACT)

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            Force value or None
        """
        if not 1 <= finger_id <= 6:
            return None

        start_idx, count = self.PDO_MAP["force"]
        array_index = start_idx + (finger_id - 1)

        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: int) -> Optional[int]:
        """
        Get actual current for a specific finger (0x6000:13-18 CURACT)

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            Current value or None
        """
        if not 1 <= finger_id <= 6:
            return None

        start_idx, count = self.PDO_MAP["current"]
        array_index = start_idx + (finger_id - 1)

        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: int) -> Optional[int]:
        """
        Get temperature for a specific finger (0x6000:25-2A TEMP)

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            Temperature in °C or None
        """
        if not 1 <= finger_id <= 6:
            return None

        start_idx, count = self.PDO_MAP["temperature"]
        array_index = start_idx + (finger_id - 1)

        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: int) -> Optional[int]:
        """
        Get error code for a specific finger (0x6000:19-1E ERROR)

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            Error code (0 = no error) or None
        """
        if not 1 <= finger_id <= 6:
            return None

        start_idx, count = self.PDO_MAP["error"]
        array_index = start_idx + (finger_id - 1)

        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: int) -> Optional[int]:
        """
        Get status word for a specific finger (0x6000:1F-24 STATUS)

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            Status word or None
        """
        if not 1 <= finger_id <= 6:
            return None

        start_idx, count = self.PDO_MAP["status"]
        array_index = start_idx + (finger_id - 1)

        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: int) -> Optional[FingerData]:
        """
        Get all data for a specific finger

        Args:
            finger_id: Finger ID (1-6)

        Returns:
            FingerData object with all finger information
        """
        if not 1 <= finger_id <= 6:
            return None

        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]:
        """
        Get data for all 6 fingers

        Returns:
            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: int, angle: int, force: int, speed: int, enable: bool = True) -> bool:
        """
        Control a specific finger

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)
            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

        Example:
            # Move finger 2 to 170 degrees with force limit 800 and speed 2000
            hand.set_finger_control(finger_id=2, angle=1700, force=800, speed=2000)
        """
        if not 1 <= finger_id <= 6:
            print(f"Error: finger_id must be 1-6, got {finger_id}")
            return False

        if not self.is_running():
            print("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

        Example:
            # Different angle for each finger (little->thumb sidesway)
            angles = [1000, 1500, 1700, 1200, 1800, 1600]
            forces = [800] * 6  # Same force for all
            speeds = [2000] * 6  # Same speed for all
            hand.set_all_fingers_control(angles, forces, speeds)
        """
        if len(angles) != 6 or len(forces) != 6 or len(speeds) != 6:
            print("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 enable_finger(self, finger_id: int) -> bool:
        """
        Enable motion control (0x7000:01 ENABLE_SET = 1)

        Note: ENABLE_SET is a global flag - this enables motion for ALL fingers.

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            True if successful, False otherwise
        """
        if not 1 <= finger_id <= 6:
            return False

        # 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_finger(self, finger_id: int) -> bool:
        """
        Disable motion control (0x7000:01 ENABLE_SET = 0)

        Note: ENABLE_SET is a global flag - this disables motion for ALL fingers.

        Args:
            finger_id: Finger ID (1=little, 2=ring, 3=middle, 4=index, 5=thumb bend, 6=thumb sidesway)

        Returns:
            True if successful, False otherwise
        """
        if not 1 <= finger_id <= 6:
            return False

        # 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 enable_all_fingers(self) -> bool:
        """
        Enable motion control (0x7000:01 ENABLE_SET = 1)

        Note: Based on MyIODemo.cpp, ENABLE_SET is a simple on/off flag, NOT a bitmask.
              Set to 1 to enable motion, 0 to disable.
        """
        return self.set_output(self.PDO_OUTPUT_MAP["enable_set"], 1)

    def disable_all_fingers(self) -> bool:
        """Disable motion control (0x7000:01 ENABLE_SET = 0)"""
        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: int) -> Optional[TactileSensor]:
        """
        Get tactile sensor data (0x6000:2B-4C)

        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:
            return None

        # Each tactile sensor has 5 values: Normal, Tangential, Direction, ProximityL, ProximityH
        base_index = self.PDO_MAP["tactile"] + (sensor_id - 1) * 5

        values = []
        for i in range(5):
            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)

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

    def get_all_tactile_sensors(self) -> List[TactileSensor]:
        """
        Get all tactile sensor data

        Returns:
            List of TactileSensor objects (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:
        """
        Get complete hand state with all finger and sensor data

        Returns:
            HandState object with complete hand information
        """
        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(f"{'ID':<4} {'Pos':<8} {'Angle':<8} {'Force':<8} {'Curr':<8} {'Temp':<8} {'Err':<6} {'Status':<8}")
        print("-" * 80)

        for finger in state.fingers:
            print(
                f"{finger.finger_id:<4} "
                f"{finger.position:<8} "
                f"{finger.angle:<8} "
                f"{finger.force:<8} "
                f"{finger.current:<8} "
                f"{finger.temperature:<8} "
                f"{finger.error:<6} "
                f"{finger.status:<8}"
            )

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

        for i, sensor in enumerate(state.tactile_sensors, 1):
            sensor_type = "Finger" if i <= 5 else "Palm"
            print(
                f"{i:<4} "
                f"{sensor.normal_force:<10} "
                f"{sensor.tangential_force:<10} "
                f"{sensor.direction:<8} "
                f"{sensor.proximity_low:<10} "
                f"{sensor.proximity_high:<10} "
                f"({sensor_type})"
            )

        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
        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

        Useful for debugging twitching or unexpected movement.
        """
        print("=" * 80)
        print("Current Output State (values being sent to device):")
        print("=" * 80)

        enable = self.get_output(self.PDO_OUTPUT_MAP["enable_set"])
        print(f"ENABLE_SET (index 0): {enable} ({'ENABLED' if enable == 1 else 'DISABLED'})")
        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(f"  Finger {i}: {angle} ({angle/10:.1f}°)")
        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(f"  Finger {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(f"  Finger {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

        Args:
            index: Object index (e.g., 0x2000)
            subindex: Subindex (e.g., 0x01)

        Returns:
            Value read or None on error

        Example:
            # Read HAND_ID
            hand_id = hand.read_sdo(0x2000, 0x01)
        """
        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

        Args:
            index: Object index (e.g., 0x2000)
            subindex: Subindex (e.g., 0x01)
            value: Value to write

        Returns:
            True on success, False on error

        Example:
            # Clear errors
            hand.write_sdo(0x2000, 0x03, 1)
        """
        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 on success, False on error
        """
        return self.write_sdo(0x2000, 0x03, 1)

    def get_finger_mode(self, finger_id: int) -> Optional[int]:
        """
        Read the motion mode for a specific finger (SDO 0x2000:1B-20)

        Args:
            finger_id: Finger ID (1-6)

        Returns:
            Mode value or None on error
        """
        if not 1 <= finger_id <= 6:
            return None
        subindex = 0x1A + finger_id  # 0x1B for finger 1, 0x1C for finger 2, etc.
        return self.read_sdo(0x2000, subindex)

    def set_finger_mode(self, finger_id: int, mode: int) -> bool:
        """
        Set the motion mode for a specific finger (SDO 0x2000:1B-20)

        Args:
            finger_id: Finger ID (1-6)
            mode: Mode value

        Returns:
            True on success, False on error
        """
        if not 1 <= finger_id <= 6:
            return False
        subindex = 0x1A + finger_id  # 0x1B for finger 1, 0x1C for finger 2, etc.
        return self.write_sdo(0x2000, subindex, mode)

    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"""
        self.stop()
        self.cleanup()

    def __del__(self):
        """Destructor"""
        if self._handle is not None:
            self.cleanup()


def main():
    """Demo of per-finger control and data access"""
    import argparse

    parser = argparse.ArgumentParser(description="RH56F1 Hand Controller")
    parser.add_argument("-d", "--duration", type=float, default=10.0, help="Run duration in seconds")
    parser.add_argument("--finger", type=int, choices=range(1, 7), help="Show data for specific finger")
    parser.add_argument("--all", action="store_true", help="Show all hand data")
    args = parser.parse_args()

    if os.geteuid() != 0:
        print("Warning: This program requires root privileges")
        print("Try running with: sudo python3 rh56f1_hand.py")
        return 1

    try:
        with RH56F1Hand() as hand:
            print("RH56F1 Hand Controller Demo")
            print("=" * 80)
            time.sleep(5.0)  # Wait for device to enter OPERATIONAL state

        start_time = time.time()

        while time.time() - start_time < args.duration:
            if args.finger:
                # Show specific finger data
                finger_data = hand.get_finger_data(args.finger)
                if finger_data:
                    print(
                        f"\nFinger {args.finger}: "
                        f"Angle={finger_data.angle}, "
                        f"Force={finger_data.force}, "
                        f"Temp={finger_data.temperature}, "
                        f"Current={finger_data.current}"
                    )

            elif args.all:
                # Show complete hand state
                os.system("clear")  # Clear screen for better display
                hand.print_hand_state()

            else:
                # Show summary
                angles = hand.get_all_angles()
                forces = hand.get_all_forces()
                temps = hand.get_all_temperatures()

                print(f"\nAngles: {angles}")
                print(f"Forces: {forces}")
                print(f"Temps:  {temps}")

                errors = hand.get_errors()
                if errors:
                    print(f"Errors: {errors}")

            time.sleep(0.5)  # Update every 500ms

        return 0

    except Exception as e:
        print(f"Error: {e}")
        import traceback

        traceback.print_exc()
        return 1


if __name__ == "__main__":
    import sys

    sys.exit(main())
