from dataclasses import dataclass
import math
import os
import time
import threading

from feathersdk.comms import CanOverloadError, CommsManager, MotorUIDType
from feathersdk.comms.system import S_uint16le
from feathersdk.utils.common import currtime, make_exception_pickleable
from feathersdk.utils.constants import MOTORS_CALIBRATION_PATH
from feathersdk.utils.feathertypes import *
from feathersdk.utils.files import read_json_file, write_json_file
from feathersdk.utils.logger import info, warning
from feathersdk.utils.math_utils import degrees_to_radians

from ..motor import Motor
from .admin import RobstrideAdminControl
from .params import *
from .params import _ROBSTRIDE_PARAMS

if TYPE_CHECKING:  # Due to circular import
    from ..motors_manager import MotorsManager, CalibrationWarnings


T = TypeVar("T")

@dataclass
class TimestampedValue(Generic[T]):
    value: T
    timestamp: float

    def __iter__(self):
        return iter((self.value, self.timestamp))

    def __len__(self):
        return 2

    def __getitem__(self, i: int):
        return (self.value, self.timestamp)[i]
    
    def update(self, value: T, timestamp: Optional[float] = None) -> None:
        """Update the value and timestamp of the TimestampedValue."""
        self.value = value
        self.timestamp = timestamp if timestamp is not None else currtime()


@make_exception_pickleable
class UnsafeCommandError(Exception):
    """Raised when attempting to execute an unsafe command."""
    def __init__(self, message: str, motor: 'RobstrideMotor'):
        super().__init__(message)
        self.motor = motor
        self.motor_name = motor.motor_name


@make_exception_pickleable
class MotorDisabledError(Exception):
    """Raised when attempting to command a motor that is disabled (in Reset mode)."""
    def __init__(self, message: str, motor: 'RobstrideMotor'):
        super().__init__(message)
        self.motor = motor
        self.motor_name = motor.motor_name


@make_exception_pickleable
class MotorModeInconsistentError(Exception):
    """Raised when attempting to command a motor with an incompatible run mode."""
    def __init__(self, message: str, motor: 'RobstrideMotor', curr_mode: RobstrideRunMode, exp_mode: RobstrideRunMode):
        super().__init__(message)
        self.motor = motor
        self.motor_name = motor.motor_name
        self.current_mode = curr_mode
        self.expected_mode = exp_mode


_MOTOR_NO_DEFAULT = object()


class RobstrideMotorConfig(TypedDict, total=False):
    version: RobstrideVersionType
    max_torque: float  # Max torque in N
    max_position_dx: float  # positional difference in radians
    joint_limits: Tuple[float, float]  # joint limits in radians
    max_velocity: float  # Max velocity in radians / seconds


