from feathersdk.comms import CommsManager
import time
from typing import Optional


class InspireFingerJoint:
    """
    Represents a single finger joint in the RH56DFTP Inspire hand.
    """

    def __init__(self, motor_id: int, ip_address: str, motor_name: str, force_set_offset: int, default_max_force: int):
        """
        Initialize a finger joint.
        
        Args:
            motor_id: The motor ID for this finger joint
            ip_address: IP address of the hand controller (can include port, e.g., "192.168.11.210:6000")
            motor_name: Name of the motor
            force_set_offset: Offset for force set command (default: 12)
        """
        self.motor_id = motor_id
        self.motor_name = motor_name
        self.ip_address = ip_address
        self.force_set_offset = force_set_offset
        self.properties = {}
        self.comms = CommsManager()
        if self.comms.is_running():
            self.set_max_force(default_max_force)

    def move_percentage(self, percentage: float):
        """
        Move the finger joint. 0% is fully closed, 100% is fully open.
        
        Args:
            percentage: Percentage of movement (0.0 to 1.0)
        """
        self.comms.tcpsend_modbus(self.ip_address, 1, 255, 6, self.motor_id, int(percentage * 1000))
        time.sleep(0.01)

    def set_max_force(self, force: int):
        """
        Set the max force for the finger joint.
        
        Args:
            force: Maximum force value
        """
        # Convert motor_id from bytes to int, add offset, then convert back to bytes
        force_set_id = int(self.motor_id) + self.force_set_offset
        self.comms.tcpsend_modbus(self.ip_address, 1, 255, 6, force_set_id, force)
        time.sleep(0.005)


def create_finger_joints_from_config(config: dict, hand_side: str) -> dict:
    """
    Create finger joints from a config dictionary.
    
    Args:
        config: Configuration dictionary containing hand motor definitions
        hand_side: "right" or "left" to specify which hand
        force_set_offset: Offset for force set command (default: 12)
        
    Returns:
        Dictionary mapping motor names to InspireFingerJoint objects
    """
    force_set_offset = config.get("constants", {}).get("RH56DFTP_FORCE_OFFSET", 12)
    default_max_force = config.get("constants", {}).get("RH56DFTP_DEFAULT_MAX_FORCE", 500)
    hand_key = f"rh56dftp_hand_{hand_side}"
    if hand_key not in config:
        return {}
    
    hand_config = config[hand_key]
    if "motors" not in hand_config:
        return {}
    
    finger_joints = {}
    for motor_name, motor_info in hand_config["motors"].items():
        finger_joints[motor_name] = InspireFingerJoint(
            motor_id=motor_info["motor_id"],
            ip_address=motor_info["ip_address"],
            motor_name=motor_info["motor_name"],
            force_set_offset=force_set_offset,
            default_max_force=default_max_force
        )
        
    return finger_joints
