from typing import List

# ================================
# Tactile Sensor Interface
# ================================

# Tactile sensor data
# Three-dimensional force values, self-approach capacitance, mutual-approach capacitance, and sensor status
class TouchFingerItem:
    normal_force1: int # Normal force 1，Unit 0.01N，For example, 1000 represents 1000 * 0.01, which is 10 N。The measurement range of the normal force is 0 ~ 25 N
    normal_force2: int # Normal force 2，Unit 0.01N
    normal_force3: int # Normal force 3，Unit 0.01N
    tangential_force1: int # Tangential Force 1，Unit 0.01N，For example, 1000 represents 1000 * 0.01, which is 10 N。The measurement range of the tangential force is 0 ~ 25 N
    tangential_force2: int # Tangential Force 2，Unit 0.01N
    tangential_force3: int # Tangential Force 3，Unit 0.01N
    tangential_direction1: int # Tangential force direction 1, numerical range is 0 ~ 359 degrees。The direction towards the fingertips is 0 degrees. It can be rotated clockwise up to a maximum of 359 degrees. When the value is 65535 (0xFFFF), it indicates that the tangential force direction is invalid.
    tangential_direction2: int # Tangential Force Direction 2
    tangential_direction3: int # Tangential Force Direction 3
    self_proximity1: int # Self-capacitance 1
    self_proximity2: int # Self-capacitance 2
    mutual_proximity: int # Mutual capacitance
    status: int # Status
    description: str # Describe
    def is_normal(self) -> bool: ...
    def is_abnormal(self) -> bool: ...
    def desc(self) -> str: ...

class TouchFingerData:
    items: list[TouchFingerItem]

# Tactile raw data
class TouchRawData:
    thumb: list[int]
    index: list[int]
    middle: list[int]
    ring: list[int]
    pinky: list[int]
    description: str
    def desc(self) -> str: ...

"""
This document demonstrates the dedicated interface for tactile sensors, applicable to the Tactile Edition of the first and second generation dexterous hands。

Tactile Sensor Function：
- Multidimensional Force Sensing: Normal Force, Tangential Force, Tangential Force Direction
- Proximity Detection: Self-Capacitance and Mutual-Capacitance Detection
- Contact status recognition: No contact, static contact, sliding contact
- Real-time data stream: supports high-frequency data collection and callback

Please confirm the device type before use：
    device_info = device.get_device_info(slave_id=1)
    if device_info.is_touch():
        print("The device supports haptic feedback ")
        if device_info.is_revo1_touch():
            print("Revo1 Touch - Generation One Dexterous Hand Tactile Version ")
        elif device_info.is_revo2_touch():
            print("Revo2 Touch - Second-Generation Dextrous Hand Tactile Version ")

Tactile Sensor Usage Process：
1. Check if the device supports haptic feedback
2. Enable and initialize the tactile sensor
3. Acquire tactile data
4. Process tactile data and implement corresponding control logic
5. Calibrate zero drift and reset the tactile sensor
"""

def get_touch_sensor_enabled(self, slave_id: int) -> bool:
    """
    Get whether the tactile sensor is enabled

    Args:
        slave_id: Device Slave ID

    Returns:
        bool: True means enabled, False means disabled

    Note:
        - The tactile sensor is defaultly turned off to save power consumption
        - Tactile data can only be obtained after enabling it.
        - Suitable for all touch-enabled devices

    Example:
        if device.get_device_info(slave_id=1).is_touch():
            enabled = device.get_touch_sensor_enabled(slave_id=1)
            if not enabled:
                print("The tactile sensor is not enabled. Please enable it first. ")
            else:
                print("Tactile sensor has been enabled ")
    """
    pass

def get_touch_sensor_fw_versions(self, slave_id: int) -> List[str]:
    """
    Get the list of firmware version numbers for the tactile sensor

    Args:
        slave_id: Device Slave ID

    Returns:
        List[str]: List of firmware version numbers for each finger touch sensor

    Note:
        - The tactile sensor needs to be enabled first
        - The sensors on each finger may have different firmware versions
        - Version information helps with problem diagnosis and compatibility confirmation

    Example:
        if device.get_touch_sensor_enabled(slave_id=1):
            versions = device.get_touch_sensor_fw_versions(slave_id=1)
            finger_names = ["Thumb ", " Index finger ", " Middle finger ", " Ring finger ", " Little finger "]

            for i, version in enumerate(versions):
                if i < len(finger_names):
                    print(f"{finger_names[i]} Tactile Sensor Firmware Version: {version}")
        else:
            print("Please enable the tactile sensor first ")
    """
    pass

