from abc import ABC, abstractmethod
import math
import threading
import time

import numpy as np

from feathersdk.comms.comms_manager import UnknownInterfaceError
from feathersdk.utils.trajectory import change_in_vel
from feathersdk.utils.common import timediff, currtime, dont_allow_on_fork
from feathersdk.utils.logger import error, info
from feathersdk.utils.math_utils import degrees_to_radians
from .motors.robstride import (
    RobstrideMotor, RobstrideMotorMode, RobstrideRunMode, MotorModeInconsistentError, MotorDisabledError
)
from .motors.motors_manager import MotorsManager, MotorNotFoundError, CalibrationState
from .steppable_system import SteppableSystem
from .battery_system import PowerEvent, BatterySystem
from feathersdk.utils.logger import warning, error
from feathersdk.utils.feathertypes import *


class NavigationMotorError(Exception):
    """Exception raised when motor errors are detected during navigation commands."""
    pass


class NavigationPlanner(ABC):

    def __init__(self):
        self.x = 0
        self.y = 0
        self.theta = 0
        self.target_x = 0
        self.target_y = 0
        self.target_theta = 0

    def reset(self):
        self.x = 0
        self.y = 0
        self.theta = 0

    @abstractmethod
    def is_at_target_position(self):
        return (
            math.isclose(self.x, self.target_x, abs_tol=0.001)
            and math.isclose(self.y, self.target_y, abs_tol=0.001)
            and math.isclose(self.theta, self.target_theta, abs_tol=0.001)
        )


