from typing import List, Optional

from .steppable_system import SteppableSystem
from .motors.robstride import RobstrideMotor
from .motors.motors_manager import MotorsManager

from .rh56dftp_hand import create_finger_joints_from_config
from .rh56f1_hand import RH56F1Hand
from .gripper.i2rt_gripper import I2RTGripper
from .motors.damiao.damaio_motor import DamiaoMotor
from .battery_system import BatterySystem
from .arm_system import ArmSystem, _confirm_calibration
from .hand_system import HandSystem
from .gripper_system import GripperSystem

# Motor calibration order: bottom-most (wrist) to top-most (shoulder)
# Physically: wrist is near the hand (bottom), shoulder connects to torso (top)
# This ensures stable calibration as lower joints are calibrated first
RIGHT_ARM_CALIBRATION_ORDER = ["WwR", "WpR", "WrR", "EpR", "SpR", "SrR", "SwR"]
LEFT_ARM_CALIBRATION_ORDER = ["WwL", "WpL", "WrL", "EpL", "SpL", "SrL", "SwL"]

class BimanualManipulationPlatform(SteppableSystem):
    def __init__(self, motors_manager: MotorsManager, config: dict, power: BatterySystem = None, manipulator_config: dict = {}):

        self.cfg = config
        self.manipulator_cfg = manipulator_config
        self.motors_manager = motors_manager
        self.power = power
        self.operating_frequency = 100

        # Initialize arms from config or with empty motors map
        self.right_arm_motors = {}
        if self.cfg.get("right_arm", False):
            for motor_name, motor_dict in self.cfg["right_arm"]["motors"].items():
                self.right_arm_motors[motor_name] = RobstrideMotor(
                    int(motor_dict["id"], 16),
                    motor_name,
                    motor_dict["motor_config"],
                )
            
        self.left_arm_motors = {}
        if self.cfg.get("left_arm", False):
            for motor_name, motor_dict in self.cfg["left_arm"]["motors"].items():
                self.left_arm_motors[motor_name] = RobstrideMotor(
                    int(motor_dict["id"], 16),
                    motor_name,
                    motor_dict["motor_config"],
                )
                
        self.right_arm = ArmSystem(motors_manager, self.right_arm_motors, "right_arm", RIGHT_ARM_CALIBRATION_ORDER)
        self.left_arm = ArmSystem(motors_manager, self.left_arm_motors, "left_arm", LEFT_ARM_CALIBRATION_ORDER)

        # Initialize hands and grippers from manipulator config
        self.right_manipulator = None
        self.left_manipulator = None
        self._initialize_manipulators()


    def _initialize_manipulators(self):
        if self.manipulator_cfg != {}:
            # Detect manipulator type from config class or by checking available sections
            manipulator_class = self.manipulator_cfg.get("class", "").lower()
            
            # Check for RH56DFTP hands
            if "rh56dftp" in manipulator_class:
                # Initialize right hand if section exists
                right_finger_joints = create_finger_joints_from_config(self.manipulator_cfg, "right")
                if right_finger_joints:
                    self.right_manipulator = HandSystem(self.motors_manager, right_finger_joints, "right_hand")
            
                # Initialize left hand if section exists
                left_finger_joints = create_finger_joints_from_config(self.manipulator_cfg, "left")
                if left_finger_joints:
                    self.left_manipulator = HandSystem(self.motors_manager, left_finger_joints, "left_hand")
        
            if "rh56f1" in manipulator_class:
                print("Initializing RH56F1 hands...")
                # Get config for right and left hands
                right_hand_cfg = self.manipulator_cfg.get("rh56f1_hand_right", {})
                left_hand_cfg = self.manipulator_cfg.get("rh56f1_hand_left", {})
                
                self.right_manipulator = RH56F1Hand(
                    master_index=right_hand_cfg.get("master_index", 0),
                    slave_position=right_hand_cfg.get("slave_position", 0)
                )
                self.left_manipulator = RH56F1Hand(
                    master_index=left_hand_cfg.get("master_index", 1),
                    slave_position=left_hand_cfg.get("slave_position", 0)
                )
                self.right_manipulator.initialize()
                self.left_manipulator.initialize()
                self.right_manipulator.start()
                self.left_manipulator.start()

                #wait for hands to be operational
                left_operational = self.left_manipulator.wait_for_operational(timeout=10.0, check_interval=0.1)
                right_operational = self.right_manipulator.wait_for_operational(timeout=10.0, check_interval=0.1)

                if left_operational and right_operational:
                    print("Both RH56F1 hands are operational!")
                else:
                    print(f"Warning: Left operational={left_operational}, Right operational={right_operational}")

            # Check for I2RT grippers
            if "linear4310" in manipulator_class:
                print("Initializing I2RT Linear 4310 grippers...")
                #pass in each arm iface to the gripper if exists, otherwise use can0
                right_can_iface = self.motors_manager.motors["SpR"].iface
                left_can_iface = self.motors_manager.motors["SpL"].iface 
                # Initialize right gripper if section exists
                right_gripper_cfg = self.manipulator_cfg.get("i2rt_gripper_right", {})
                if right_gripper_cfg and "motors" in right_gripper_cfg:
                    right_gripper_motors = {}
                    for motor_name, motor_dict in right_gripper_cfg["motors"].items():
                        if motor_dict.get("type") == "damiao":
                            motor_id = int(motor_dict["id"])
                            motor_config = motor_dict.get("motor_config", {})
                            
                            # Create DamiaoMotor instance
                            damiao_motor = DamiaoMotor(
                                motor_name=motor_name,
                                motor_id=motor_id,
                                config=motor_config,
                                iface=right_can_iface,
                            )
                            
                            # Create I2RTGripper instance
                            i2rt_gripper = I2RTGripper(motor=damiao_motor, power=self.power)
                            right_gripper_motors[motor_name] = i2rt_gripper
                    
                    if right_gripper_motors:
                        self.right_manipulator = GripperSystem(
                            motors_manager=self.motors_manager,
                            motors_map=right_gripper_motors,
                            name="right_gripper",
                        )
                
                # Initialize left gripper if section exists
                left_gripper_cfg = self.manipulator_cfg.get("i2rt_gripper_left", {})
                if left_gripper_cfg and "motors" in left_gripper_cfg:
                    left_gripper_motors = {}
                    for motor_name, motor_dict in left_gripper_cfg["motors"].items():
                        if motor_dict.get("type") == "damiao":
                            motor_id = int(motor_dict["id"])
                            motor_config = motor_dict.get("motor_config", {})
                            
                            # Create DamiaoMotor instance
                            damiao_motor = DamiaoMotor(
                                motor_name=motor_name,
                                motor_id=motor_id,
                                config=motor_config,
                                iface=left_can_iface,
                            )
                            # Create I2RTGripper instance
                            i2rt_gripper = I2RTGripper(motor=damiao_motor, power=self.power)
                            left_gripper_motors[motor_name] = i2rt_gripper
                    
                    if left_gripper_motors:
                        self.left_manipulator = GripperSystem(
                            motors_manager=self.motors_manager,
                            motors_map=left_gripper_motors,
                            name="left_gripper",
                        )
        
    def enable_arms(self, enable_right: bool = True, enable_left: bool = True):
        if enable_right:
            for motor_name in self.right_arm.motors.keys():
                self.motors_manager.enable(motor_name)
        if enable_left:
            for motor_name in self.left_arm.motors.keys():
                self.motors_manager.enable(motor_name)

    def disable_arms(self, disable_right: bool = True, disable_left: bool = True):
        if disable_right:
            for motor_name in self.right_arm.motors.keys():
                self.motors_manager.disable(motor_name)
        if disable_left:
            for motor_name in self.left_arm.motors.keys():
                self.motors_manager.disable(motor_name)
    
    def capture_position(self):
        for motor_name in self.right_arm.motors.keys():
            print(motor_name, self.motors_manager.read_param_sync(motor_name, "mech_pos"))
        for motor_name in self.left_arm.motors.keys():
            print(motor_name, self.motors_manager.read_param_sync(motor_name, "mech_pos"))

    def recalibrate(
        self,
        calibrate_right: bool = True,
        calibrate_left: bool = True,
        motor_names: Optional[List[str]] = None,
        step_time: float = 0.01,
        verbose: bool = False
    ) -> None:
        """Recalibrate arm motors.
        
        Calibrates motors from bottom-most (wrist) to top-most (shoulder) for each arm.
        
        Args:
            calibrate_right: Whether to calibrate right arm (default True)
            calibrate_left: Whether to calibrate left arm (default True)
            motor_names: Optional list of specific motors to calibrate.
                        If None, calibrates all motors in selected arms.
            step_time: Delay between steps (default 0.01s)
            verbose: Enable debug output (default False)
            
        Raises:
            Exception: If user cancels calibration
        """
        if not calibrate_right and not calibrate_left:
            print("No arms selected for calibration.")
            return
            
        if not _confirm_calibration("arms"):
            raise Exception("Calibration cancelled by user")
        
        if motor_names is not None:
            # Split motor names by arm
            right_motors = [m for m in motor_names if m in self.right_arm.motors]
            left_motors = [m for m in motor_names if m in self.left_arm.motors]            
        else:
            right_motors = self.right_arm.motors.keys()
            left_motors = self.left_arm.motors.keys()

        if calibrate_right:
            self.right_arm.recalibrate(
                motor_names=right_motors,
                step_time=step_time,
                verbose=verbose,
                _skip_confirmation=True
            )
        if calibrate_left:
            self.left_arm.recalibrate(
                motor_names=left_motors,
                step_time=step_time,
                verbose=verbose,
                _skip_confirmation=True
            )

    def is_calibrated(self, check_right: bool = True, check_left: bool = True) -> bool:
        """Check if arms are calibrated.
        
        Args:
            check_right: Check right arm calibration state
            check_left: Check left arm calibration state
            
        Returns:
            True if all specified arms are calibrated
        """
        if check_right and not self.right_arm.is_calibrated():
            return False
        if check_left and not self.left_arm.is_calibrated():
            return False
        return True

    def get_state(self) -> dict:
        """Get current state of all motors in the manipulation platform.
        
        Returns:
            Dict mapping motor name to its state dict containing:
                - 'angle': current angle in radians
                - 'velocity': current velocity in radians/sec
            Includes motors from both arms and both hands (if present).
        """
        state = {}
        
        # Collect arm states
        state.update(self.right_arm.get_state())
        state.update(self.left_arm.get_state())
        
        return state

    def _step(self):
        pass