def touch_sensor_setup(self, slave_id: int, bits: int):
    """
    Enable and initialize the tactile sensor system

    Args:
        slave_id: Device Slave ID
        bits: Sensor bit mask, specifying the sensors to be enabled
              - Use bitwise operations to specify which fingers to enable
              - For example: 0x1F (0b11111) represents enabling all 5 fingers
              - For example: 0x03 (0b00011) indicates that only the thumb and index finger are enabled.

    Sensor Bit Mask Description:
        - Bit 0: Thumb
        - Bit 1: Index finger
        - Bit 2: Middle finger
        - Bit 3: Ring finger
        - Bit 4: Little finger

    Note:
        - This is the first step in using a tactile sensor
        - After enabling, the sensor will perform initialization and self-check.
        - The initialization process

    Example:
        # Enable the tactile sensors of all fingers
        device.touch_sensor_setup(slave_id=1, bits=0x1F)
        print("All tactile sensors have been enabled ")

        # Only enable the tactile sensors of the thumb and index finger
        device.touch_sensor_setup(slave_id=1, bits=0x03)
        print("The tactile sensor on the thumb and index finger has been enabled ")

        # Only enable the tactile sensor of the index finger
        device.touch_sensor_setup(slave_id=1, bits=0x02)
        print("The index finger tactile sensor has been enabled ")
    """
    pass

def touch_sensor_reset(self, slave_id: int, bits: int):
    """
    Reset the specified tactile sensor

    Args:
        slave_id: Device Slave ID
        bits: Sensor bit mask, specifying the sensors to be reset

    Note:
        - During reset, the sensor status register will continuously return an abnormal state.
        - After reset, the sensor will resume normal operation
        - Used to resolve sensor anomalies or reinitialize
        - Resetting will not change the enabled status of the sensor

    Example:
        # Reset all tactile sensors
        device.touch_sensor_reset(slave_id=1, bits=0x1F)
        print("All tactile sensors have been reset ")

        # Reset only the thumb sensor (if an anomaly is detected)
        device.touch_sensor_reset(slave_id=1, bits=0x01)
        print("The thumb tactile sensor has been reset ")
    """
    pass

def touch_sensor_calibrate(self, slave_id: int, bits: int):
    """
    Perform tactile sensor zero-drift calibration

    Args:
        slave_id: Device Slave ID
        bits: Sensor bit mask, specifying the sensors to be calibrated

    Calibration Notes:
        - When calibrating, make sure your fingers do not touch any objects
        - The calibration process will eliminate the zero-drift error of the sensor
        - It is recommended to perform calibration after the ambient temperature stabilizes
        - After calibration is completed, the sensor accuracy will be significantly improved

    Recommended calibration timing:
        - When using the device for the first time
        - When the ambient temperature changes significantly
        - When sensor readings show a significant offset
        - During regular maintenance

    Example:
        # Calibrate all tactile sensors
        print("Start calibrating the tactile sensor, please make sure your fingers are not in contact with any objects...")
        device.touch_sensor_calibrate(slave_id=1, bits=0x1F)
        print("Tactile sensor calibration completed ")

        # Calibrate the thumb sensor only
        device.touch_sensor_calibrate(slave_id=1, bits=0x01)
        print("Thumb tactile sensor calibration completed ")
    """
    pass

def get_touch_raw_data(self, slave_id: int) -> TouchRawData:
    """
    Get raw data from the tactile sensor（Mainly used for Revo1 Touch）

    Args:
        slave_id: Device Slave ID

    Returns:
        : Tactile raw data object, which contains the following properties：
            - thumb: List[int] - Thumb original sensor data
            - index: List[int] - Index finger raw sensor data
            - middle: List[int] - Middle finger original sensor data
            - ring: List[int] - Raw sensor data of the ring finger
            - pinky: List[int] - Little finger original sensor data
            - description: str - Data Description

        TouchRawData The provided method：
            - desc() -> str: Get data description

    Note:
        - Only applicable to touch-enabled devices, primarily Revo1 Touch
        - The tactile sensor needs to be enabled first
        - The original data consists of unprocessed sensor readings that need to be parsed manually.
        - Data format and meaning may vary depending on the sensor version

    Example:
        if device.get_device_info(slave_id=1).is_revo1_touch():
            if device.get_touch_sensor_enabled(slave_id=1):
                raw_data = device.get_touch_raw_data(slave_id=1)

                print(f" Thumb raw data: {raw_data.thumb}")
                print(f" Index finger raw data: {raw_data.index}")
                print(f" Data Description: {raw_data.desc()}")

                # Example of raw data processing
                if len(raw_data.thumb) > 0:
                    avg_thumb = sum(raw_data.thumb) / len(raw_data.thumb)
                    print(f" Thumb average: {avg_thumb}")
    """
    pass