class VelocityNavigationPlanner(NavigationPlanner):
    """Allows the robot to be controlled by a specified velocity.
    TODO: The name of this class is misleading. A Planner builds a plan but does not control the robot in pure terms.
    This should be called VelocityController to better represent its purpose. Leaving for now for backward compatibility.
    
    Developer Notes: Any public methods of this class should
    follow _validate_and_accept_command() pattern in existing methods to ensure safety and error handling.
    """

    _BwC_MOTOR_DIRECTION = -1
    # Wheel circumference in meters, derived from wheel diameter: 0.12 m * π ≈ 0.376991 m
    _WHEEL_CIRCUMFERENCE_M = 0.12 * math.pi
    _SMALL_DELAY_SECONDS = 0.1
    
    # ±90 degrees in radians for extreme angle optimization
    _PLUS_90_DEG_RAD = math.pi / 2
    _MINUS_90_DEG_RAD = -math.pi / 2
    
    # Mapping from turning motor to corresponding spinning motor
    # Turning == turning motors, spinning == drive motors
    _TURNING_TO_SPINNING_MOTOR = {
        "BwC": "BpC",
        "BwR": "BpR",
        "BwL": "BpL",
    }

    def __init__(
        self, nav_platform, config: dict
    ):
        super().__init__()
        self.cfg = config
        self._current_wheel_angle = 0
        self._nav_platform : ThreeWheelServeDrivePlatform = nav_platform
        self._motor_manager = self._nav_platform.motors_manager
        self._turning_motors_vel_max_setting : Optional[float] = config.get("turning_motors_vel_max_setting", None)
        self._turning_motors_acc_set_setting : Optional[float] = config.get("turning_motors_acc_set_setting", None)
        self._max_velocity = config["max_velocity"]
        self._max_acceleration = config["max_acceleration"]
        self._calibration_torque_threshold = config.get("calibration_torque_threshold", 5.5)
        # rad/s and rad/s2
        self._max_rotation_velocity = config["max_rotation_velocity"]
        self._max_rotation_acceleration = config["max_rotation_acceleration"]
        self._acceleration_ramp_time = self._max_acceleration / config["max_jerk"]
        self._acceleration_ramp_delta_velocity = change_in_vel(0, config["max_jerk"], 0, self._acceleration_ramp_time)
        self._max_jerk = config["max_jerk"]
        self.current_spd_ref = {"BpC": 0, "BpR": 0, "BpL": 0}
        self.current_acceleration = 0
        self._last_drive_key = currtime()
        self._last_turn_key = currtime()
        self._is_rotating = False
        self._last_rotate_key = currtime()
        self._rotate_key_start_time = -1
        self._rotation_target_angles = self.cfg["rotation_target_angles"]
        self._rotation_velocity = 0
        self._rotation_acceleration = 0
        self._motor_shutdown_on_abort = True
        self._enabled = False
        
        # Command state variables - these are set by command methods and read by _step()
        self._target_turning_angle_rad = None  # Dictionary: {motor_name: target_angle_rad} or None
        self._set_target_spinning_velocity_to_zero()  # Dictionary: {motor_name: target_velocity_rad_s} or None
        
        # Speed sign adjustment per spinning motor for ±90 degree optimization
        # When target angle is ±π/2 and we choose the closer angle, this tracks the sign flip needed
        self._speed_sign_per_motor = {motor_name: 1 for motor_name in self._nav_platform.SPINNING_MOTORS}
        
        # Track last sent commands to avoid redundant CAN messages
        self._last_sent_turning_angles = {}  # {motor_name: last_sent_angle_rad}
        self._last_sent_spinning_velocities = {}  # {motor_name: last_sent_velocity_rad_s}
        self._COMMAND_REFRESH_INTERVAL = 0.1  # Refresh commands every 100ms for reliability
        self.next_force_refresh = currtime() + self._COMMAND_REFRESH_INTERVAL
        
        # Feedback delay detection - threshold in seconds above which we enter pause
        # Note: Motors log at their own threshold (default 1s); this is the platform pause threshold
        self._pause_on_feedback_timeout_seconds = config.get("pause_on_feedback_timeout_seconds", 5.0)
        # Pause state: True when in feedback-delay pause
        self._in_feedback_delay_pause = False
        # Last time we logged in _validate_and_accept_command during pause (0 = not yet); throttle to once per 1s
        self._feedback_delay_pause_last_log_time = 0.0
        # Delayed motors list from step loop, for error message in _validate_and_accept_command
        self._feedback_delay_pause_delayed_motors = []
        
        # Turning gate: do not spin until turning motors are within tolerance of commanded angle
        tolerance_degrees = config.get("turning_settled_angle_tolerance_degrees", None)
        if tolerance_degrees is not None:
            self._turning_settled_angle_tolerance_rad = degrees_to_radians(tolerance_degrees)
        else:
            self._turning_settled_angle_tolerance_rad = config.get(
                "turning_settled_angle_tolerance_rad", degrees_to_radians(5.0)
            )
        self._turning_settled = True  # True until we have a turning target and check feedback
        
        # Error tracking - stores the last error that occurred during step() execution
        self._last_error = None  # Exception object or None

        # Lock protecting shared state between main thread (_validate_and_accept_command, disable_motors, enable_motors) and step loop (step).
        # RLock allows the same thread to re-enter (e.g. disable_motors calls step() while holding the lock).
        self._lock = threading.RLock()

    def _validate_and_accept_command(self, command):
        """
        Validate system state and accept a command for later execution in the step loop.
        The command function sets state variables that will be executed by step().
        
        During feedback-delay pause, user API calls return without updating state. If we have not
        logged in the past 1 second, we log the pause reason (from step loop data) and reset the timer.
        
        Args:
            command (callable): A callable that sets navigation command state variables
            
        Raises:
            Exception: If the navigation platform is not in a healthy state (turning motors need recalibration)
            NavigationMotorError: If motor-related errors occur, with a detailed error message

        Developer Notes:
            I considered an alternate name ```_validate_and_queue_command```, however since we are
            only maintaining the last sent command (queue size 1), I chose the current name.
        """
        with self._lock:
            # Check for errors from previous step() execution
            if self._nav_platform.is_aborting():
                error("Informational: Navigation platform is aborting, skipping command")
                return

            if self._last_error is not None:
                e = self._last_error
                if isinstance(e, MotorNotFoundError):
                    raise NavigationMotorError(
                        f"Motor {e.motor_name_or_id} not found during navigation command: {str(e)}."
                    ) from e
                elif isinstance(e, MotorModeInconsistentError):
                    raise NavigationMotorError(
                        f"Motor {e.motor_name} is not in compatible mode for the command attempted. "
                        f"Current mode: {e.current_mode.name}, Expected mode: {e.expected_mode.name}"
                    ) from e
                elif isinstance(e, MotorDisabledError):
                    raise NavigationMotorError(
                        f"Motor {e.motor_name} is in disabled state during navigation command. Try restarting the power cycle for Robot base."
                    ) from e
                else:
                    # General exception - include exception in message
                    raise NavigationMotorError(
                        f"Unexpected error during navigation command execution: {str(e)}"
                    ) from e

            if not self._enabled:
                raise Exception("Error: Navigation platform is not enabled. Try calling enable_motors method on NavigationPlanner/Platform to start the navigation system.")

            state = self._nav_platform.calibration_state

            if state.value == CalibrationState.RECALIBRATING.value:
                raise Exception("Error: Cannot execute navigation commands while recalibration is in progress. Please wait for calibration to complete.")

            if state.value != CalibrationState.HEALTHY.value:
                if state.value == CalibrationState.UNCALIBRATED.value:
                    raise Exception("Error: Turning motors are not calibrated. Please use load_calibration_state() method on NavigationPlatform and follow the recommended actions.")
                elif state.value == CalibrationState.OUT_OF_RANGE.value:
                    raise Exception("Error: Turning motors are out of valid range. Please use load_calibration_state() method on NavigationPlatform and follow the recommended actions.")
                elif state.value == CalibrationState.CALIBRATION_FAILED.value:
                    raise Exception("Error: Previous calibration attempt failed. Please use load_calibration_state() method on NavigationPlatform and follow the recommended actions.")
                else:
                    raise Exception(f"Error: Navigation platform is not ready (state: {state.value}). Please use load_calibration_state() method on NavigationPlatform and follow the recommended actions.")

            # If in feedback-delay pause: log at most once per 1s (from step loop delayed_motors), then return without accepting command
            if self._in_feedback_delay_pause:
                now = currtime()
                if self._feedback_delay_pause_last_log_time == 0 or (now - self._feedback_delay_pause_last_log_time) >= 5.0:
                    parts = [f"{name} {delay:.1f}s" for name, delay in self._feedback_delay_pause_delayed_motors]
                    msg = "Navigation in feedback delay pause (motors delayed: {}). Resuming when delay clears.".format(
                        ", ".join(parts) if parts else "unknown"
                    )
                    error(msg)
                    self._feedback_delay_pause_last_log_time = now
                return

            command()

    def _set_run_mode(self):
        try:
            for motor_name in self._nav_platform.TURNING_MOTORS:
                self._motor_manager.disable(motor_name)
                self._motor_manager.set_run_mode(motor_name, RobstrideRunMode.Position)
                if self._turning_motors_vel_max_setting is not None:
                    self._motor_manager.write_param(motor_name, "vel_max", self._turning_motors_vel_max_setting)
                if self._turning_motors_acc_set_setting is not None:
                    self._motor_manager.write_param(motor_name, "acc_set", self._turning_motors_acc_set_setting)
            for motor_name in self._nav_platform.SPINNING_MOTORS:
                self._motor_manager.disable(motor_name)
                self._motor_manager.set_run_mode(motor_name, RobstrideRunMode.Speed)
                self._motor_manager.write_param(motor_name, "limit_spd", 4 * math.pi) # for safety right now.
                self._motor_manager.zero_position(motor_name) # spinning motors don't need calibration and are in velocity mode, so we zero position the motors at the time we initialize.
        except UnknownInterfaceError as e:
            error(f"Could not find interface for motors: {e}, navigation platform will be disabled")
        except Exception as e:
            error(f"Error: Enabling Navigation platform failed: {e}, commands to move the robot will not work correctly")

    def _enable_on_start_if_needed(self):
        if self.cfg.get("enable_motors_on_startup", False):
            self.enable_motors()

    def enable_motors(self):
        """Enable all navigation motors and verify they enter Run mode.
        
        Raises:
            UnknownInterfaceError: If communication interface is not available, platform is disabled.
            Exception: If enabling motors fails for any reason, platform is disabled.
        """
        with self._lock:
            try:
                for motor_name in self._nav_platform.ALL_MOTORS:
                    self._motor_manager.enable(motor_name)
                time.sleep(self._SMALL_DELAY_SECONDS)
                found_disabled_motors = False
                for motor_name in self._nav_platform.ALL_MOTORS:
                    current_mode = self._motor_manager.motors[motor_name].mode.value
                    if current_mode != RobstrideMotorMode.Run:
                        # Refresh the mode by reading the current state of the motor forcing the feedback as another fallback.
                        motor_state = self._motor_manager.read_current_state_sync(motor_name)
                        current_mode = motor_state["mode"].value
                        if current_mode != RobstrideMotorMode.Run:
                            found_disabled_motors = True
                            error (f"Motor {motor_name} is not in run mode during enable_motors, navigation platform will be disabled. Motor state:")
                            for key, value in motor_state.items():
                                error(f"  {key}: {value}")
                self._enabled = not found_disabled_motors
            except UnknownInterfaceError as e:
                error(f"Could not find interface for motors: {e}, navigation platform will be disabled")
                self._enabled = False
            except Exception as e:
                error(f"Error: Enabling motors failed: {e}, navigation platform will be disabled")
                self._enabled = False

    def set_motor_shutdown_on_abort(self, shutdown_on_abort: bool):
        self._motor_shutdown_on_abort = shutdown_on_abort

    def disable_motors(self, is_power_off: bool = False):
        """Disable motors and clear command state.
        
        Args:
            is_power_off (bool): If True, motors are not disabled (for power-off scenarios)
        """
        with self._lock:
            # Clear command state when disabling motors
            self._target_turning_angle_rad = None
            self._set_target_spinning_velocity_to_zero()
            # Reset speed sign adjustments
            self._speed_sign_per_motor = {motor_name: 1 for motor_name in self._nav_platform.SPINNING_MOTORS}
            # Clear errors and pause state when disabling motors
            self._last_error = None
            self._in_feedback_delay_pause = False
            self._feedback_delay_pause_delayed_motors = []
            
            # Send zero velocity command directly to all spinning motors to stop the robot
            # This is critical - without this, motors will continue at last commanded velocity
            for motor_name in self._nav_platform.SPINNING_MOTORS:
                try:
                    self._motor_manager.set_target_velocity(motor_name, 0.0)
                    self._last_sent_spinning_velocities[motor_name] = 0.0
                except Exception as e:
                    warning(f"Failed to set zero velocity for {motor_name} during disable: {e}")
            
            # Clear tracking to ensure fresh state when re-enabled
            self._last_sent_turning_angles.clear()
            
            if self._motor_shutdown_on_abort:
                if not is_power_off:
                    for motor_name in self._nav_platform.ALL_MOTORS:
                        self._motor_manager.disable(motor_name)
            self._enabled = False

    def _set_target_spinning_velocity_to_zero(self):
        self._target_spinning_velocity_rad_s = {motor_name: 0.0 for motor_name in self._nav_platform.SPINNING_MOTORS}

    def step(self):
        """Execute pending navigation commands and monitor feedback delays.
        
        When feedback delay is detected on any motor, the robot is stopped (zero velocity
        sent once) and the step loop is paused until delay clears. During pause, feedback
        is triggered (set_active_reporting(False)) so delay can be re-checked. No exception
        is raised; commands are not sent until pause ends.
        """
        with self._lock:
            # Stop execution if disabled (e.g., due to previous errors)
            if not self._enabled:
                return

            # Check calibration state - don't send commands if not healthy
            state = self._nav_platform.calibration_state
            if state.value != CalibrationState.HEALTHY.value:
                return

            # Use monotonic time for duration calculations (refresh interval check)
            current_time = currtime()
            force_refresh = (current_time >= self.next_force_refresh)
            if force_refresh:
                self.next_force_refresh += self._COMMAND_REFRESH_INTERVAL

            # Use absolute time for feedback delay calculations (must match motor.temp.timestamp which uses time.time())
            command_sent = False

            try:
                delayed_motors = []
                # Calculate and store feedback delay for all motors using motor-level detection
                for motor_name in self._nav_platform.ALL_MOTORS:
                    motor = self._motor_manager.motors[motor_name]

                    # Use motor-level timeout check (logs critical message at motor's threshold, e.g. 1s)
                    # Only add to delayed_motors if exceeds abort threshold for error escalation
                    delay = motor.check_feedback_timeout(threshold=self._pause_on_feedback_timeout_seconds)
                    if delay is not None:
                        delayed_motors.append((motor_name, delay))

                # If currently in feedback-delay pause, check whether to resume or keep pausing
                if self._in_feedback_delay_pause:
                    if not delayed_motors:
                        # Delay cleared - resume immediately
                        self._in_feedback_delay_pause = False
                    else:
                        # Still in pause: keep delayed_motors for _validate_and_accept_command to use when logging
                        self._feedback_delay_pause_delayed_motors = list(delayed_motors)
                        # For pitch motors (spinning), set zero velocity to keep the robot not in moving state and force trigger feedback
                        for motor_name in self._nav_platform.SPINNING_MOTORS:
                            try:
                                self._motor_manager.set_target_velocity(motor_name, 0.0)
                            except Exception as e:
                                warning(f"Failed to set zero velocity for {motor_name} during feedback delay pause: {e}")
                        # For turning motors, use set_active_reporting to trigger feedback
                        for motor_name in self._nav_platform.TURNING_MOTORS:
                            self._motor_manager.motors[motor_name].set_active_reporting(False)
                        return

                # If not in pause and delay detected: stop once, enter pause, return
                if any(delayed_motors):
                    self._set_target_spinning_velocity_to_zero()
                    for motor_name in self._nav_platform.SPINNING_MOTORS:
                        self._motor_manager.set_target_velocity(motor_name, 0.0)
                    self._in_feedback_delay_pause = True
                    self._feedback_delay_pause_delayed_motors = list(delayed_motors)
                    self._feedback_delay_pause_last_log_time = 0  # so first API call can log
                    return

                # Handle turning motor angle command
                if self._target_turning_angle_rad is not None:
                    for motor_name, target_angle in self._target_turning_angle_rad.items():
                        # The goal of this code block where we use a different chosen_target_angle
                        # is meant to minimize the turning angle of the wheels when ordered to do side
                        # ways motion (-90 or +90 for go_to degrees). This is useful in a scenario
                        # where most motions are either rotate in place OR move sideways or forward/backward
                        # as in one of our customer's key scenario.
                        # In this use case, -90 and +90 are physically same orientation for wheels, and
                        # a clever trick to minimize the turning angle motion for example when going from
                        # 90 degrees to -90 degrees is to not turn the wheels but reverse the sign of the speed
                        # specified by the user.
                        # The below code block is implementing that clever trick. When we
                        # need to move the wheel to +/- 90, we choose the closest angle to the current position
                        # and set that as the chosen_target_angle. When this chosen_target_angle is different
                        # from the target_angle, we reverse the sign of the speed for the corresponding spinning motor.
                        m = self._motor_manager.motors[motor_name]
                        chosen_target_angle = target_angle
                        spinning_motor = self._TURNING_TO_SPINNING_MOTOR.get(motor_name)

                        last_angle = self._last_sent_turning_angles.get(motor_name)
                        if abs(m.torque[0]) > self._calibration_torque_threshold:
                            dx = m.target_position - abs(m.angle[0])
                            if math.isclose(m.calibration_homing_pos + self._PLUS_90_DEG_RAD, m.angle[0], abs_tol=0.1):
                                warning("Encoder drift situation detected high, correcting calibration homing position.")
                                warning(f"Motor {motor_name} angle: {m.angle[0]}, torque: {m.torque[0]}, target position: {m.target_position}, calibration homing pos: {m.calibration_homing_pos}")
                                dx = (m.calibration_homing_pos + self._PLUS_90_DEG_RAD - m.angle[0])
                                if abs(dx) < 0.001:
                                    dx = 0.001
                                m.calibration_homing_pos -= dx
                                m.upper_limit -= dx
                                m.lower_limit -= dx
                                force_refresh = True
                            elif math.isclose(m.calibration_homing_pos + self._MINUS_90_DEG_RAD, m.angle[0], abs_tol=0.1):
                                warning("Encoder drift situation detected low, correcting calibration homing position.")
                                warning(f"Motor {motor_name} angle: {m.angle[0]}, torque: {m.torque[0]}, target position: {m.target_position}, calibration homing pos: {m.calibration_homing_pos}")
                                dx = (m.calibration_homing_pos + self._MINUS_90_DEG_RAD - m.angle[0])
                                if abs(dx) < 0.001:
                                    dx = -0.001
                                m.calibration_homing_pos -= dx
                                m.upper_limit -= dx
                                m.lower_limit -= dx
                                force_refresh = True
                            else:
                                warning(f"Warning: High torque detected on motor {motor_name} during navigation step.")
                                warning(f"Motor {motor_name} angle: {m.angle[0]}, torque: {m.torque[0]}, target position: {m.target_position}, calibration homing pos: {m.calibration_homing_pos}")
                                chosen_target_angle = m.angle[0] - motor.calibration_homing_pos
                                force_refresh = True

                        # In the sideways motion, to optimize turns, it's best to choose the direction of rotation
                        # angle per motor and flip the speed sign as needed.
                        # This approach works the best when the use case is mostly either rotate in place OR move sideways or forward/backward.
                        # For a general purpose use case, there are other better ways we tried first before settling on this approach becasue this is the major use
                        # case right now. We can revisit this approach if needed.
                        if math.isclose(abs(target_angle), self._PLUS_90_DEG_RAD, rel_tol=1e-9):
                            motor = self._motor_manager.motors[motor_name]
                            rotation_angle_for_motor = self._rotation_target_angles.get(motor_name)

                            if rotation_angle_for_motor < 0:
                                chosen_target_angle = self._MINUS_90_DEG_RAD
                            else:
                                chosen_target_angle = self._PLUS_90_DEG_RAD

                            # Update speed sign for corresponding spinning motor
                            if spinning_motor is not None:
                                if chosen_target_angle != target_angle:
                                    self._speed_sign_per_motor[spinning_motor] = -1
                                else:
                                    self._speed_sign_per_motor[spinning_motor] = 1
                        else:
                            # Reset speed sign to 1 for non-extreme angles
                            if spinning_motor is not None:
                                self._speed_sign_per_motor[spinning_motor] = 1

                        # From here starts the core logic of seding the chosen target angle to the motor when needed.
                        if force_refresh or last_angle != chosen_target_angle:
                            motor = self._motor_manager.motors[motor_name]
                            self._motor_manager.set_target_position(
                                motor_name, motor.calibration_homing_pos + chosen_target_angle
                            )
                            self._last_sent_turning_angles[motor_name] = chosen_target_angle
                            command_sent = True
                else:
                    # Force all motors to be updated at force_refresh
                    # if force_refresh:
                    #     for motor_name in self._nav_platform.TURNING_MOTORS:
                    #         self._motor_manager.enable(motor_name)
                    # Clear tracking when target is None
                    self._last_sent_turning_angles.clear()
                    # Reset speed signs when no turning angle is set
                    for motor_name in self._nav_platform.SPINNING_MOTORS:
                        self._speed_sign_per_motor[motor_name] = 1

                # Gate spinning on turning settled: do not spin until turning motors are within tolerance
                if self._target_turning_angle_rad is not None and self._last_sent_turning_angles:
                    turning_settled = all(
                        motor_name in self._last_sent_turning_angles
                        and abs(
                            self._motor_manager.motors[motor_name].calibrated_angle.value
                            - self._last_sent_turning_angles[motor_name]
                        )
                        <= self._turning_settled_angle_tolerance_rad
                        for motor_name in self._nav_platform.TURNING_MOTORS
                    )
                else:
                    turning_settled = True
                self._turning_settled = turning_settled

                # Handle spinning motor velocity command
                if self._target_spinning_velocity_rad_s is not None:
                    # Gate: send 0 when feedback delay or turning not settled (feedback delay has precedence)
                    spin_gated = any(delayed_motors) or not turning_settled
                    for motor_name, target_velocity in self._target_spinning_velocity_rad_s.items():
                        # Apply speed sign adjustment for ±90 degree optimization
                        speed_sign = self._speed_sign_per_motor.get(motor_name, 1)
                        chosen_velocity = target_velocity * speed_sign
                        effective_velocity = 0.0 if spin_gated else chosen_velocity

                        last_velocity = self._last_sent_spinning_velocities.get(motor_name)
                        if force_refresh or last_velocity != effective_velocity:
                            self._motor_manager.set_target_velocity(motor_name, effective_velocity)
                            self._last_sent_spinning_velocities[motor_name] = effective_velocity
                            command_sent = True
                else:
                    # Clear tracking when target is None
                    self._last_sent_spinning_velocities.clear()

                # Update command time whenever a command is sent (state change or periodic refresh)
                if command_sent:
                    # Clear error on successful command execution
                    self._last_error = None
            except Exception as e:
                self._last_error = e
                self._enabled = False

    # TODO: Do we really need to call this every time we want to execute a command?
    # This was done during an older pattern of keeping the constructor of the class
    # putting the motors in disabled state but doing lazy enabling of the motors on the
    # first time we execute a command. We changed the code since then to enable the motors
    # during the initialization of the NavigationPlanner.
    def _ensure_motors_enabled(self, motor_names):
        found_disabled_motors = False
        for motor_name in motor_names:
            if self._motor_manager.motors[motor_name].mode[0] != RobstrideMotorMode.Run:
                found_disabled_motors = True
                self._motor_manager.enable(motor_name)
        if found_disabled_motors:
            time.sleep(self._SMALL_DELAY_SECONDS)
            found_disabled_motors = False
            for motor_name in motor_names:
                if self._motor_manager.motors[motor_name].mode[0] != RobstrideMotorMode.Run:
                    found_disabled_motors = True
                    break
            if found_disabled_motors:
                self._enabled = False
                raise Exception("Error: Some motors are not in run mode. Try calling enable_motors method on NavigationPlanner again or try a power cycle on the Robot.")

    def orient_rotate(self):
        """Orient wheels to rotation configuration and stop spinning motors.
        
        Raises:
            Exception: If platform not enabled or calibration state is unhealthy
            NavigationMotorError: If motor errors occurred in previous step() execution
        """
        def _execute_orient_rotate():
            self._ensure_motors_enabled(self._nav_platform.TURNING_MOTORS)
            # Set state to orient wheels for rotation: each motor gets its specific angle from config
            self._target_turning_angle_rad = self._rotation_target_angles.copy()
            self._set_target_spinning_velocity_to_zero()
        
        self._validate_and_accept_command(_execute_orient_rotate)

    def rotate_in_place(self, speed):
        """Rotate the robot in place by setting the speed of the spinning motors.
        
        Args:
            speed (float): The speed in degrees/second at which all the spinning motors will rotate.
        
        Raises:
            Exception: If platform not enabled or calibration state is unhealthy
            NavigationMotorError: If motor errors occurred in previous step() execution
        """
        def _execute_rotate():
            self._target_turning_angle_rad = self._rotation_target_angles.copy()
            self._ensure_motors_enabled(self._nav_platform.SPINNING_MOTORS)
            target_velocity_rad_s = degrees_to_radians(speed)
            self._target_spinning_velocity_rad_s = {
                motor_name: target_velocity_rad_s
                for motor_name in self._nav_platform.SPINNING_MOTORS
            }
        
        self._validate_and_accept_command(_execute_rotate)

    def _orient_angle(self, degrees):
        """Internal method to set target angle state. Actual execution happens in step()."""
        target_angle_rad = degrees_to_radians(-1 * degrees)
        self._target_turning_angle_rad = {
            motor_name: target_angle_rad
            for motor_name in self._nav_platform.TURNING_MOTORS
        }

    def _set_speed(self, speed):
        """Internal method to set target speed state. Actual execution happens in step()."""
        degrees_per_second = speed / self._WHEEL_CIRCUMFERENCE_M * 360
        base_velocity_rad_s = degrees_to_radians(degrees_per_second)
        self._target_spinning_velocity_rad_s = {
            motor_name: base_velocity_rad_s * self._motor_manager.motors[motor_name].direction
            for motor_name in self._nav_platform.SPINNING_MOTORS
        }

    def go_to(self, degrees, speed, motors_is_on=False):
        """Set wheel orientation and speed for directional movement.
        
        Args:
            degrees (float): Target wheel angle in degrees from -90 to 90 (0 = forward).
                All wheels will be turned to this angle.
            speed (float): Target linear speed in m/s, range -1.3 to 1.3 m/s.
            motors_is_on (bool): If False, enables motors before command. If True, assumes motors already enabled.
        
        Raises:
            ValueError: If degrees or speed are outside valid ranges
            Exception: If platform not enabled or calibration state is unhealthy
            NavigationMotorError: If motor errors occurred in previous step() execution

        Note: 
            For target angles of exactly -90 or +90, each turning motor will choose
            whichever angle is closer to its current position to minimize rotation. The speed
            sign is adjusted per motor accordingly to maintain the intended direction of travel.
            This optimization is applied in step() at execution time.
        """
        # TODO: The -90 and +90 values are not an immediate problem now 
        # given our expected range is 220 degrees for turning motors.
        # However, to be quite reliable, the -90 and +90 shouldn't be
        # hardcoded but they should be calculated as min(90, minimum_value_among_all_turning_motors_range / 2) 
        def _execute_go_to():
            if degrees < -90 or degrees > 90:
                raise ValueError("Degrees must be between -90 and 90 degrees")
            if speed < -1.3 or speed > 1.3:
                raise ValueError(f"Speed must be between -1.3 and 1.3 m/s, got {speed} m/s")
            if not motors_is_on:
                self._ensure_motors_enabled(self._nav_platform.TURNING_MOTORS)
                self._ensure_motors_enabled(self._nav_platform.SPINNING_MOTORS)
            self._orient_angle(degrees)
            self._set_speed(speed)
        
        self._validate_and_accept_command(_execute_go_to)

    def orient_angle(self, degrees):
        """Set wheel orientation without changing speed.
        
        Args:
            degrees (float): Target angle in degrees from -90 to 90 (0 = forward, clockwise positive).
        
        Raises:
            Exception: If platform not enabled or calibration state is unhealthy
            NavigationMotorError: If motor errors occurred in previous step() execution
        """
        def _execute_orient():
            self._ensure_motors_enabled(self._nav_platform.TURNING_MOTORS)
            self._orient_angle(degrees)

        self._validate_and_accept_command(_execute_orient)
        # angle should be in degrees from -90 to 90 degrees. 0 represents the center. All the wheels will be turned to this angle.
        # clockwise convention. clockwise is positive.

    def set_speed(self, speed):
        """Set linear speed without changing wheel orientation.
        
        Args:
            speed (float): Linear speed in m/s, range -1.3 to 1.3 m/s.
        
        Raises:
            ValueError: If speed is outside valid range
            Exception: If platform not enabled or calibration state is unhealthy
            NavigationMotorError: If motor errors occurred in previous step() execution
        """
        if speed < -1.3 or speed > 1.3:
            raise ValueError(f"Speed must be between -1.3 and 1.3 m/s, got {speed} m/s")
        def _execute_set_speed():
            self._ensure_motors_enabled(self._nav_platform.SPINNING_MOTORS)
            self._set_speed(speed)
        
        self._validate_and_accept_command(_execute_set_speed)

    def is_at_target_position(self):
        """Check if robot is at target position.
        
        For velocity-based control, this always returns True as there is no discrete target position.
        
        Returns:
            bool: Always True for velocity control mode
        """
        return True


