Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharpNavigation.cs

@@ -0,0 +1,874 @@
+using RobotNet.VDA5050.Order;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Detection;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.Navigation.CSharp;
+using RobotNet10.RobotApp.Services.Robot.Models;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.Services.Navigation;
+
+public class NavigationConfig
+{
+    public double MaxLinearVelocity { get; set; }
+    public double MaxAngularVelocity { get; set; }
+    public double MinLinearVelocity { get; set; }
+    public double RotateAngularVelocity { get; set; }
+    public double Acceleration { get; set; } = 0.5;
+    public double Deceleration { get; set; } = 0.5;
+    public double ReachedRadius { get; set; } = 0.03;
+    public double HeadingTolerance { get; set; } = 3.0;
+    public double InitialRotationThreshold { get; set; } = 5.0;
+    public double DockToMaxSpeed { get; set; } = 0.3;
+    public double DockToRetrySpeed { get; set; } = 0.05;
+    public double DockToRotateSpeed { get; set; } = 0.05;
+    public Dictionary<SafetySpeed, double> SafetySpeedMap { get; set; } = [];
+    /// <summary>
+    /// Maximum distance (meters) from goal at which a Moving overshoot is still accepted as Completed.
+    /// Default: 0.15m
+    ///
+    /// Meaning: When overshoot is detected during Moving, if robot is within this radius of the goal,
+    /// navigation proceeds to final rotation → Completed. Otherwise → Error.
+    ///
+    /// ↑ Increase (0.2-0.3):
+    ///   ✓ More tolerant of overshoot — fewer Error states
+    ///   ✗ Robot may report Completed at a position far from goal
+    ///
+    /// ↓ Decrease (0.05-0.1):
+    ///   ✓ Higher positional accuracy requirement
+    ///   ✗ More likely to trigger Error on minor overshoot
+    ///
+    /// Tuning Tips:
+    /// - Should be ≥ ReachedRadius
+    /// - Must be ≤ MovingOvershootDetectionRadius to be meaningful
+    /// - For high-precision tasks: 0.05-0.1
+    /// - For general navigation: 0.15-0.2
+    /// </summary>
+    public double OvershootAcceptanceRadius { get; set; } = 0.15;
+
+    /// <summary>
+    /// Distance (meters) from goal at which overshoot detection begins during Moving.
+    /// Default: 0.5m
+    ///
+    /// Meaning: Overshoot detection only activates when robot is within this radius of the final goal.
+    /// Outside this radius, distance fluctuations are ignored.
+    ///
+    /// ↑ Increase (0.8-1.0):
+    ///   ✓ Earlier overshoot detection
+    ///   ✗ May false-trigger on path curvature near goal
+    ///
+    /// ↓ Decrease (0.2-0.3):
+    ///   ✓ Fewer false triggers
+    ///   ✗ Late detection — robot may travel further past goal before stopping
+    ///
+    /// Tuning Tips:
+    /// - Should be > OvershootAcceptanceRadius
+    /// - Typical: 2-5x the ReachedRadius
+    /// - If robot has high inertia/speed: increase to 0.8-1.0
+    /// </summary>
+    public double MovingOvershootDetectionRadius { get; set; } = 0.5;
+
+    /// <summary>
+    /// Distance (meters) from checkpoint at which PID deceleration begins during Moving.
+    /// Default: 5.0m
+    ///
+    /// Meaning: When distance to checkpoint > this value, robot runs at MaxLinearVelocity.
+    /// Below this distance, PID ramps velocity down proportionally.
+    ///
+    /// ↑ Increase (7-10):
+    ///   ✓ Earlier, smoother deceleration
+    ///   ✗ Slower average speed on long paths
+    ///
+    /// ↓ Decrease (2-3):
+    ///   ✓ Faster average speed — stays at max longer
+    ///   ✗ Sharper deceleration, may overshoot on heavy robots
+    ///
+    /// Tuning Tips:
+    /// - Depends on MaxLinearVelocity and robot mass/inertia
+    /// - Rule of thumb: stopping distance ≈ v² / (2 × deceleration)
+    /// - Heavy/fast robot: 7-10m; Light/slow robot: 2-3m
+    /// </summary>
+    public double DecelerationDistance { get; set; } = 5.0;
+
+    /// <summary>
+    /// Maximum linear velocity (m/s) when robot is carrying a load during Moving.
+    /// Default: 0.3 m/s
+    ///
+    /// When hasLoad=true, MaxLinearVelocity is capped at min(MaxLinearVelocity, LoadedMaxLinearVelocity).
+    /// </summary>
+    public double LoadedMaxLinearVelocity { get; set; } = 0.3;
+
+    /// <summary>
+    /// Maximum heading error (degrees) allowed when starting MoveStraight or Docking with a load.
+    /// Default: 10.0 degrees
+    ///
+    /// When hasLoad=true, the robot cannot rotate to correct heading before MoveStraight/Docking.
+    /// If the heading error exceeds this threshold at start, navigation transitions to Error.
+    /// </summary>
+    public double LoadedHeadingErrorThresholdDegrees { get; set; } = 10.0;
+}
+
+public partial class CSharpNavigation : INavigation, IDisposable
+{
+    public bool IsReady { get; private set; }
+    public bool Driving => NavState is NavigationState.Rotating or NavigationState.Moving or NavigationState.Docking or NavigationState.FinePositioning or NavigationState.MovingStraight or NavigationState.SafetyStop;
+    public double VelocityX => VelController.ActualVelocity.Linear;
+    public double VelocityY { get; private set; }
+    public double Omega => VelController.ActualVelocity.Angular;
+    public NavigationState State => NavState;
+    public IReadOnlyList<NavigationNode>? CurrentWaypoints => MovePurePursuit?.Waypoints_Value ?? MoveStraightController?.Waypoints_Value ?? DockToController?.Waypoints_Value;
+
+    // DockTo monitoring properties (read-only, for NavigationMonitor)
+    public bool IsDockingActive => NavState is NavigationState.Docking or NavigationState.FinePositioning;
+    public NavigationNode? DockGoal => DockToController?.Goal;
+    public string DockPhase => NavState switch
+    {
+        NavigationState.Docking => "Approaching",
+        NavigationState.FinePositioning when _finePositioningIsAligning => "Aligning",
+        NavigationState.FinePositioning => "Advancing",
+        _ => ""
+    };
+    public string DockDirection => DockToController?.DockConfig?.DockToDirection.ToString() ?? "";
+    public int DockRetryCount => _finePositioningRetryCount;
+    public int DockMaxRetries => _finePositioningMaxRetries;
+    public int DockWaypointCount => DockToController?.Waypoints_Value?.Count ?? 0;
+    public NavigationNode? DockStartNode => DockToController?.StartNode;
+    public IReadOnlyList<NavigationNode>? DockWaypoints => DockToController?.Waypoints_Value;
+
+    public event Action<NavigationState>? OnNavigationFinished;
+
+    private readonly ILocalization Localization;
+    public readonly IVelocityController VelController;
+    private readonly INavigationConfig NavigationConfig;
+    private readonly ILogger<CSharpNavigation> Logger;
+
+    private NavigationState NavState = NavigationState.Idle;
+    private NavigationState ResumeState = NavigationState.Idle;
+
+    // Safety stop: saves previous state for Refresh() to resume from
+    private NavigationState _safetyStopPreviousState = NavigationState.Idle;
+
+    private WatchThread<CSharpNavigation>? NavThread = null;
+    private const int CycleHandlerMilliseconds = 30;
+
+    private PID? MovePID;
+    private PurePursuit? MovePurePursuit;
+    private DockToController? DockToController;
+    private DockToController? MoveStraightController;
+    private IDetectSession? _dockSession;
+
+    private OrderNode? GoalRotate;
+    private OrderNode? CurrentBaseNode;
+    private HashSet<string> ProcessedRotations = [];
+
+    private double TargetAngle = 0;
+    private PID? RotatePID;
+
+    private readonly NavigationConfig NavCog;
+    private double MaxLinearVelocity = 0;
+
+    // Overshoot detection
+    private double _oldDistanceToGoal = double.MaxValue;
+    private bool _wasApproaching = false;
+
+    // Initial rotation skip
+    private bool _isInitialRotation = false;
+
+    // Fine Positioning state
+    private int _finePositioningRetryCount = 0;
+    private int _finePositioningMaxRetries = 3;
+    private int _finePositioningCycleCount = 0;
+    private int _finePositioningTimeoutMs = 6000;
+    private bool _finePositioningIsAligning = true;
+    private PID? _finePositionRotatePID = null;
+    private RobotDirection _finePositioningDirection = RobotDirection.FORWARD;
+    private bool _fpWasApproaching = false;
+    private int _fpOvershootCounter = 0;
+
+    private int _disposed = 0;
+
+    // HasLoad: robot is carrying a load — affects speed limits, rotation, and FinePositioning
+    private bool _hasLoad = false;
+
+    // Tracks whether velocity (0,0) has already been sent when entering Paused/SafetyStop.
+    // Prevents Navigation from continuously overwriting ManualControl velocity commands.
+    private bool _idleVelocityZeroSent = false;
+
+    private void ResetOvershootState()
+    {
+        _oldDistanceToGoal = double.MaxValue;
+        _wasApproaching = false;
+        _overshootCounter = 0;
+    }
+
+    public CSharpNavigation(IServiceProvider ServiceProvider)
+    {
+        Localization = ServiceProvider.GetRequiredService<ILocalization>();
+        VelController = ServiceProvider.GetRequiredService<IVelocityController>();
+        NavigationConfig = ServiceProvider.GetRequiredService<INavigationConfig>();
+        Logger = ServiceProvider.GetRequiredService<ILogger<CSharpNavigation>>();
+        NavCog = NavigationConfig.GetNavigationConfig();
+        MaxLinearVelocity = NavCog.MaxLinearVelocity;
+    }
+
+    public void Dispose()
+    {
+        if (Interlocked.Exchange(ref _disposed, 1) != 0) return;
+        HandleNavigationStop();
+        Clear();
+        OnNavigationFinished?.Invoke(NavState);
+        GC.SuppressFinalize(this);
+    }
+
+    public void Start()
+    {
+        IsReady = true;
+    }
+
+    public void Stop()
+    {
+        Dispose();
+    }
+
+    protected void HandleNavigationStart()
+    {
+        NavThread = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
+        NavThread.Start();
+    }
+
+    protected void HandleNavigationStop()
+    {
+        NavThread?.Dispose();
+        NavThread = null;
+    }
+
+    public void CancelMovement()
+    {
+        NavState = NavigationState.Canceled;
+        Dispose();
+    }
+
+    public void Move(RobotNet.VDA5050.Order.OrderMsg order, bool hasLoad = false)
+    {
+        var nodes = order.Nodes;
+        var edges = order.Edges;
+        if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
+
+        _hasLoad = hasLoad;
+        NavState = NavigationState.Initializing;
+        if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
+        {
+            NavState = NavigationState.Idle;
+            throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
+        }
+        VelController.LoadConfig();
+        VelController.SetAcceleration(NavCog.Acceleration);
+        VelController.SetDeceleration(NavCog.Deceleration);
+        MovePID = new PID(NavigationConfig.GetMovePidConfig());
+        var ppConfig = NavigationConfig.GetPurepursuitConfig();
+        MovePurePursuit = new PurePursuit(NavigationConfig.GetPurepursuitConfig(), NavigationConfig.GetStanleyConig()).WithPath(nodes, edges, Localization.Theta);
+
+        // Reset overshoot detection state
+        ResetOvershootState();
+        _isInitialRotation = true;
+
+        (_, int index) = MovePurePursuit.OnNode(Localization.X, Localization.Y);
+        if (index >= MovePurePursuit.Waypoints_Value.Count - 1)
+        {
+            NavState = NavigationState.Completed;
+            Dispose();
+            return;
+        }
+
+        // === Local Planner: sinh approach path ===
+        var lpConfig = NavigationConfig.GetLocalPlannerConfig();
+        double heading;
+
+        var approachResult = lpConfig.Enabled
+            ? MovePurePursuit.GenerateAndPrependApproachPath(
+                  Localization.X, Localization.Y, index, lpConfig)
+            : ApproachResult.Disabled;
+
+        if (approachResult == ApproachResult.ApproachGenerated)
+        {
+            // Heading = hướng tiếp tuyến đầu approach curve (P0 → P1)
+            var wp0 = MovePurePursuit.Waypoints_Value[0];
+            var wp1 = MovePurePursuit.Waypoints_Value[1];
+            heading = Math.Atan2(wp1.Y - wp0.Y, wp1.X - wp0.X);
+
+            if (wp0.Direction == RobotDirection.BACKWARD)
+                heading += Math.PI;
+        }
+        else if (approachResult == ApproachResult.AlreadyOnPath)
+        {
+            // Robot đã trên path → dùng path tangent tại closest waypoint
+            var wp = MovePurePursuit.Waypoints_Value[index];
+            var wpNext = MovePurePursuit.Waypoints_Value[index + 1];
+            heading = Math.Atan2(wpNext.Y - wp.Y, wpNext.X - wp.X);
+
+            if (wp.Direction == RobotDirection.BACKWARD)
+                heading += Math.PI;
+        }
+        else
+        {
+            // TooFarFromPath / Disabled: heading hướng về lookahead point
+            double lookahead = (ppConfig.LookaheadMin + ppConfig.LookaheadMax) / 2;
+            var targetPoint = FindLookaheadTarget(index, lookahead);
+            targetPoint ??= MovePurePursuit.Waypoints_Value[^1];
+
+            heading = Math.Atan2(targetPoint.Y - Localization.Y, targetPoint.X - Localization.X);
+            if (targetPoint.Direction == RobotDirection.BACKWARD)
+                heading += Math.PI;
+        }
+
+        heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+        // Cap speed when loaded
+        if (_hasLoad)
+        {
+            MaxLinearVelocity = Math.Min(MaxLinearVelocity, NavCog.LoadedMaxLinearVelocity);
+        }
+
+        Rotate(heading);
+    }
+
+    public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
+    {
+        if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
+
+        _hasLoad = hasLoad;
+        NavState = NavigationState.Initializing;
+        if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
+        {
+            NavState = NavigationState.Idle;
+            throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
+        }
+        VelController.LoadConfig();
+        VelController.SetAcceleration(NavCog.Acceleration);
+        VelController.SetDeceleration(NavCog.Deceleration);
+
+        MovePID = new PID(NavigationConfig.GetMovePidConfig());
+        var straightCfg = NavigationConfig.GetMoveStraightConfig().Clone();
+        if (direction.HasValue) straightCfg.DockToDirection = direction.Value;
+
+        var startNode = new NavigationNode
+        {
+            Id = Guid.NewGuid(),
+            X = Localization.X,
+            Y = Localization.Y,
+        };
+
+        var goalNode = new NavigationNode
+        {
+            Id = Guid.NewGuid(),
+            X = x,
+            Y = y,
+        };
+
+        MoveStraightController = new DockToController(straightCfg).WithPath(startNode, goalNode);
+
+        (_, int index) = MoveStraightController.GetClosestAheadWaypoint(Localization.X, Localization.Y);
+        if (index >= MoveStraightController.Waypoints_Value.Count - 1)
+        {
+            NavState = NavigationState.Completed;
+            Dispose();
+            return;
+        }
+
+        double heading = Math.Atan2(y - Localization.Y, x - Localization.X);
+        if (straightCfg.DockToDirection == RobotDirection.BACKWARD)
+            heading += Math.PI;
+        heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+        ResetOvershootState();
+        MaxLinearVelocity = NavCog.MaxLinearVelocity;
+
+        if (_hasLoad)
+        {
+            // When loaded: no rotation allowed. Check heading error.
+            double headingError = heading - Localization.Theta;
+            if (headingError > Math.PI) headingError -= 2 * Math.PI;
+            else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+            double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
+            if (Math.Abs(headingError) > thresholdRad)
+            {
+                Logger.LogError($"MoveStraight hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold {NavCog.LoadedHeadingErrorThresholdDegrees:F1}°");
+                NavState = NavigationState.Error;
+                Dispose();
+                return;
+            }
+
+            // Skip rotation, go directly to MovingStraight
+            NavState = NavigationState.MovingStraight;
+            HandleNavigationStart();
+        }
+        else
+        {
+            _isInitialRotation = true;
+            Rotate(heading);
+        }
+    }
+
+    public void DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
+    {
+        if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
+        var goal = session.Goal ?? throw new NavigationException("Dock to Goal is not existed");
+        _hasLoad = hasLoad;
+        _dockSession = session;
+        NavState = NavigationState.Initializing;
+
+        if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
+        {
+            NavState = NavigationState.Idle;
+            throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
+        }
+        VelController.LoadConfig();
+        VelController.SetAcceleration(NavCog.Acceleration);
+        VelController.SetDeceleration(NavCog.Deceleration);
+
+
+        MovePID = new PID(NavigationConfig.GetMovePidConfig());
+        var docktoConfig = NavigationConfig.GetDockToConfig().Clone();
+        if (direction.HasValue) docktoConfig.DockToDirection = direction.Value;
+        var currentGoal = new NavigationNode()
+        {
+            Id = Guid.NewGuid(),
+            X = goal.Pose.Position.X,
+            Y = goal.Pose.Position.Y,
+            Speed = NavCog.DockToMaxSpeed,
+            Theta = goal.Pose.Orientation.ToYawRadian(),
+        };
+
+        var startNode = GetDockToStartNode(Localization.X, Localization.Y, currentGoal.X, currentGoal.Y, currentGoal.Theta ?? 0, docktoConfig.DockToLength);
+        DockToController = new DockToController(docktoConfig).WithPath(startNode, currentGoal);
+
+        (_, int index) = DockToController.GetClosestAheadWaypoint(Localization.X, Localization.Y);
+        if (index >= DockToController.Waypoints_Value.Count - 1)
+        {
+            NavState = NavigationState.Completed;
+
+            Dispose();
+            return;
+        }
+
+        var pathAngle = Math.Atan2(currentGoal.Y - startNode.Y, currentGoal.X - startNode.X);
+
+        double frontX = DockToController.Waypoints_Value[index].X + docktoConfig.WheelBase * Math.Cos(pathAngle);
+        double frontY = DockToController.Waypoints_Value[index].Y + docktoConfig.WheelBase * Math.Sin(pathAngle);
+
+        double dx = frontX - Localization.X;
+        double dy = frontY - Localization.Y;
+        double heading = Math.Atan2(dy, dx);
+
+        if (docktoConfig.DockToDirection == RobotDirection.BACKWARD)
+            heading += Math.PI;
+
+        heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+        ResetOvershootState();
+        MaxLinearVelocity = NavCog.DockToMaxSpeed;
+
+        // Load Fine Positioning config from DockToConfig
+        _finePositioningTimeoutMs = docktoConfig.FinePositioningTimeoutMs;
+        _finePositioningMaxRetries = docktoConfig.FinePositioningMaxRetries;
+
+        // Reset Fine Positioning state
+        _finePositioningRetryCount = 0;
+        _finePositioningCycleCount = 0;
+        _finePositioningIsAligning = true;
+        _finePositionRotatePID = null;
+        _finePositioningDirection = RobotDirection.FORWARD;
+        _fpWasApproaching = false;
+        _fpOvershootCounter = 0;
+
+        if (_hasLoad)
+        {
+            // When loaded: no rotation allowed. Check heading error.
+            double headingError = heading - Localization.Theta;
+            if (headingError > Math.PI) headingError -= 2 * Math.PI;
+            else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+            double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
+            if (Math.Abs(headingError) > thresholdRad)
+            {
+                Logger.LogError($"DockTo hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold {NavCog.LoadedHeadingErrorThresholdDegrees:F1}°");
+                NavState = NavigationState.Error;
+                Dispose();
+                return;
+            }
+
+            // Skip rotation, go directly to Docking
+            NavState = NavigationState.Docking;
+            HandleNavigationStart();
+        }
+        else
+        {
+            _isInitialRotation = true;
+            Rotate(heading);
+        }
+    }
+
+    public void Pause()
+    {
+        ResumeState = NavState;
+        _idleVelocityZeroSent = false;
+        NavState = NavigationState.Paused;
+    }
+
+    public void SafetyStop()
+    {
+        _safetyStopPreviousState = NavState;
+        _idleVelocityZeroSent = false;
+        NavState = NavigationState.SafetyStop;
+    }
+
+    public void Refresh()
+    {
+        _idleVelocityZeroSent = false;
+        if (NavState != NavigationState.SafetyStop)
+        {
+            NavState = NavigationState.Idle;
+            return;
+        }
+
+        var prevState = _safetyStopPreviousState;
+        var x = Localization.X;
+        var y = Localization.Y;
+
+        if (prevState is NavigationState.Moving
+            && MovePurePursuit?.Waypoints_Value is { Count: > 2 })
+        {
+            MovePurePursuit.ResetTracking();
+            MovePurePursuit.RebuildPath();
+
+            (_, int index) = MovePurePursuit.OnNode(x, y);
+            if (index >= MovePurePursuit.Waypoints_Value.Count - 1)
+            { NavState = NavigationState.Completed; Dispose(); return; }
+
+            MovePID = new PID(NavigationConfig.GetMovePidConfig());
+            ResetOvershootState();
+
+            // === Local Planner (same logic as Move) ===
+            var lpConfig = NavigationConfig.GetLocalPlannerConfig();
+            var ppConfig = NavigationConfig.GetPurepursuitConfig();
+            double heading;
+
+            var approachResult = lpConfig.Enabled
+                ? MovePurePursuit.GenerateAndPrependApproachPath(
+                      x, y, index, lpConfig)
+                : ApproachResult.Disabled;
+
+            if (approachResult == ApproachResult.ApproachGenerated)
+            {
+                var wp0 = MovePurePursuit.Waypoints_Value[0];
+                var wp1 = MovePurePursuit.Waypoints_Value[1];
+                heading = Math.Atan2(wp1.Y - wp0.Y, wp1.X - wp0.X);
+                if (wp0.Direction == RobotDirection.BACKWARD) heading += Math.PI;
+            }
+            else if (approachResult == ApproachResult.AlreadyOnPath)
+            {
+                var wp = MovePurePursuit.Waypoints_Value[index];
+                var wpNext = MovePurePursuit.Waypoints_Value[index + 1];
+                heading = Math.Atan2(wpNext.Y - wp.Y, wpNext.X - wp.X);
+                if (wp.Direction == RobotDirection.BACKWARD) heading += Math.PI;
+            }
+            else
+            {
+                double lookahead = (ppConfig.LookaheadMin + ppConfig.LookaheadMax) / 2;
+                var target = FindLookaheadTarget(index, lookahead)
+                             ?? MovePurePursuit.Waypoints_Value[^1];
+                heading = Math.Atan2(target.Y - y, target.X - x);
+                if (target.Direction == RobotDirection.BACKWARD) heading += Math.PI;
+            }
+
+            heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+            _isInitialRotation = true;
+            RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+            TargetAngle = heading;
+            NavState = NavigationState.Rotating;
+        }
+        else if (prevState is NavigationState.Docking
+                 && DockToController?.Waypoints_Value is { Count: > 2 })
+        {
+            DockToController.ResetTracking();
+            (_, int index) = DockToController.GetClosestAheadWaypoint(x, y);
+            if (index >= DockToController.Waypoints_Value.Count - 1)
+            { NavState = NavigationState.Completed; Dispose(); return; }
+
+            MovePID = new PID(NavigationConfig.GetMovePidConfig());
+            ResetOvershootState();
+
+            var pathAngle = Math.Atan2(
+                DockToController.Goal.Y - DockToController.Waypoints_Value[0].Y,
+                DockToController.Goal.X - DockToController.Waypoints_Value[0].X);
+            double frontX = DockToController.Waypoints_Value[index].X
+                + DockToController.DockConfig.WheelBase * Math.Cos(pathAngle);
+            double frontY = DockToController.Waypoints_Value[index].Y
+                + DockToController.DockConfig.WheelBase * Math.Sin(pathAngle);
+            double heading = Math.Atan2(frontY - y, frontX - x);
+            if (DockToController.DockConfig.DockToDirection == RobotDirection.BACKWARD)
+                heading += Math.PI;
+            heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+            if (_hasLoad)
+            {
+                // When loaded: check heading error, resume Docking directly without rotation
+                double headingError = heading - Localization.Theta;
+                if (headingError > Math.PI) headingError -= 2 * Math.PI;
+                else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+                double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
+                if (Math.Abs(headingError) > thresholdRad)
+                {
+                    Logger.LogError($"Refresh Docking hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold");
+                    NavState = NavigationState.Error;
+                    Dispose();
+                    return;
+                }
+                NavState = NavigationState.Docking;
+            }
+            else
+            {
+                _isInitialRotation = true;
+                RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+                TargetAngle = heading;
+                NavState = NavigationState.Rotating;
+            }
+        }
+        else if (prevState is NavigationState.MovingStraight
+                 && MoveStraightController?.Waypoints_Value is { Count: > 2 })
+        {
+            MoveStraightController.ResetTracking();
+            (_, int index) = MoveStraightController.GetClosestAheadWaypoint(x, y);
+            if (index >= MoveStraightController.Waypoints_Value.Count - 1)
+            { NavState = NavigationState.Completed; Dispose(); return; }
+
+            MovePID = new PID(NavigationConfig.GetMovePidConfig());
+            ResetOvershootState();
+
+            double heading = Math.Atan2(
+                MoveStraightController.Goal.Y - y,
+                MoveStraightController.Goal.X - x);
+            if (MoveStraightController.DockConfig.DockToDirection == RobotDirection.BACKWARD)
+                heading += Math.PI;
+            heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+            if (_hasLoad)
+            {
+                // When loaded: check heading error, resume MovingStraight directly without rotation
+                double headingError = heading - Localization.Theta;
+                if (headingError > Math.PI) headingError -= 2 * Math.PI;
+                else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+                double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
+                if (Math.Abs(headingError) > thresholdRad)
+                {
+                    Logger.LogError($"Refresh MovingStraight hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold");
+                    NavState = NavigationState.Error;
+                    Dispose();
+                    return;
+                }
+                NavState = NavigationState.MovingStraight;
+            }
+            else
+            {
+                _isInitialRotation = true;
+                RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+                TargetAngle = heading;
+                NavState = NavigationState.Rotating;
+            }
+        }
+        else if (prevState is NavigationState.Rotating)
+        {
+            RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+            NavState = NavigationState.Rotating;
+        }
+        else if (prevState is NavigationState.FinePositioning)
+        {
+            _finePositioningCycleCount = 0;
+            _finePositioningIsAligning = true;
+            _finePositionRotatePID = null;
+            NavState = NavigationState.Docking;
+        }
+        else
+        {
+            NavState = NavigationState.Idle;
+        }
+    }
+
+    public void RefreshOrder(Node[] nodes, Edge[] edges)
+    {
+    }
+
+    public void Resume()
+    {
+        if (ResumeState == NavigationState.FinePositioning)
+        {
+            _finePositioningCycleCount = 0;
+        }
+        _idleVelocityZeroSent = false;
+        NavState = ResumeState;
+    }
+
+    public void Rotate(double angle)
+    {
+        if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
+        
+        if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
+        {
+            NavState = NavigationState.Idle;
+            throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
+        }
+        
+        RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+        TargetAngle = SpaceCompute.NormalizeRadianAngle(angle);
+        NavState = NavigationState.Rotating;
+        HandleNavigationStart();
+    }
+
+    public void SetSpeed(double speed)
+    {
+        MaxLinearVelocity = _hasLoad ? Math.Min(speed, NavCog.LoadedMaxLinearVelocity) : speed;
+    }
+
+    protected void UpdateGoal(string goalId)
+    {
+        MovePurePursuit?.UpdateGoal(goalId);
+    }
+
+    public void UpdateOrder(string newBaseNodeId)
+    {
+        var newBaseNode = MovePurePursuit?.OrderNodes.FirstOrDefault(n => n.NodeId == newBaseNodeId);
+        if (newBaseNode is not null && newBaseNode.NodeId != CurrentBaseNode?.NodeId)
+        {
+            CurrentBaseNode = newBaseNode;
+            var newGoalRotate = FindNextRotateGoal();
+            if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
+            {
+                GoalRotate = newGoalRotate;
+                UpdateGoal(newGoalRotate.NodeId);
+            }
+        }
+    }
+
+    private void Clear()
+    {
+        VelController.SetVelocity(0, 0);
+        CurrentBaseNode = null;
+        MovePurePursuit = null;
+        MovePID = null;
+        RotatePID = null;
+        GoalRotate = null;
+        DockToController = null;
+        MoveStraightController = null;
+        _dockSession = null;
+        ProcessedRotations = [];
+        ResetOvershootState();
+        _isInitialRotation = false;
+        _finePositioningRetryCount = 0;
+        _finePositioningCycleCount = 0;
+        _finePositioningIsAligning = true;
+        _finePositionRotatePID = null;
+        _finePositioningDirection = RobotDirection.FORWARD;
+        _fpWasApproaching = false;
+        _fpOvershootCounter = 0;
+        _hasLoad = false;
+        _idleVelocityZeroSent = false;
+    }
+
+    /// <summary>
+    /// Tìm target point tại lookahead distance từ vị trí hiện tại trên path.
+    /// Nội suy giữa các waypoint để tạo điểm mượt.
+    /// </summary>
+    private NavigationNode? FindLookaheadTarget(int startIndex, double lookaheadDistance)
+    {
+        if (MovePurePursuit is null || startIndex >= MovePurePursuit.Waypoints_Value.Count - 1)
+            return null;
+
+        double accumulatedDistance = 0;
+
+        for (int i = startIndex; i < MovePurePursuit.Waypoints_Value.Count - 1; i++)
+        {
+            double dx = MovePurePursuit.Waypoints_Value[i + 1].X - MovePurePursuit.Waypoints_Value[i].X;
+            double dy = MovePurePursuit.Waypoints_Value[i + 1].Y - MovePurePursuit.Waypoints_Value[i].Y;
+            double segmentLength = Math.Sqrt(dx * dx + dy * dy);
+
+            if (accumulatedDistance + segmentLength >= lookaheadDistance)
+            {
+                double t = (lookaheadDistance - accumulatedDistance) / segmentLength;
+                return new NavigationNode
+                {
+                    Id = Guid.NewGuid(),
+                    NodeId = string.Empty,
+                    X = MovePurePursuit.Waypoints_Value[i].X + t * dx,
+                    Y = MovePurePursuit.Waypoints_Value[i].Y + t * dy,
+                    Direction = MovePurePursuit.Waypoints_Value[i].Direction
+                };
+            }
+
+            accumulatedDistance += segmentLength;
+        }
+
+        return null;
+    }
+
+    /// <summary>
+    /// Tìm node có IsWaitingRotate đầu tiên trong path từ currentNode đến currentGoal
+    /// </summary>
+    protected OrderNode? FindNextRotateGoal()
+    {
+        if (CurrentBaseNode == null || MovePurePursuit is null || MovePurePursuit.OrderNodes.Length == 0) return null;
+
+        int goalIndex = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == CurrentBaseNode.NodeId);
+        if (goalIndex == -1) return null;
+
+        int lastNodeIdx = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == GoalRotate?.NodeId);
+        lastNodeIdx = lastNodeIdx == -1 ? 0 : lastNodeIdx + 1;
+
+        // Tìm từ node hiện tại đến goal
+        for (int i = lastNodeIdx; i <= goalIndex; i++)
+        {
+            var node = MovePurePursuit.OrderNodes[i];
+
+            // Tìm node có IsWaitingRotate và chưa xử lý
+            if (node.IsWaitRotating && !ProcessedRotations.Contains(node.NodeId))
+            {
+                return node;
+            }
+        }
+        return CurrentBaseNode;
+    }
+
+    private NavigationNode GetDockToStartNode(double x, double y, double goalX, double goalY, double goalTheta, double length)
+    {
+        // Hướng vuông góc với theta
+        double perpAngle = SpaceCompute.NormalizeRadianAngle(goalTheta + Math.PI / 2);
+
+        // Hai endpoint của đoạn thẳng, cách goal ±length theo hướng vuông góc
+        double ep1X = goalX + length * Math.Cos(perpAngle);
+        double ep1Y = goalY + length * Math.Sin(perpAngle);
+
+        double ep2X = goalX - length * Math.Cos(perpAngle);
+        double ep2Y = goalY - length * Math.Sin(perpAngle);
+
+        // Chọn endpoint gần (x, y) nhất
+        double dist1Sq = (ep1X - x) * (ep1X - x) + (ep1Y - y) * (ep1Y - y);
+        double dist2Sq = (ep2X - x) * (ep2X - x) + (ep2Y - y) * (ep2Y - y);
+
+        double startX = dist1Sq <= dist2Sq ? ep1X : ep2X;
+        double startY = dist1Sq <= dist2Sq ? ep1Y : ep2Y;
+
+        return new NavigationNode
+        {
+            Id = Guid.NewGuid(),
+            X = startX,
+            Y = startY,
+        };
+    }
+}