import time
from typing import Optional
from ..motors.damiao.damaio_motor import DamiaoMotor
from ..motors.damiao.utils import FeedbackFrameInfo
from ..battery_system import BatterySystem
from feathersdk.utils.logger import info, warning, debug



class I2RTGripper:
    """I2RT Gripper interface for high-level gripper control.
    
    This class provides high-level control methods for the gripper, delegating
    all firmware communication to the DamiaoMotor class.
    """

    def __init__(
        self,
        motor: DamiaoMotor,
        power: BatterySystem = None,
    ):
        """Initialize the I2RT Gripper and determine healthy state.

        Args:
            motor (DamiaoMotor): DamiaoMotor instance containing motor configuration and firmware interface.
            power (BatterySystem, optional): BatterySystem instance for power state tracking. Defaults to None.
        """
        self.motor = motor
        self.power = power
        self.healthy_state = False
        
        self.motor_id = self.motor.motor_id
        self.motor_name = self.motor.motor_name

        self.enable()

        if self.motor.calibration_time is not None and self.motor.iface is not None and self.power is not None:
            last_powered_up_time = self.power.last_powered_up_time()
            if last_powered_up_time > self.motor.calibration_time:
                self.healthy_state = False
                info(f"{self.motor.motor_name} needs to be recalibrated in order to use position control.")
            else:
                self.healthy_state = self.motor.check_in_range()
               
    

    def enable(self) -> FeedbackFrameInfo:
        """Enable the gripper motor.

        Returns:
            FeedbackFrameInfo: Current motor state after enabling.
        """
        return self.motor.motor_on()

    def clear_error(self) -> None:
        """Clear motor error state.
        """
        return self.motor.clear_error()

    def disable(self) -> None:
        """Disable the gripper motor.
        """
        return self.motor.motor_off()

    def hold_current_position(self, kp: float, kd: float) -> FeedbackFrameInfo:
        """Hold the gripper at its current position using position control.
        
        Args:
            kp (float): Position gain for position control.
            kd (float): Damping gain for position control.
            
        Returns:
            FeedbackFrameInfo: Current motor state after setting hold command.
        """
        position = self.motor.get_state().position
        return self.motor.set_control(
            pos=position,
            vel=0.0,
            kp=kp,
            kd=kd,
            torque=0.0
        )

    def get_state(self) -> FeedbackFrameInfo:
        """Get the current gripper state without enabling the motor.
        
        Returns:
            FeedbackFrameInfo: Current motor state containing position, velocity, torque, and temperatures.
        """
        # Get state without enabling motor - uses set_control with all zeros
        return self.motor.get_state()
        
    
    def go_to_position(self, position: float, kp: float, kd: float) -> FeedbackFrameInfo:
        """Move gripper to a specific position using position control.
        
        Args:
            position (float): Target position in radians.
            kp (float): Position gain.
            kd (float): Damping gain. Must be greater than KD_MIN.
        
        Raises:
            Exception: If gripper is not in a healthy state (needs recalibration).
            ValueError: If position is out of valid range or gains are invalid.
        """
        if not self.healthy_state:
            raise Exception("Please recalibrate gripper.")
        if self.motor.lower_limit is not None and self.motor.upper_limit is not None:
            if position < self.motor.lower_limit or position > self.motor.upper_limit:
                raise ValueError(f"Position {position} is out of range {self.motor.lower_limit} to {self.motor.upper_limit}")
        if kd <= self.motor.motor_constants.KD_MIN or kd > self.motor.motor_constants.KD_MAX:
            raise ValueError(f"Damping gain must be greater than {self.motor.motor_constants.KD_MIN} and less or equal to {self.motor.motor_constants.KD_MAX}. Got {kd}")
        if kp < self.motor.motor_constants.KP_MIN or kp > self.motor.motor_constants.KP_MAX:
            raise ValueError(f"Position gain must be between {self.motor.motor_constants.KP_MIN} and {self.motor.motor_constants.KP_MAX}. Got {kp}")
        
        return self.motor.set_control(pos=position, vel=0.0, kp=kp, kd=kd, torque=0.0)

    def apply_constant_torque(self, torque: float, kd: float) -> FeedbackFrameInfo:
        """Apply constant torque to the gripper in the desired direction.
        
        Args:
            torque (float): Target torque in Nm. Positive for closing, negative for opening.
            kd (float): Damping gain for torque control.
        
        Raises:
            Exception: If damping gain or torque values are outside valid ranges.
        """
        if kd < self.motor.motor_constants.KD_MIN or kd > self.motor.motor_constants.KD_MAX:
            raise Exception(f"Damping gain must be between {self.motor.motor_constants.KD_MIN} and {self.motor.motor_constants.KD_MAX}. Got {kd}")
        if torque < self.motor.motor_constants.TORQUE_MIN or torque > self.motor.motor_constants.TORQUE_MAX:
            raise Exception(f"Opening torque must be between {self.motor.motor_constants.TORQUE_MIN} and {self.motor.motor_constants.TORQUE_MAX}. Got {torque}")
        return self.motor.set_control(pos=0.0, vel=0.0, kp=0.0, kd=kd, torque=torque)


    def apply_torque_and_hold_position(
        self,
        torque: float,
        kd: float,
        holding_kp: float,
        holding_kd: float,
        verbose: bool = False
    ) -> FeedbackFrameInfo:
        """Apply torque until velocity drops below threshold, then hold position.
        
        Applies constant torque until the gripper reaches a break condition (low velocity
        for 1 second or velocity < 0.05 with sufficient torque), then switches to position
        control to hold the current position.
        
        Args:
            torque (float): Torque to apply in Nm. Positive for closing, negative for opening.
            kd (float): Damping gain for torque control phase.
            holding_kp (float): Position gain for holding position after break condition.
            holding_kd (float): Damping gain for holding position after break condition.
            verbose (bool): Enable verbose debug output. Defaults to False.
        
        Raises:
            ValueError: If damping gain or torque values are outside valid ranges.
        """

        if kd < self.motor.motor_constants.KD_MIN or kd > self.motor.motor_constants.KD_MAX:
            raise ValueError(f"Damping gain must be between {self.motor.motor_constants.KD_MIN} and {self.motor.motor_constants.KD_MAX}. Got {kd}")
        if torque < self.motor.motor_constants.TORQUE_MIN or torque > self.motor.motor_constants.TORQUE_MAX:
            raise ValueError(f"Opening torque must be between {self.motor.motor_constants.TORQUE_MIN} and {self.motor.motor_constants.TORQUE_MAX}. Got {torque}")
        # Move to Max limit
        low_velocity_start_time = None
        while True:
            motor_info = self.motor.set_control(
                pos=0.0,
                vel=0.0,
                kp=0.0,
                kd=kd,  # can add damping here
                torque=torque
            )
            if verbose:
                debug(f"torque: {motor_info.torque:.4f} Nm")
                debug(f"velocity: {motor_info.velocity:.4f} rad/s")
                debug(f"position: {motor_info.position:.4f} rad")
                debug("")
            
            # Track when velocity first drops below 0.05
            if abs(motor_info.velocity) < 0.05:
                if low_velocity_start_time is None:
                    low_velocity_start_time = time.time()
                # Check if velocity has been low for more than 1 second
                elif time.time() - low_velocity_start_time >= 1.0:
                    # Velocity has been low for 1 second, hold and break even if torque condition not met
                    if verbose:
                        debug("Velocity below 0.05 for 1 second, holding position")
                        debug(f"Holding position: {motor_info.position:.4f} rad")
                        debug(f"torque: {motor_info.torque:.4f} Nm")
                        debug(f"velocity: {motor_info.velocity:.4f} rad/s")
                        debug(f"temperature: {motor_info.temperature_mos:.3f}°C")
                    
                    # Hold position
                    self.hold_current_position(kp=holding_kp, kd=holding_kd)
                    break
            else:
                # Velocity is not low, reset the timer
                low_velocity_start_time = None
            
            # Break condition: should end with pos torque, stop when velocity is close to 0
            if abs(motor_info.velocity) < 0.05 and abs(motor_info.torque) >= abs(torque):
                
                if verbose:
                    debug("reached break condition")
                    debug(f"MAX position reached, holding position: {motor_info.position:.4f} rad")
                    debug(f"torque: {motor_info.torque:.4f} Nm")
                    debug(f"velocity: {motor_info.velocity:.4f} rad/s")
                    debug(f"temperature: {motor_info.temperature_mos:.3f}°C")

                # Hold position
                time.sleep(1)
                return self.hold_current_position(kp=holding_kp, kd=holding_kd)
                

    def recalibrate(self) -> None:
        """Recalibrate the gripper by finding full range of motion.
        
        Opens gripper to find minimum position, saves zero position, closes gripper
        to find maximum position, saves calibration limits, and returns to open position.
        Sets healthy_state to True upon successful completion.
        
        Raises:
            Exception: If any step of the calibration process fails, including invalid
                calibration limits or errors during movement.
        """
        info("=== Starting gripper recalibration ===")
        
        try:
            # Open gripper to find minimum position
            self.apply_torque_and_hold_position(torque=-0.5, kd=0.2, holding_kp=5.0, holding_kd=0.5)
            
            #wait for 1 second
            info("Zeroing position...")
            # Save zero position
            self.motor.save_zero_position()
            time.sleep(1)
            motor_info = self.get_state()
            lower_limit = motor_info.position

            info("Closing gripper...")
            # Close gripper to find maximum position
            self.apply_torque_and_hold_position(torque=0.5, kd=0.2, holding_kp=5.0, holding_kd=0.5, verbose=True)
            time.sleep(1)
            motor_info = self.get_state()
            upper_limit = motor_info.position

            # Validate limits
            if lower_limit >= upper_limit:
                raise Exception(f"Invalid calibration limits: lower_limit ({lower_limit}) >= upper_limit ({upper_limit})")

            info(f"lower_limit: {lower_limit}, upper_limit: {upper_limit}, total_range: {abs(upper_limit - lower_limit)}")
            #write upper and lower limits to calibration file
            self.motor.save_calibration_state(lower_limit=lower_limit, upper_limit=upper_limit, calibration_homing_pos=0, total_range=abs(upper_limit - lower_limit))
            self.healthy_state = True

            #go to open position
            self.go_to_position(position=lower_limit, kp=1.0, kd=0.5)

            info("=== Gripper recalibration complete ===")
        except Exception as e:
            self.healthy_state = False
            error_msg = f"Gripper recalibration failed: {str(e)}"
            raise Exception(error_msg) from e