def get_single_touch_sensor_status(self, slave_id: int, index: int) -> TouchFingerItem:
    """
    Get tactile sensor data for the specified finger

    Args:
        slave_id: Device Slave ID
        index: Finger index (0-4, corresponding to thumb to little finger)

    Note:
        - The tactile sensor of the corresponding finger needs to be enabled first

    Example:
        if device.get_device_info(slave_id=1).is_revo2_touch():
            # Get thumb tactile status
            thumb_status = device.get_single_touch_sensor_status(slave_id=1, index=0)
    """
    pass

def get_touch_sensor_status(self, slave_id: int) -> TouchFingerData:
    """
    Get tactile sensor data for all fingers

    Args:
        slave_id: Device Slave ID

    Returns:
        TouchFingerData

    Note:
        - A single call retrieves the tactile status of all fingers, resulting in higher efficiency
        - Suitable for applications that require the integrated processing of tactile information from multiple fingers

    Example:
        if device.get_device_info(slave_id=1).is_revo2_touch():
            touch_data = device.get_touch_sensor_status(slave_id=1)
            finger_names = ["Thumb ", " Index finger ", " Middle finger ", " Ring finger ", " Little finger "]
    """
    pass


def check_touch_sensor_health(self, slave_id: int) -> dict:
    """
    Check the health status of the tactile sensor (utility function)

    Args:
        slave_id: Device Slave ID

    Returns:
        dict: Sensor Health Status Report

    Example:
        health_report = self.check_touch_sensor_health(slave_id=1)
        print(f" Health Status Report: {health_report}")
    """
    health_report = {
        "enabled": False,
        "fw_versions": [],
        "sensor_status": [],
        "errors": []
    }

    try:
        # Check if tactile support is available
        device_info = self.get_device_info(slave_id)
        if not device_info.is_touch():
            health_report["errors"].append("The device does not support haptic feedback ")
            return health_report

        # Check if enabled
        touch_enabled = self.get_touch_sensor_enabled(slave_id)
        if not touch_enabled:
            health_report["errors"].append("Tactile sensor is not enabled ")
            return health_report

        # Get firmware version
        health_report["fw_versions"] = self.get_touch_sensor_fw_versions(slave_id)

        # Check the status of each sensor
        touch_data = self.get_modulus_touch_data(slave_id)
        finger_names = ["Thumb ", " Index finger ", " Middle finger ", " Ring finger ", " Little finger "]

        for i, item in enumerate(touch_data.items):
            finger_name = finger_names[i] if i < len(finger_names) else f" Finger {i}"
            status = {
                "finger": finger_name,
                "normal": item.is_normal(),
                "description": item.desc()
            }
            health_report["sensor_status"].append(status)

            if not item.is_normal():
                health_report["errors"].append(f"{finger_name} Sensor anomaly: {item.desc()}")

    except Exception as e:
        health_report["errors"].append(f" An error occurred during the check: {str(e)}")

    return health_report

# ================================
# Using example
# ================================

def touch_sensor_basic_example():
    """
    Basic Usage Example of Tactile Sensor
    """
    slave_id = 1

    device = modbus_open("/dev/ttyUSB0", Baudrate.Baud115200)

    # Check if the device supports haptics
    device_info = device.get_device_info(slave_id)
    if not device_info.is_touch():
        print("The device does not support haptic feedback ")
        return

    print(f" The device supports haptic features: {device_info.sku_type}")

    # Enable the tactile sensor
    print("Enable the tactile sensor...")
    device.touch_sensor_setup(slave_id, bits=0x1F)  # Enable all fingers

    # Wait for initialization to complete
    import time
    time.sleep(2)

    # Check if enabled successfully
    if device.get_touch_sensor_enabled(slave_id):
        print("Tactile sensor enabled successfully ")

        # Get firmware version
        versions = device.get_touch_sensor_fw_versions(slave_id)
        print(f" Sensor Firmware Version: {versions}")

        # Perform calibration
        print("Perform calibration（Please ensure that your fingers are not touching any objects）...")
        device.touch_sensor_calibrate(slave_id, bits=0x1F)
        time.sleep(3)
        print("Calibration completed ")

    else:
        print("Tactile sensor activation failed ")
        return

    # Data Acquisition Loop
    print("Start acquiring tactile data...")
    try:
        while True:
            finger_names = ["Thumb ", " Index finger ", " Middle finger ", " Ring finger ", " Little finger "]
            # Obtain three-dimensional force data
            touch_data = device.get_touch_sensor_status(slave_id)
            # Get the original data
            raw_data = device.get_touch_raw_data(slave_id)


    except KeyboardInterrupt:
        print("\\nStop acquiring tactile data")