class ThreeWheelServeDrivePlatform(SteppableSystem):

    TURNING_MOTORS = ["BwC", "BwR", "BwL"]
    SPINNING_MOTORS = ["BpC", "BpR", "BpL"]
    ALL_MOTORS = TURNING_MOTORS + SPINNING_MOTORS

    def __init__(self, motors_manager: MotorsManager, config: dict, power: 'BatterySystem' = None):
        """Initialize three-wheel swerve drive navigation platform.
        
        Args:
            motors_manager (MotorsManager): Manager for motor communication
            config (dict): Configuration dict with "motors" and "nav_planner" keys
            power (BatterySystem, optional): Battery system instance for power management
        """
        self.cfg = config
        self.power = power
        self.motors_map = {}
        self.calibration_warnings = {}
        for motor_name, motor_dict in self.cfg["motors"].items():
            self.motors_map[motor_name] = RobstrideMotor(int(motor_dict["id"], 16), motor_name, motor_dict["motor_config"])
        self.motors_manager = motors_manager
        self.motors_manager.add_motors(self.motors_map, family_name="wheels")
        self.motors_manager.find_motors(list(self.motors_map.keys()))

        for motor_name in self.ALL_MOTORS:
            assert motor_name in self.motors_map, f"Motor {motor_name} is expected to be in the config for navigation platform, without it, the navigation platform will not work correctly"

        # Initialize calibration state
        self.calibration_state = CalibrationState.UNINITIALIZED

        self.operating_frequency = 100
        assert "nav_planner" in self.cfg, "nav_planner configuration is required"
        self.nav_planner = VelocityNavigationPlanner(
            nav_platform=self,
            config=self.cfg["nav_planner"]["config"]
        )
        self._aborting = False
        self.is_forked = False

        self._on_start()
    
    def disable_drive_motor_current(self):
        for motor_name in self.SPINNING_MOTORS:
            self.motors_manager.get_motor(motor_name).write_param("limit_cur", 0.0)
    
    def enable_drive_motor_current(self):
        for motor_name in self.SPINNING_MOTORS:
            self.motors_manager.get_motor(motor_name).write_param("limit_cur", 10.0)

    @dont_allow_on_fork
    def _on_start(self):
        """Initialize platform: load calibration, set motor modes, enable if configured."""
        self.load_calibration_data()
        self.nav_planner._set_run_mode()
        self.nav_planner._enable_on_start_if_needed()

    @dont_allow_on_fork
    def on_abort(self):
        """Disable motors on system abort."""
        # Prevent double calls (e.g., from both signal handler and atexit)
        # Note: Signal handlers and atexit both run in main thread, so no threading issues
        if self._aborting:
            return
        self._aborting = True
        self.nav_planner.disable_motors()

    @dont_allow_on_fork
    def is_aborting(self):
        return self._aborting

    @dont_allow_on_fork
    def on_power_event(self, event: PowerEvent):
        """Handle power events from the BatterySystem.
        
        Args:
            event (PowerEvent): The power event (POWER_OFF or POWER_RESTORED)
        """
        if event == PowerEvent.POWER_OFF:
            self.nav_planner.disable_motors(is_power_off=True)
        elif event == PowerEvent.POWER_RESTORED:
            try:
                self._on_start()
            except Exception as e:
                error(f"Warning: Failed to re-enable navigation platform after power restore: {e}, Navigation system will not work, try recreating the Robot instance.")

    @dont_allow_on_fork
    def _is_at_target_position(self):
        return (
            math.isclose(self.x, self.target_x, abs_tol=0.001)
            and math.isclose(self.y, self.target_y, abs_tol=0.001)
            and math.isclose(self.theta, self.target_theta, abs_tol=0.001)
        )

    @dont_allow_on_fork
    def is_motors_enabled(self):
        """Check if all motors are in Run mode.
        
        Returns:
            bool: True if all motors are in Run mode, False otherwise
        """
        for motor in self.motors_map.values():
            if not motor.mode[0] == RobstrideMotorMode.Run:
                return False
        return True

    @dont_allow_on_fork
    def _step(self):
        """Execute navigation commands at the operating frequency.
        
        Delegates to the navigation planner's step method to process pending commands.
        """
        self.nav_planner.step()

    @dont_allow_on_fork
    def _reset_global_origin(self):
        self.x = 0
        self.y = 0
        self.theta = 0

    @dont_allow_on_fork
    def _update_trajectory_plan(self):
        # x is forward, y is left, wheel_angle starts at 0 and is CCW
        # calculate the wheel angle to reach the target position
        target_wheel_angle = math.atan2(self.target_y, self.target_x)
        # calculate the distance to the target position
        distance = math.sqrt(self.target_x**2 + self.target_y**2)
        # calculate the time to reach the target position
        time = distance / self.target_velocity

    @dont_allow_on_fork
    def _set_target_position(self, x=None, y=None, theta=None):
        if x is not None:
            self.target_x = x
        if y is not None:
            self.target_y = y
        if x is not None or y is not None:
            self._update_trajectory_plan()
        if theta is not None:
            self.target_theta = theta

    @dont_allow_on_fork
    def _set_target_velocity(self, velocity=None, dtheta=None):
        if velocity is not None:
            self.target_velocity = velocity
        if dtheta is not None:
            self.target_dtheta = dtheta

    @dont_allow_on_fork
    def _set_target_acceleration(self, acceleration=None, dtheta_accel=None):
        if acceleration is not None:
            self.target_accel = acceleration
        if dtheta_accel is not None:
            self.target_dtheta_accel = dtheta_accel

    @dont_allow_on_fork
    def _set_target_jerk(self, jerk=None, dtheta_jerk=None):
        if jerk is not None:
            self.target_jerk = jerk
        if dtheta_jerk is not None:
            self.target_dtheta_jerk = dtheta_jerk

    @dont_allow_on_fork
    def _set_target_state(
        self,
        x=None,
        y=None,
        theta=None,
        velocity=None,
        dtheta=None,
        acceleration=None,
        dtheta_accel=None,
        jerk=None,
        dtheta_jerk=None,
    ):
        self._set_target_position(x, y, theta)
        self._set_target_velocity(velocity, dtheta)
        self._set_target_acceleration(acceleration, dtheta_accel)
        self._set_target_jerk(jerk, dtheta_jerk)

    @dont_allow_on_fork
    def disable_motors(self):
        self.nav_planner.disable_motors()
    
    @dont_allow_on_fork
    def enable_motors(self):
        self.nav_planner.enable_motors()

    @dont_allow_on_fork
    def get_system_state(self):
        """Get current system state: turning motor angles and spinning motor velocities.
        
        Returns:
            Dict[str, float]: Dictionary with keys like "BwC_angle", "BpC_velocity" etc.
                Values are angles in radians for turning motors and velocities in rad/s for spinning motors.
        """
        state = {}
        for motor_name in self.TURNING_MOTORS:
            motor = self.motors_manager.get_motor(motor_name)
            state[f"{motor_name}_angle"] = motor.calibrated_angle.value
        for motor_name in self.SPINNING_MOTORS:
            motor = self.motors_manager.get_motor(motor_name)
            state[f"{motor_name}_velocity"] = motor.velocity.value
        return state

    @dont_allow_on_fork
    def load_calibration_data(self):
        """Load calibration data and check motor health.
        
        Raises:
            Exception: If checking motor health fails, calibration_state is set to UNCALIBRATED
        """
        self.calibration_state = CalibrationState.UNINITIALIZED
        try:
            if self.power is not None:
                last_powered_up_time = self.power.last_powered_up_time()
            else:
                last_powered_up_time = None

            for motor_name in self.TURNING_MOTORS:
                motor = self.motors_manager.get_motor(motor_name)
                if not motor.load_calibration_state():
                    self.calibration_state = CalibrationState.UNCALIBRATED
                elif motor.dual_encoder and motor.iface is not None:
                    self.motors_manager.recover_from_power_cycle(motor_name)
                elif not motor.dual_encoder and (last_powered_up_time is None or last_powered_up_time > motor.calibration_time):
                    self.calibration_state = CalibrationState.UNCALIBRATED

            # Check if motors are in valid range
            if self.calibration_state.value != CalibrationState.UNCALIBRATED.value:
                if self.motors_manager.check_motors_in_range(self.TURNING_MOTORS):
                    self.calibration_state = CalibrationState.HEALTHY
                else:
                    self.calibration_state = CalibrationState.OUT_OF_RANGE
                    
            if self.calibration_state.value != CalibrationState.HEALTHY.value:
                error(f"Navigation requires recalibration. Use recalibrate() method on Robot's Navigation Platform to recalibrate the motors. Calibration state: {self.calibration_state.name}")
            else:
                # TODO: convert all print statements in this class to info when we change the default logging level to INFO.
                print(f"Navigation platform calibration state restored successfully and the system is ready to use!")
        except Exception as e:
            error(f"Warning: Could not check turning motors health: {e}, the calibration state is left as UNCALIBRATED, try calling load_calibration_data() again and follow the recommended actions.")
            self.calibration_state = CalibrationState.UNCALIBRATED

    @dont_allow_on_fork
    def recalibrate(self):
        """Recalibrate all turning motors.
        
        Finds range, zeros at homing position, and saves calibration data.
        
        Raises:
            Exception: If already recalibrating or calibration fails
        """
        for motor_name in self.TURNING_MOTORS:
            self.motors_manager.write_param(motor_name, "vel_max", 10.0)
            self.motors_manager.write_param(motor_name, "acc_set", 10.0)

        self._recalibrate_private()
        # Quick hack to update all the motors properties after calibration. Otherwise, it'll be stale.
        for motor_name in self.TURNING_MOTORS:
            self.motors_manager.enable(motor_name)
            self.motors_manager.write_param(motor_name, "vel_max", self.nav_planner._turning_motors_vel_max_setting)
            self.motors_manager.write_param(motor_name, "acc_set", self.nav_planner._turning_motors_acc_set_setting)
        
            self.motors_manager.read_param(motor_name, "vel_max")
            self.motors_manager.read_param(motor_name, "acc_set")
        time.sleep(0.1)
        for motor_name in self.TURNING_MOTORS:
            motor = self.motors_manager.get_motor(motor_name)
            print(f"Motor {motor_name} vel_max: {motor.properties['vel_max'].value}, acc_set: {motor.properties['acc_set'].value}")

    @dont_allow_on_fork
    def _recalibrate_private(self, motor_name=None, verbose=False):
        """Recalibrate motors (all or single motor if motor_name provided).
        
        Args:
            motor_name (str, optional): If provided, calibrate only this motor. Otherwise calibrate all.
            verbose (bool): If True, print detailed calibration progress
        
        Raises:
            Exception: If already recalibrating, motor_name is invalid, or calibration fails
            
        Note: Uses the new calibrate_motor method which:
            - Finds range and moves to homing position (defaults to 0 if not set in config)
            - Sets zero position at homing
            - Captures any residual offset after zeroing
            - Saves calibration data with calibration_homing_pos
        """
        if self.calibration_state.value == CalibrationState.RECALIBRATING.value:
            raise Exception("Error: Navigation platform is already recalibrating, please wait for calibration to complete.")
        
        try:
            self.calibration_state = CalibrationState.RECALIBRATING
            if motor_name is not None:
                if motor_name not in self.TURNING_MOTORS:
                    raise ValueError(f"Motor {motor_name} is not a turning motor")
                motor_names = [motor_name]
            else:
                motor_names = self.TURNING_MOTORS
            
            for name in motor_names:
                info(f"Recalibrating {name}")
                # calibrate_motor handles retry logic and saves calibration internally
                warnings = self.motors_manager._calibrate_motor(name, torque_threshold_for_limit_detection=self.nav_planner._calibration_torque_threshold, verbose=verbose)
                self.calibration_warnings[name] = warnings
                time.sleep(0.02)
            
            if motor_name is None:
                self.calibration_state = CalibrationState.HEALTHY
            else:
                range_check_for_all_motors = self.motors_manager.check_motors_in_range(self.TURNING_MOTORS, raise_error=False)
                if range_check_for_all_motors:
                    self.calibration_state = CalibrationState.HEALTHY
                else:
                    self.calibration_state = CalibrationState.OUT_OF_RANGE
                    error (f"Navigation platform calibration partial: You may need to calibrate other turning motors. Calibration state: {self.calibration_state.name}")
            
            if self.calibration_state.value == CalibrationState.HEALTHY.value:
                info(f"Navigation platform calibration completed successfully!")
        except Exception as e:
            self.calibration_state = CalibrationState.CALIBRATION_FAILED
            raise Exception(f"Error: Navigation platform recalibration failed: {e}")

    @dont_allow_on_fork
    async def health_check(self):
        """Perform health check on all motors.
        
        Returns:
            dict: Dictionary with keys:
                - "calibration_state": Current calibration state name
                - "enabled": Whether navigation planner is enabled
                - Per-motor keys with parameters: mech_pos, loc_ref, mech_vel, iqf, loc_kp,
                  temp, torque, angle_internal, angle, velocity
        """
        states = {"calibration_state": self.calibration_state.name}
        states["enabled"] = self.nav_planner._enabled # TODO: Fix this private variable access.
        for motor_name in self.motors_map.keys():
            # Trigger feedback frame and wait for fresh feedback data (preserves enabled/disabled state)
            await self.motors_manager.read_current_state_async(motor_name)
            motor = self.motors_manager.motors[motor_name]
            result = {
                "mech_pos": await self.motors_manager.read_param_async(motor_name, "mech_pos"),
                "loc_ref": await self.motors_manager.read_param_async(motor_name, "loc_ref"),
                "mech_vel": await self.motors_manager.read_param_async(motor_name, "mech_vel"),
                "iqf": await self.motors_manager.read_param_async(motor_name, "iqf"),
                "loc_kp": await self.motors_manager.read_param_async(motor_name, "loc_kp"),
                "temp": motor.temp,
                "torque": motor.torque,
                "angle_internal": motor.angle,
                "angle": motor.calibrated_angle,
                "velocity": motor.velocity,
            }
            states[motor_name] = result
        return states

    def on_fork(self):
        self.cfg = {}
        self.power = None
        self.motors_map = {}
        self.calibration_warnings = {}
        self.motors_manager = None
        self.calibration_state = CalibrationState.UNINITIALIZED
        self.nav_planner = None
        self._aborting = True
        self.is_forked = True