class RobstrideMotor(Motor):
    """Class to control a Robstride motor.
    
    Args:
        motor_id: The motor ID to use for the communication.
        name: The name of the motor.
        config: The configuration for the motor.
        iface: The interface to use for the communication.
    """
    # Class-level list of feedback listeners - empty list is falsy for fast check
    _feedback_listeners: List[Callable[['RobstrideMotor', float], None]] = []

    # Class-level list of command listeners - notified when loc_ref or spd_ref is written
    _command_listeners: List[Callable[['RobstrideMotor', str, float, float], None]] = []

    @classmethod
    def add_feedback_listener(cls, listener: Callable[['RobstrideMotor', float], None]) -> None:
        """Register a callback to receive feedback events after each update_feedback call."""
        cls._feedback_listeners.append(listener)

    @classmethod
    def remove_feedback_listener(cls, listener: Callable[['RobstrideMotor', float], None]) -> None:
        """Unregister a feedback listener."""
        cls._feedback_listeners.remove(listener)

    @classmethod
    def add_command_listener(
        cls, listener: Callable[['RobstrideMotor', str, float, float], None]
    ) -> None:
        """Register a callback for target position/velocity commands (loc_ref, spd_ref)."""
        cls._command_listeners.append(listener)

    @classmethod
    def remove_command_listener(
        cls, listener: Callable[['RobstrideMotor', str, float, float], None]
    ) -> None:
        """Unregister a command listener."""
        cls._command_listeners.remove(listener)

    def __init__(self, motor_id: int, name: str, config: RobstrideMotorConfig = {}, iface: Optional[str] = None):
        if "model" in config and "version" not in config:
            config["version"] = config["model"]
        if "version" not in config:
            raise KeyError("\"version\" key is required for RobstrideMotorConfig")
        self.version = get_robstride_version(config["version"])

        if motor_id < 0 or motor_id >= 0xFE:
            raise ValueError(f"Error: motor_id must be between [0, 0xFD], got {motor_id}")

        self.manager: Optional['MotorsManager'] = None

        self.comms = CommsManager()
        self.motor_id: int = motor_id
        self.default_host_id: int = 0xAA
        self.uid: MotorUIDType = MotorUIDType(self.motor_id)
        self.motor_name: str = name
        self.calibration_time = None
        self._homing_pos = config.get("homing_pos", 0)
        self.direction = config.get("direction", 1)
        self.enable_lock = threading.Lock()

        self.range = config.get("range", {
            "min": -math.pi / 2,
            "max": math.pi / 2,
        })

        self.upper_limit = None
        self.lower_limit = None
        # calibration_homing_pos stores the actual encoder value where the motor's 
        # homing position is located after calibration. This may have a small residual 
        # offset from 0 due to motor drift after zeroing.
        # This value is used as an internal offset for motor commands and calibrated_angle.
        self.calibration_homing_pos = None
        self.total_range = None
        self.last_kp = config.get("starting_kp", 30)
        self.last_kd = config.get("starting_kd", 5)
        self._should_monitor = False
        
        self.calibrated_angle = (0, -1)
        self.dual_encoder: bool = self._get_dual_encoder_property()

        self.family_name: Union[str, None] = None
        self.iface: Union[str, None] = iface
        self.properties: Dict[str, TimestampedValue] = {}
        self.joint_limits = config.get("joint_limits", None)

        if self.joint_limits is not None:
            if (
                self.joint_limits[0] < -math.pi
                or self.joint_limits[1] > math.pi
                and self.joint_limits[0] <= self.joint_limits[1]
            ):
                print("Warning: Joint limits must be from -pi to pi for ", name)
        self.max_torque = config.get("max_torque", None)
        self.max_position_dx = config.get("max_position_dx", None)
        self.max_velocity = config.get("max_velocity", None)
        self.run_mode = RobstrideRunMode.Operation
        self.compliance_mode: bool = False
        self._disable_compliance_mode: bool = False
        # Target position is the target angle to reach. It's relative to angle.
        self.target_position = 0
        self.last_compliance_pos = 0
        self.compliance_mode_torque_threshold = config.get("compliance_mode_torque_threshold", 1.0)
        self.compliance_mode_dx = config.get("compliance_mode_dx", 0.01)

        self.mode: TimestampedValue[RobstrideMotorMode] = TimestampedValue(RobstrideMotorMode.Reset, -1)
        # Angle is the raw angle from the encoder.
        self.angle: TimestampedValue[float] = TimestampedValue(0, -1)
        # Calibrated angle is around 0 that should be used for URDFs. It is the angle - calibration_homing_pos.
        self.calibrated_angle: TimestampedValue[float] = TimestampedValue(0, -1)
        self.velocity: TimestampedValue[float] = TimestampedValue(0, -1)
        self.torque: TimestampedValue[float] = TimestampedValue(0, -1)
        self.temp: TimestampedValue[float] = TimestampedValue(0, -1)
        self.errors: TimestampedValue[List[RobstrideMotorError]] = TimestampedValue([], -1)

        # Feedback delay detection
        self._last_command_time: float = 0.0  # Time when last feedback-triggering command was sent
        self._feedback_delay_threshold: float = config.get("feedback_delay_threshold", 1.0)
        self._feedback_delay_detected: bool = False

        self.load_calibration_state()

        self._admin: RobstrideAdminControl = RobstrideAdminControl(self)

    def set_monitor(self, should_monitor: bool = True) -> None:
        """Set whether to monitor the motor."""
        self._should_monitor = should_monitor

    def is_runable(self) -> bool:
        """ Returns True if the motor is a state where it's ready to listen to and execute motion commands. """
        return self.mode[0] == RobstrideMotorMode.Run and not self.compliance_mode

    def _get_dual_encoder_property(self) -> bool:
        if self.version == RobstrideVersion.RS01:
            return False
        elif self.version in [RobstrideVersion.RS00, RobstrideVersion.RS02, RobstrideVersion.RS03,
                              RobstrideVersion.RS04, RobstrideVersion.RS05, RobstrideVersion.RS06]:
            return True
        else:
            raise NotImplementedError(f"Unknown motor version for dual_encoder logic: {self.version}")

    @property
    def expected_range(self) -> float:
        return self.range["max"] - self.range["min"]

    def _record_command_sent(self) -> None:
        """Record that a command expecting feedback was just sent."""
        self._last_command_time = currtime()

    def _log_feedback_delay(self, delay_seconds: float) -> None:
        """Log warning message when feedback delay is detected."""
        warning(
            f"FEEDBACK DELAY: Motor '{self.motor_name}' (ID: {self.motor_id}) "
            f"delayed {delay_seconds:.3f}s (threshold: {self._feedback_delay_threshold}s)"
        )

    def _log_robstride_error_changes(
        self, new_errors: Iterable[RobstrideMotorError], source: str
    ) -> None:
        """Log when drive-reported fault bits appear or clear (compared to previous stored errors)."""
        if not new_errors and not self.errors[0]:
            return

        prior = set(self.errors[0]) if self.errors[0] else set()
        new_set = set(new_errors) if new_errors else set()

        if prior == new_set:
            return

        added = new_set - prior
        removed = prior - new_set
        if added:
            names = ", ".join(e.name for e in sorted(added, key=lambda e: e.value))
            warning(
                f"Robstride motor '{self.motor_name}' (id=0x{self.motor_id:02x}) [{source}]: "
                f"fault(s) active: {names}"
            )
        if removed:
            names = ", ".join(e.name for e in sorted(removed, key=lambda e: e.value))
            warning(
                f"Robstride motor '{self.motor_name}' (id=0x{self.motor_id:02x}) [{source}]: "
                f"fault(s) cleared: {names}"
            )

    @property
    def feedback_delay_detected(self) -> bool:
        """Returns True if feedback delay was detected on the last command."""
        return self._feedback_delay_detected

    def get_pending_feedback_time(self) -> float:
        """Returns time since last command awaiting feedback, or 0 if none pending."""
        if self._last_command_time <= 0:
            return 0.0
        return currtime() - self._last_command_time

    def check_feedback_timeout(self, threshold: Optional[float] = None) -> Optional[float]:
        """Check if waiting for feedback that hasn't arrived within threshold.
        
        Args:
            threshold: Optional timeout threshold in seconds. If not provided,
                uses the motor's configured _feedback_delay_threshold.
        
        Returns:
            Delay in seconds if timeout detected, None otherwise.
        """
        if self._last_command_time <= 0:
            return None
        elapsed = currtime() - self._last_command_time
        effective_threshold = threshold if threshold is not None else self._feedback_delay_threshold
        if elapsed > effective_threshold:
            self._log_feedback_delay(elapsed)
            self._feedback_delay_detected = True
            return elapsed
        return None

    @property
    def homing_pos(self) -> float:
        """Get the encoder value for the homing position.
        
        Returns the calibration_homing_pos plus any config-defined offset (_homing_pos).
        After calibrate_motor is called, calibration_homing_pos contains the actual 
        encoder value at homing, which may have a small residual offset from 0.
        
        Returns:
            The encoder value to command when moving to homing position
        """
        if self.calibration_homing_pos is not None:
            return self.calibration_homing_pos + self._homing_pos
        return self._homing_pos

    def update_angle(self, angle: float):
        self.angle = TimestampedValue(angle, currtime())
        if self.calibration_homing_pos is not None:
            self.calibrated_angle = TimestampedValue(self.convert_angle_to_calibrated_angle(angle), currtime())
        else:
            self.calibrated_angle = TimestampedValue(angle, currtime())

    def convert_calibrated_angle_to_angle(self, calibrated_angle: float) -> float:
        if self.calibration_homing_pos is None:
            return calibrated_angle
        return calibrated_angle + self.calibration_homing_pos

    def convert_angle_to_calibrated_angle(self, angle: float) -> float:
        if self.calibration_homing_pos is None:
            return angle
        return angle - self.calibration_homing_pos

    def update_feedback(
        self, angle: float, velocity: float, torque: float, temp: float, errors: List[RobstrideMotorError], mode: RobstrideMotorMode
    ):
        last_update = currtime()
        
        # Check for feedback delay
        if self._last_command_time > 0:
            feedback_delay = last_update - self._last_command_time
            if feedback_delay > self._feedback_delay_threshold:
                self._log_feedback_delay(feedback_delay)
                self._feedback_delay_detected = True
            else:
                self._feedback_delay_detected = False
            self._last_command_time = 0.0  # Clear pending command
        
        # Raw angle coming from the encoder.
        self.update_angle(angle)
        self.velocity = TimestampedValue(velocity, last_update)
        self.torque = TimestampedValue(torque, last_update)
        self.temp = TimestampedValue(temp, last_update)
        self._log_robstride_error_changes(set(errors) if errors else set(), "motor_feedback")
        self.errors = TimestampedValue(errors, last_update)
        self.mode = TimestampedValue(mode, last_update)

        # Event notification - empty list check is O(1) and falsy
        if RobstrideMotor._feedback_listeners:
            for listener in RobstrideMotor._feedback_listeners:
                listener(self, last_update)

    def update_property(self, property_name: str, value: Union[float, int]):
        self.properties[property_name] = TimestampedValue(value, currtime())
    
    def get_property_value(self, property_name: str, default: Any = _MOTOR_NO_DEFAULT) -> Any:
        """Get the currently stored value for a property in this motor. 
        
        If the property is not found, return the default value. If the default value is not provided, raise a KeyError.
        """
        if property_name in self.properties:
            return self.properties[property_name].value
        if default is not _MOTOR_NO_DEFAULT:
            return default
        raise KeyError(f"Property {property_name} not found in motor {self.motor_name}")

    def get_property_timestamp(self, property_name: str, default: Any = _MOTOR_NO_DEFAULT) -> Any:
        """Get the timestamp of the currently stored value for a property in this motor. 
        
        If the property is not found, return the default value. If the default value is not provided, raise a KeyError.
        """
        if property_name in self.properties:
            return self.properties[property_name].timestamp
        if default is not _MOTOR_NO_DEFAULT:
            return default
        raise KeyError(f"Property {property_name} not found in motor {self.motor_name}")

    def is_safe_position_update(self, target_position: float) -> bool:
        if self.max_position_dx is not None:
            return abs(target_position - self.angle[0]) <= self.max_position_dx
            # if not abs(target_position - self.angle[0]) <= self.max_position_dx:
            #     return abs(target_position - self.target_position) <= self.max_position_dx
            # else:
            #     return True
        return True

    def update_target_position(self, target_position: float, from_compliance_mode: bool = False) -> None:
        """
        Used for safety purposes.
        If max_position_dx is set, the motor will be stopped if the position is outside the max_position_dx.
        """
        if self.compliance_mode and not from_compliance_mode:
            raise UnsafeCommandError(message="Commands currently blocked - in compliance mode", motor=self)

        self.target_position = target_position

    def should_trigger_compliance_mode(self):
        if self._disable_compliance_mode:
            return False

        if self.compliance_mode or self.mode[0] == RobstrideMotorMode.Reset:
            return False

        if self.max_position_dx is not None:
            if abs(self.angle[0] - self.target_position) > self.max_position_dx:
                warning(
                    f"Compliance mode triggered: Motor {self.motor_name} Position {self.angle[0]} is outside max position dx {self.max_position_dx} Target position {self.target_position}"
                )
                return True

        if self.max_torque is not None:
            if abs(self.torque[0]) > self.max_torque:
                warning(
                    f"Compliance mode triggered: Motor {self.motor_name} Torque {self.torque[0]} is greater than max torque {self.max_torque}"
                )
                return True

        if self.joint_limits is not None:
            if self.calibrated_angle[0] > self.joint_limits[1] or self.calibrated_angle[0] < self.joint_limits[0]:
                warning(
                    f"Compliance mode triggered: Motor {self.motor_name} Angle {self.calibrated_angle[0]} is outside joint limits {self.joint_limits}"
                )
                return True

        if self.max_velocity is not None:
            if abs(self.velocity[0]) > self.max_velocity:
                warning(
                    f"Compliance mode triggered: Motor {self.motor_name} Velocity {self.velocity[0]} is greater than max velocity {self.max_velocity}"
                )
                return True

        return False

    def _disable_compliance_mode_for_calibration(self):
        """Disable compliance mode triggering during calibration."""
        self._disable_compliance_mode = True

    def _enable_compliance_mode_after_calibration(self):
        """Re-enable compliance mode triggering after calibration."""
        self._disable_compliance_mode = False

    def save_calibration_state(self, lower_limit: float, upper_limit: float, calibration_homing_pos: float, total_range: float):
        """Set the calibration data for this motor.
        
        Args:
            lower_limit: The encoder value at the lower physical limit
            upper_limit: The encoder value at the upper physical limit
            calibration_homing_pos: The encoder value at the homing position after calibration.
                This may have a small residual offset from 0 due to motor drift after zeroing.
            total_range: The total range of motion in radians
        """
        self._disable_compliance_mode = False
        # we must use time.time() here instead of monotonic because
        # we need to compare this with system power up time which is more inline with time.time().
        self.calibration_time = time.time()
        self.lower_limit = lower_limit
        self.upper_limit = upper_limit
        self.calibration_homing_pos = calibration_homing_pos
        self.total_range = total_range

        data = {
            "calibration_time": self.calibration_time,
            "lower_limit": self.lower_limit,
            "upper_limit": self.upper_limit,
            "calibration_homing_pos": self.calibration_homing_pos,
            "total_range": self.total_range,
        }
        write_json_file(data, MOTORS_CALIBRATION_PATH + "/" + self.motor_name + ".json")

    def load_calibration_state(self) -> bool:
        """Load the calibration state from the file.
        
        Returns True if the calibration state was loaded successfully, False otherwise.
        Returns False if the file doesn't exist or uses the old calibration format
        (middle_pos instead of calibration_homing_pos), requiring recalibration.
        """
        file_path = MOTORS_CALIBRATION_PATH + "/" + self.motor_name + ".json"
        if not os.path.exists(file_path):
            return False
        data = read_json_file(file_path)
        # Check for new calibration format - return False if using old format
        if "calibration_homing_pos" not in data:
            return False
        self.calibration_time = data["calibration_time"]
        self.lower_limit = data["lower_limit"]
        self.upper_limit = data["upper_limit"]
        self.calibration_homing_pos = data["calibration_homing_pos"]
        self.total_range = data["total_range"]
        return True

    def _calibration_checks(self) -> 'CalibrationWarnings':
        """Check calibration values and return any warnings.
        
        This method should be called after a motor is calibrated to validate
        that the calibration values are within expected tolerances.
        
        Returns:
            CalibrationWarnings flags indicating any issues found
        """
        # Import here to avoid circular import at module level
        from ..motors_manager import CalibrationWarnings
        
        assert self.calibration_time is not None, "This method should be called after a motor is calibrated."
        warnings = CalibrationWarnings.NONE
        if abs(self.expected_range - self.total_range) > degrees_to_radians(1.0):
            warnings |= CalibrationWarnings.RANGE_MISMATCH
        if abs(self.calibration_homing_pos) > degrees_to_radians(5.0):
            warnings |= CalibrationWarnings.HOMING_POSITION_NONZERO
        three_degrees_in_radians = degrees_to_radians(3.0)
        if abs(self.lower_limit - self.range["min"]) > three_degrees_in_radians:
            warnings |= CalibrationWarnings.LOWER_LIMIT_MISMATCH
        if abs(self.upper_limit - self.range["max"]) > three_degrees_in_radians:
            warnings |= CalibrationWarnings.UPPER_LIMIT_MISMATCH
        return warnings

    def _msg_can_id(self, op: RobstrideMotorMsg, motor_id: int, host_id: Union[int, bytes, None] = None, e_byte: int = 0) -> int:
        """Build a CAN ID for a Robstride motor message.
        
        Args:
            op: The operation to build the CAN ID for.
            motor_id: The motor ID to build the CAN ID for.
            host_id: The host ID to build the CAN ID for. If int, then data will be checked to be within valid bounds.
                If you wish to skip this check, pass it as a bytes object with one element.
            e_byte: The extra byte to build the CAN ID for.
        """
        if isinstance(host_id, bytes):
            assert len(host_id) == 1, f"Error: host_id must be 1 byte, got {len(host_id)}"
            host_id = host_id[0]
        else:
            host_id = self.default_host_id if host_id is None else host_id
            if host_id < 0 or host_id > 0xFD:
                raise ValueError(f"Error: host_id must be between [0, 0xFD], got {host_id}")
        
        if motor_id < 0 or motor_id > 0xFD:
            raise ValueError(f"Error: motor_id must be between [0, 0xFD], got {motor_id}")
        if e_byte < 0 or e_byte > 0xFF:
            raise ValueError(f"Error: e_byte must be between [0, 0xFF], got {e_byte}")
        
        return (op.value << 24) | (e_byte << 16) | (host_id << 8) | motor_id
    
    def get_device_id(self) -> int:
        """Get the device id of the motor."""
        can_id = self._msg_can_id(RobstrideMotorMsg.GetDeviceId, self.motor_id)
        self.comms.cansend(self.iface, True, can_id, bytes([0] * 8), self.uid)

    def enable(self):
        """Send the 'enable' command to the motor."""
        can_id = self._msg_can_id(RobstrideMotorMsg.Enable, self.motor_id)
        self.comms.cansend(self.iface, True, can_id, bytes([0] * 8), self.uid)
        self._record_command_sent()

    def disable(self, clear_faults: bool = False):
        """Send the 'disable' command to the motor.
        
        Args:
            clear_faults: If True, clear the fault bits in the disable command.
        """
        can_id = self._msg_can_id(RobstrideMotorMsg.Disable, self.motor_id)
        self.comms.cansend(self.iface, True, can_id, bytes([1] + [0] * 7) if clear_faults else bytes([0] * 8), self.uid)
        self._record_command_sent()
    
    def set_active_reporting(self, active: bool, scan_time_ms: int = 10):
        """Turn on/off active reporting for the motor.
        
        Args:
            active: If True, turn on active reporting. False to disable.
            scan_time_ms: Decides the time between feedback messages. Minimum is 10ms, max is 327680ms (about 5.5
                minutes). Note: The motor only supports multiples of 5ms, so the actual scan time will be rounded
                down to the nearest multiple of 5ms.
        """
        if scan_time_ms < 10 or scan_time_ms > 327680:
            raise ValueError(f"Invalid scan time: {scan_time_ms}ms. Must be between 10ms and 327680ms.")
        
        # For the motor, 1 = 10ms, +1 = +5ms
        self.write_param(_ROBSTRIDE_PARAMS.epscan_time, int(scan_time_ms) // 5 - 1)

        can_id = self._msg_can_id(RobstrideMotorMsg.SetMotorActiveReporting, self.motor_id)
        data = bytes([1, 2, 3, 4, 5, 6, 1 if active else 0, 8])
        self.comms.cansend(self.iface, True, can_id, data, self.uid)
    
    def zero_position(self):
        """Set the zero position for the motor."""
        can_id = self._msg_can_id(RobstrideMotorMsg.ZeroPos, self.motor_id)
        self.comms.cansend(self.iface, True, can_id, bytes([0x01] + [0] * 7), self.uid)
        self._record_command_sent()

    def read_param(self, param_id: Union[int, str, RobstrideParam]) -> float:
        """Read a parameter from the motor."""
        can_id = self._msg_can_id(RobstrideMotorMsg.ReadParam, self.motor_id)
        data = S_uint16le.pack(get_param_info(param_id).id) + bytes([0] * 6)
        self.comms.cansend(self.iface, True, can_id, data, self.uid)

    def write_param(self, p_id: Union[int, str, RobstrideParam], value: Union[float, int]) -> None:
        self._write_param_checked(p_id, value, force_run=False)

    def _write_param_checked(self, p_id: Union[int, str, RobstrideParam], value: Union[float, int], force_run: bool = False) -> None:
        """Write a parameter value to the specified motor.

        Args:
            p_id: The parameter ID to write.
            value: The value to write.
            force_run: If True, force the write to be executed, ignoring the safety checks.
        """
        param = get_param_info(p_id)
        if not force_run and not param.is_within_range(value, self.version):
            raise ValueError(f"Value {value} is out of range for param {param.name}: {param.range[self.version]}")
            
        data = S_uint16le.pack(param.id) + bytes([0] * 2) + param.dtype.pack(value, pad_end=4, pad_val=0)
        
        if not force_run and param.name == "loc_ref":
            if self.is_safe_position_update(value):
                self.update_target_position(value)  # can raise an exception to cancel the run.
            else:
                warning(f"Triggering compliance mode [{self.motor_name}]: Target position {value} is too far from current position {self.angle[0]} last target position {self.target_position} (max dx: {self.max_position_dx})")
                self.trigger_compliance_mode()
                raise UnsafeCommandError(
                    message=f"Safety Error: Target position {value} is too far from current position {self.angle[0]} last target position {self.target_position} (max dx: {self.max_position_dx})",
                    motor=self
                ) # stop the program safely.

        can_id = self._msg_can_id(RobstrideMotorMsg.WriteParam, self.motor_id)

        try:
            self.comms.cansend(self.iface, True, can_id, data, self.uid)
            self._record_command_sent()
        except CanOverloadError:
            time.sleep(0.01)
            self.comms.cansend(self.iface, True, can_id, data, self.uid)
            self._record_command_sent()

        if param.name in ("loc_ref", "spd_ref") and RobstrideMotor._command_listeners:
            ts = currtime()
            for listener in RobstrideMotor._command_listeners:
                listener(self, param.name, value, ts)
        
    def set_run_mode(self, run_mode: RobstrideRunMode):
        self.write_param("run_mode", run_mode.value)
        self.run_mode = run_mode

    def set_target_position(self, target_position: float) -> None:
        """Set the target position (loc_ref) for the specified motor."""
        # Check if motor is disabled (in Reset mode)
        if self.mode[0] == RobstrideMotorMode.Reset:
            raise MotorDisabledError(
                message=f"Cannot set target position for motor {self.motor_name}: "
                        f"Motor is disabled (in Reset mode). Enable the motor first.",
                motor=self
            )
        
        # Check if motor is in the correct run mode for position control
        if self.run_mode != RobstrideRunMode.Position:
            raise MotorModeInconsistentError(
                f"Cannot set target position for motor {self.motor_name}: "
                f"Motor is in {self.run_mode.name} mode, but Position mode is required. "
                f"Use set_run_mode() to switch to Position mode first.",
                motor=self,
                curr_mode=self.run_mode,
                exp_mode=RobstrideRunMode.Position
            )
        
        self.write_param("loc_ref", target_position)
    
    def set_target_velocity(self, target_velocity: float) -> None:
        """Set the target velocity (spd_ref) for the specified motor."""
        # Check if motor is disabled (in Reset mode)
        if self.mode[0] == RobstrideMotorMode.Reset:
            raise MotorDisabledError(
                message=f"Cannot set target velocity for motor {self.motor_name}: "
                        f"Motor is disabled (in Reset mode). Enable the motor first.",
                motor=self
            )
        
        # Check if motor is in the correct run mode for velocity control
        if self.run_mode != RobstrideRunMode.Speed:
            raise MotorModeInconsistentError(
                f"Cannot set target velocity for motor {self.motor_name}: "
                f"Motor is in {self.run_mode.name} mode, but Speed mode is required. "
                f"Use set_run_mode() to switch to Speed mode first.",
                motor=self,
                curr_mode=self.run_mode,
                exp_mode=RobstrideRunMode.Speed
            )
        
        self.write_param("spd_ref", target_velocity)
    
    def operation_command(self, op: RobstrideOperationCommand, apply_target_position: bool = True, from_compliance_mode: bool = False):
        """ target_torque range (-60Nm to 60Nm)"""
        motor = op.motor
        motor.last_kp = op.kp
        motor.last_kd = op.kd        
        if self._should_monitor:
            print(self.motor_name, op.target_angle, op.target_torque, op.target_velocity, motor.last_kp, motor.last_kd, motor.torque)
        can_id, data = op.build_command()

        if apply_target_position:
            if self.is_safe_position_update(op.target_angle) or from_compliance_mode:
                self.update_target_position(op.target_angle, from_compliance_mode)  # can raise an exception to cancel the run.
            else:
                warning(f"Triggering compliance mode [{self.motor_name}]: Target position {op.target_angle} is too far from current position {self.angle[0]} last target position {self.target_position} (max dx: {self.max_position_dx})")
                self.trigger_compliance_mode()
                raise UnsafeCommandError(
                    message=f"Safety Error: Target position {op.target_angle} is too far from current position {self.angle[0]} last target position {self.target_position} (max dx: {self.max_position_dx})",
                    motor=self
                ) # stop the program safely.

        self.comms.cansend(self.iface, True, can_id, data, self.uid)
        self._record_command_sent()
    
    def trigger_compliance_mode(self):
        self.manager.trigger_compliance_mode(self.family_name, self.motor_name)


MotorMap = Dict[str, RobstrideMotor]

