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.

        Args:
            motor: DamiaoMotor instance containing motor configuration and firmware interface.
        """
        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_motor()

        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_motor(self) -> FeedbackFrameInfo:
        """
        Returns:
            FeedbackFrameInfo: Current motor state.
        """
        return self.motor.motor_on()

    def clear_error(self) -> None:
        return self.motor.clear_error()

    def disable_motor(self) -> None:
        return self.motor.motor_off()

    def hold_current_position(self, kp: float, kd: float) -> FeedbackFrameInfo:
        """Hold the gripper at the current position.
        
        Args:
            kp: Position gain.
            kd: Damping gain.
            
        Returns:
            FeedbackFrameInfo: Current motor state.
        """
        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.
            position: current position in radians
            velocity: current velocity in rad/s
            torque: current torque in Nm
            temperature_mos: MOSFET temperature in °C
            temperature_rotor: rotor temperature in °C
        """
        # 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) -> None:
        """Go to a specific position. Checks if within range and validates gains.
        motor must be calibrated and in a healthy state.
        
        Args:
            position: Target position in radians.
            kp: Position gain.
            kd: Damping gain. Must not be set to 0.
        """
        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}")
        
        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) -> None:
        """Control the gripper using torque. Applies constant torque in desired direction.
        
        Args:
            torque: Target torque in Nm. Positive for closing, negative for opening.
            kd: Damping gain.
            kp: Position gain.
        """
        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}")
        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
    ) -> None:
        """Close gripper using torque control.
        
        Applies torque until the velocity is close to 0, 
        then holds that position.
        
        Args:
            torque: Magnitude of torque to apply in Nm. Positive for closing, negative for opening.
            kd: Damping gain [0, 5].
            holding_kp: Position gain for holding position.
            holding_kd: Damping gain for holding position.
            verbose: Print verbose output.
        """

        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)
                self.hold_current_position(kp=holding_kp, kd=holding_kd)
                break


    def clear_error(self) -> None:
        return self.motor.clear_error()

    def recalibrate(self) -> None:
        """Recalibrate the gripper. 
        
        Goes to closed position, saves zero position, then goes to open position
        to determine the full range of motion.
        
        Raises:
            Exception: If any step of the calibration process fails.
        """
        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
