using RobotNet10.Common;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.ConfigManager;
using RobotNet10.RobotApp.Services.Navigation.CSharp;
using RobotNet10.RobotApp.Services.Simulation;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.Shared.Geometry;

namespace RobotNet10.RobotApp.Services.Navigation;

public partial class CSharpNavigation
{
    private bool IsBackToPath = false;
    private double? BackToAngle;
    private int _overshootCounter = 0;
    private readonly int _overshootOut = 5;

    private void Rotating()
    {
        if (RotatePID is not null)
        {
            double Error = SpaceCompute.NormalizeRadianAngle(TargetAngle - Localization.Theta);

            // Skip initial rotation nếu heading error đủ nhỏ
            if (_isInitialRotation)
            {
                double initialThresholdRad = (NavCog?.InitialRotationThreshold ?? 5.0) * Math.PI / 180.0;
                if (Math.Abs(Error) < initialThresholdRad)
                {
                    _isInitialRotation = false;
                    VelController.SetVelocity(0, 0);
                    if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2)
                    {
                        ResetOvershootState();
                        NavState = NavigationState.Moving;
                    }
                    else if (MoveStraightController is not null && MoveStraightController.Waypoints_Value is not null && MoveStraightController.Waypoints_Value.Count > 2)
                    {
                        ResetOvershootState();
                        NavState = NavigationState.MovingStraight;
                    }
                    else if (DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Waypoints_Value.Count > 2)
                    {
                        ResetOvershootState();
                        NavState = NavigationState.Docking;
                    }
                    else
                    {
                        NavState = NavigationState.Completed;
                        Logger.LogInformation($"Navigation Reached initial heading: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
                        Dispose();
                    }
                    return;
                }
            }

            double headingToleranceRad = (NavCog?.HeadingTolerance ?? 3.0) * Math.PI / 180.0;
            if (Math.Abs(Error) < headingToleranceRad)
            {
                _isInitialRotation = false;
                if (IsBackToPath && BackToAngle.HasValue)
                {
                    TargetAngle = BackToAngle.Value;
                    BackToAngle = null;
                    IsBackToPath = false;
                }
                else
                {
                    ResetOvershootState();
                    if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2)
                    {
                        NavState = NavigationState.Moving;
                    }
                    else if (MoveStraightController is not null && MoveStraightController.Waypoints_Value is not null && MoveStraightController.Waypoints_Value.Count > 2)
                    {
                        NavState = NavigationState.MovingStraight;
                    }
                    else if (DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Waypoints_Value.Count > 2)
                    {
                        NavState = NavigationState.Docking;
                    }
                    else
                    {
                        NavState = NavigationState.Completed;
                        Logger.LogInformation($"Navigation Reached heading: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
                        Dispose();
                    }
                }
            }
            else
            {
                var SpeedCal = RotatePID.PID_step(Error, NavCog?.RotateAngularVelocity ?? 0.1, -(NavCog?.RotateAngularVelocity ?? 0.1), CycleHandlerMilliseconds / 1000.0);
                VelController.SetVelocity(0, SpeedCal);
            }
        }
    }

    private void Moving()
    {
        if (MovePID is not null && MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit?.OrderNodes is not null && MovePurePursuit.OrderNodes.Length > 1 && GoalRotate is not null)
        {
            var DistanceToGoal = Math.Sqrt(Math.Pow(Localization.X - MovePurePursuit.OrderNodes[^1].X, 2) + Math.Pow(Localization.Y - MovePurePursuit.OrderNodes[^1].Y, 2));
            var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Localization.X - GoalRotate.X, 2) + Math.Pow(Localization.Y - GoalRotate.Y, 2));
            var reachedRadius = NavCog?.ReachedRadius ?? 0.05;
            var deviation = GoalRotate.NodeId == MovePurePursuit.OrderNodes[^1].NodeId ? reachedRadius : GoalRotate.AllowedDeviationXY ?? 0.1;

            // Overshoot detection: phát hiện robot đi qua goal
            var overshootDetectionRadius = NavCog?.MovingOvershootDetectionRadius ?? 0.5;
            if (DistanceToGoal < overshootDetectionRadius)
            {
                if (DistanceToGoal < _oldDistanceToGoal)
                {
                    _wasApproaching = true;
                    _overshootCounter = 0;
                }
                else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
                {
                    _overshootCounter++;
                    if (_wasApproaching && _overshootCounter >= _overshootOut)
                    {
                        _wasApproaching = false;
                        VelController.SetVelocity(0, 0);

                        // Kiểm tra vị trí overshoot có chấp nhận được không
                        double acceptanceRadius = NavCog?.OvershootAcceptanceRadius ?? 0.15;
                        if (DistanceToGoal > acceptanceRadius)
                        {
                            Logger.LogError($"Moving overshoot too far: distance={DistanceToGoal:F4}m > acceptance={acceptanceRadius}m");
                            NavState = NavigationState.Error;
                            Dispose();
                            return;
                        }

                        Logger.LogWarning($"Overshoot detected at distance {DistanceToGoal:F4}m (within acceptance={acceptanceRadius}m), transitioning to final rotation");
                        if (MovePurePursuit.OrderNodes[^1].Theta is { } overshootTheta)
                        {
                            TargetAngle = overshootTheta;
                            NavState = NavigationState.Rotating;
                            RotatePID?.Reset();
                            MovePurePursuit = null;
                        }
                        else
                        {
                            NavState = NavigationState.Completed;
                            Dispose();
                        }
                        return;
                    }
                }
                else _overshootCounter = 0;
            }
            _oldDistanceToGoal = DistanceToGoal;

            if (DistanceToCheckingNode > deviation)
            {
                double dt = CycleHandlerMilliseconds / 1000.0;
                double decelerationDist = NavCog?.DecelerationDistance ?? 5.0;
                double maxLinearVel = DistanceToCheckingNode > decelerationDist
                    ? MaxLinearVelocity
                    : MovePID.PID_step(DistanceToCheckingNode, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
                maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);

                (double linearVelActual, _) = VelController.ActualVelocity;
                (double LinearVel, double AngularVel) = MovePurePursuit.PurePursuit_step(Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);

                // Clamp angular velocity
                AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);

                // Clamp linear velocity (giữ dấu cho backward)
                var linearSign = Math.Sign(LinearVel);
                LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);

                VelController.SetVelocity(LinearVel, AngularVel);
            }
            else if (DistanceToGoal < reachedRadius)
            {
                VelController.SetVelocity(0, 0);
                if (MovePurePursuit.OrderNodes[^1].Theta is { } theta)
                {
                    TargetAngle = theta;
                    NavState = NavigationState.Rotating;
                    RotatePID?.Reset();
                    MovePurePursuit = null;
                }
                else
                {
                    NavState = NavigationState.Completed;
                    Logger.LogInformation($"Navigation Reached: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
                    Dispose();
                }
            }
            else
            {
                double? targetAngle = null;
                if (GoalRotate.Theta is { } theta)
                {
                    targetAngle = theta;
                    ProcessedRotations.Add(GoalRotate.NodeId);
                    BackToAngle = GoalRotate?.ContinueTheta;
                    IsBackToPath = true;
                }

                var newGoalRotate = FindNextRotateGoal();
                if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
                {
                    GoalRotate = newGoalRotate;
                    UpdateGoal(newGoalRotate.NodeId);
                }

                if (targetAngle.HasValue)
                {
                    TargetAngle = targetAngle.Value;
                    NavState = NavigationState.Rotating;
                    RotatePID?.Reset();
                }
            }
        }
    }

    private void Docking()
    {
        if (MovePID is not null && DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Goal != null)
        {
            var dockCfg = DockToController.DockConfig;

            // --- Continuous goal update from detection session ---
            TryUpdateDockGoalFromSession(NavCog.DockToMaxSpeed);

            var DistanceToGoal = Math.Sqrt(Math.Pow(Localization.X - DockToController.Goal.X, 2) + Math.Pow(Localization.Y - DockToController.Goal.Y, 2));
            var deviation = dockCfg.ReachedRadius;

            // Overshoot detection: phát hiện robot đi qua goal
            var dockOvershootRadius = dockCfg.DockingOvershootDetectionRadius;
            if (DistanceToGoal < dockOvershootRadius)
            {
                if (DistanceToGoal < _oldDistanceToGoal)
                {
                    _wasApproaching = true;
                    _overshootCounter = 0;
                }
                else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
                {
                    _overshootCounter++;
                    if (_wasApproaching && _overshootCounter >= _overshootOut)
                    {
                        _wasApproaching = false;
                        _overshootCounter = 0;
                        VelController.SetVelocity(0, 0);

                        if (_hasLoad)
                        {
                            // When loaded: no FinePositioning allowed, go to Error
                            Logger.LogError($"Docking overshoot with load at distance {DistanceToGoal:F4}m. No FinePositioning allowed.");
                            NavState = NavigationState.Error;
                            Dispose();
                            return;
                        }

                        Logger.LogWarning(
                            $"Overshoot detected at distance {DistanceToGoal:F4}m. " +
                            $"Entering FinePositioning (attempt {_finePositioningRetryCount + 1}/{_finePositioningMaxRetries}).");

                        // Transition to FinePositioning instead of giving up
                        _finePositioningCycleCount = 0;
                        _finePositioningIsAligning = true;
                        _finePositionRotatePID = new PID(NavigationConfig.GetRotatePidConfig());
                        _oldDistanceToGoal = double.MaxValue;
                        _fpWasApproaching = false;
                        _fpOvershootCounter = 0;
                        NavState = NavigationState.FinePositioning;
                        return;
                    }
                }
                else _overshootCounter = 0;
            }
            _oldDistanceToGoal = DistanceToGoal;

            if (DistanceToGoal > deviation)
            {
                double dt = CycleHandlerMilliseconds / 1000.0;
                double dockDecelerationDist = dockCfg.DecelerationDistance;
                double maxLinearVel = DistanceToGoal > dockDecelerationDist
                    ? MaxLinearVelocity
                    : MovePID.PID_step(DistanceToGoal, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
                maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);

                (double linearVelActual, _) = VelController.ActualVelocity;
                (double LinearVel, double AngularVel) = DockToController.FinalApproachController(Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);

                // Clamp angular velocity
                AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);

                // Clamp linear velocity (giữ dấu cho backward)
                var linearSign = Math.Sign(LinearVel);
                LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);

                VelController.SetVelocity(LinearVel, AngularVel);
            }
            else
            {
                Console.WriteLine($"Dock To reached. Pose= ({Localization.X} - {Localization.Y} - {Localization.Theta}), Distance to goal: {DistanceToGoal}");
                VelController.SetVelocity(0, 0);
                if (!_hasLoad && DockToController.Goal.Theta is { } theta)
                {
                    TargetAngle = theta;
                    NavState = NavigationState.Rotating;
                    RotatePID?.Reset();
                    DockToController = null;
                }
                else
                {
                    NavState = NavigationState.Completed;
                    Logger.LogInformation($"DockTo Reached: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
                    Dispose();
                }
            }
        }
    }

    private void MovingStraight()
    {
        if (MovePID is not null && MoveStraightController is not null
            && MoveStraightController.Waypoints_Value is not null && MoveStraightController.Goal != null)
        {
            var cfg = MoveStraightController.DockConfig;
            var DistanceToGoal = Math.Sqrt(
                Math.Pow(Localization.X - MoveStraightController.Goal.X, 2) +
                Math.Pow(Localization.Y - MoveStraightController.Goal.Y, 2));
            var deviation = cfg.ReachedRadius;

            // Overshoot detection (same pattern as Moving)
            var overshootRadius = cfg.DockingOvershootDetectionRadius;
            if (DistanceToGoal < overshootRadius)
            {
                if (DistanceToGoal < _oldDistanceToGoal)
                {
                    _wasApproaching = true;
                    _overshootCounter = 0;
                }
                else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
                {
                    _overshootCounter++;
                    if (_wasApproaching && _overshootCounter >= _overshootOut)
                    {
                        _wasApproaching = false;
                        VelController.SetVelocity(0, 0);

                        double acceptanceRadius = NavCog?.OvershootAcceptanceRadius ?? 0.15;
                        if (DistanceToGoal > acceptanceRadius)
                        {
                            Logger.LogError($"MovingStraight overshoot too far: distance={DistanceToGoal:F4}m > acceptance={acceptanceRadius}m");
                            NavState = NavigationState.Error;
                            Dispose();
                            return;
                        }

                        Logger.LogWarning($"MovingStraight overshoot at {DistanceToGoal:F4}m (within acceptance). Completing.");
                        NavState = NavigationState.Completed;
                        Dispose();
                        return;
                    }
                }
                else _overshootCounter = 0;
            }
            _oldDistanceToGoal = DistanceToGoal;

            if (DistanceToGoal > deviation)
            {
                double dt = CycleHandlerMilliseconds / 1000.0;
                double decelerationDist = cfg.DecelerationDistance;
                double maxLinearVel = DistanceToGoal > decelerationDist
                    ? MaxLinearVelocity
                    : MovePID.PID_step(DistanceToGoal, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
                maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);

                (double linearVelActual, _) = VelController.ActualVelocity;
                (double LinearVel, double AngularVel) = MoveStraightController.FinalApproachController(
                    Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);

                AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);

                // Clamp linear velocity (giữ dấu cho backward)
                var linearSign = Math.Sign(LinearVel);
                LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);

                VelController.SetVelocity(LinearVel, AngularVel);
            }
            else
            {
                Logger.LogInformation($"MoveStraight reached. Pose=({Localization.X} - {Localization.Y} - {Localization.Theta}), Distance: {DistanceToGoal}");
                VelController.SetVelocity(0, 0);
                NavState = NavigationState.Completed;
                Dispose();
            }
        }
    }

    /// <summary>
    /// FinePositioning: Cơ chế retry chính xác khi Docking overshoot.
    /// Phase 1 (Align): Xoay tại chỗ hướng về goal theo góc nhỏ nhất (forward hoặc backward).
    ///   Hướng được lock 1 lần khi vào align, không thay đổi trong suốt quá trình xoay.
    /// Phase 2 (Advance): Tiến thẳng về goal ở DockToRetrySpeed với P-correction trên angular velocity.
    /// Success: distance ≤ ReachedRadius → final rotation hoặc Completed.
    /// Failure: hết retry hoặc timeout → Error.
    /// </summary>
    private void FinePositioning()
    {
        if (DockToController?.Goal is not { } goal)
        {
            Logger.LogError("FinePositioning: DockToController or Goal is null. Aborting.");
            NavState = NavigationState.Error;
            Dispose();
            return;
        }

        // Continuously update goal from dock session (same as Docking phase)
        var updatedGoal = TryUpdateDockGoalFromSession(NavCog.DockToRetrySpeed);
        if (updatedGoal is not null) goal = updatedGoal;

        // Per-attempt time-based timeout
        _finePositioningCycleCount++;
        int elapsedMs = _finePositioningCycleCount * CycleHandlerMilliseconds;
        if (elapsedMs > _finePositioningTimeoutMs)
        {
            _finePositioningRetryCount++;
            Logger.LogWarning(
                $"FinePositioning attempt {_finePositioningRetryCount} timed out " +
                $"(elapsed {elapsedMs}ms > {_finePositioningTimeoutMs}ms).");

            if (_finePositioningRetryCount >= _finePositioningMaxRetries)
            {
                Logger.LogError(
                    $"FinePositioning exhausted all {_finePositioningMaxRetries} retries. Transitioning to Error.");
                VelController.SetVelocity(0, 0);
                NavState = NavigationState.Error;
                Dispose();
            }
            else
            {
                VelController.SetVelocity(0, 0);
                _finePositioningCycleCount = 0;
                _finePositioningIsAligning = true;
                _finePositionRotatePID?.Reset();
                _oldDistanceToGoal = double.MaxValue;
                _fpWasApproaching = false;
                _fpOvershootCounter = 0;
            }
            return;
        }

        double robotX = Localization.X;
        double robotY = Localization.Y;
        double robotTheta = Localization.Theta;

        double dx = goal.X - robotX;
        double dy = goal.Y - robotY;
        double distanceToGoal = Math.Sqrt(dx * dx + dy * dy);

        // Success check
        double reachedRadius = DockToController.DockConfig.ReachedRadius;
        if (distanceToGoal <= reachedRadius)
        {
            Logger.LogInformation(
                $"FinePositioning SUCCESS: distance={distanceToGoal:F4}m, " +
                $"Pose({robotX:F3}, {robotY:F3}, {robotTheta:F3}) → Goal({goal.X:F3}, {goal.Y:F3}, {goal.Theta:F3}, {_dockSession?.Goal?.Header.FrameId}, {_dockSession?.Goal?.Header.Stamp}). ");
            VelController.SetVelocity(0, 0);

            if (goal.Theta is { } finalTheta)
            {
                TargetAngle = finalTheta;
                NavState = NavigationState.Rotating;
                RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
                DockToController = null;
            }
            else
            {
                NavState = NavigationState.Completed;
                DockToController = null;
                Dispose();
            }
            return;
        }

        // Chọn hướng tiếp cận có góc xoay nhỏ nhất (forward hoặc backward)
        // Direction chỉ được chọn 1 lần khi vào align phase (cycle đầu tiên),
        // sau đó giữ nguyên suốt align + advance để tránh flip do sensor noise.
        double rawHeading = Math.Atan2(dy, dx);

        if (_finePositioningIsAligning && _finePositioningCycleCount == 1)
        {
            double fwdErr = SpaceCompute.NormalizeRadianAngle(rawHeading) - robotTheta;
            if (fwdErr > Math.PI) fwdErr -= 2 * Math.PI;
            else if (fwdErr < -Math.PI) fwdErr += 2 * Math.PI;

            double bwdErr = SpaceCompute.NormalizeRadianAngle(rawHeading + Math.PI) - robotTheta;
            if (bwdErr > Math.PI) bwdErr -= 2 * Math.PI;
            else if (bwdErr < -Math.PI) bwdErr += 2 * Math.PI;

            _finePositioningDirection = Math.Abs(fwdErr) <= Math.Abs(bwdErr)
                ? RobotDirection.FORWARD
                : RobotDirection.BACKWARD;
        }

        // Tính heading error theo hướng đã lock
        double targetHeading = _finePositioningDirection == RobotDirection.BACKWARD
            ? SpaceCompute.NormalizeRadianAngle(rawHeading + Math.PI)
            : SpaceCompute.NormalizeRadianAngle(rawHeading);

        double headingError = targetHeading - robotTheta;
        if (headingError > Math.PI) headingError -= 2 * Math.PI;
        else if (headingError < -Math.PI) headingError += 2 * Math.PI;

        double FineAlignThresholdRad = (DockToController.DockConfig.FineAlignThresholdDegrees) * Math.PI / 180.0;
        double ReAlignThresholdRad = (DockToController.DockConfig.ReAlignThresholdDegrees) * Math.PI / 180.0;

        // Overshoot detection during advance phase — consecutive increase pattern
        if (!_finePositioningIsAligning)
        {
            if (distanceToGoal < _oldDistanceToGoal)
            {
                _fpWasApproaching = true;
                _fpOvershootCounter = 0;
            }
            else if (_fpWasApproaching && distanceToGoal > _oldDistanceToGoal)
            {
                _fpOvershootCounter++;
                if (_fpOvershootCounter >= DockToController.DockConfig.FinePositioningOvershootCount)
                {
                    _finePositioningRetryCount++;
                    _fpWasApproaching = false;
                    _fpOvershootCounter = 0;
                    VelController.SetVelocity(0, 0);

                    if (_finePositioningRetryCount >= _finePositioningMaxRetries)
                    {
                        Logger.LogError("FinePositioning: repeated overshoot during advance. Giving up.");
                        NavState = NavigationState.Error;
                        Dispose();
                        return;
                    }

                    Logger.LogWarning(
                        $"FinePositioning: overshoot during advance (dist={distanceToGoal:F4}m). " +
                        $"Retry {_finePositioningRetryCount}/{_finePositioningMaxRetries}.");
                    _finePositioningIsAligning = true;
                    _finePositionRotatePID?.Reset();
                    _finePositioningCycleCount = 0;
                    _oldDistanceToGoal = double.MaxValue;
                    return;
                }
            }
            else
            {
                _fpOvershootCounter = 0;
            }
        }
        _oldDistanceToGoal = distanceToGoal;

        // Phase 1: ALIGN — xoay tại chỗ theo góc nhỏ nhất
        if (_finePositioningIsAligning)
        {
            if (Math.Abs(headingError) < FineAlignThresholdRad)
            {
                _finePositioningIsAligning = false;
                VelController.SetVelocity(0, 0);
                Logger.LogInformation(
                    $"FinePositioning aligned ({_finePositioningDirection}): " +
                    $"headingError={headingError * 180 / Math.PI:F1}°. Advancing.");
            }
            else
            {
                _finePositionRotatePID ??= new PID(NavigationConfig.GetRotatePidConfig());
                double rotateAngularVel = NavCog?.DockToRotateSpeed ?? 0.05;
                double angularCmd = _finePositionRotatePID.PID_step(
                    headingError, rotateAngularVel, -rotateAngularVel, CycleHandlerMilliseconds / 1000.0);
                VelController.SetVelocity(0, angularCmd);
                Console.WriteLine(
                    $"FinePos-Align({_finePositioningDirection}): " +
                    $"HeadErr={headingError * 180 / Math.PI:F1}°, AngVel={angularCmd:F4}, Dist={distanceToGoal:F4}m");
            }
            return;
        }

        // Phase 2: ADVANCE — tiến thẳng về goal theo hướng đã chọn ở Phase 1
        if (Math.Abs(headingError) > ReAlignThresholdRad)
        {
            _finePositioningIsAligning = true;
            _finePositionRotatePID?.Reset();
            VelController.SetVelocity(0, 0);
            Logger.LogWarning($"FinePositioning heading drift: {headingError * 180 / Math.PI:F1}°. Re-aligning.");
            return;
        }

        double minLinVel = NavCog?.DockToRetrySpeed ?? 0.05;
        double linearCmd = _finePositioningDirection == RobotDirection.BACKWARD ? -minLinVel : minLinVel;

        // Corrective angular velocity proportional to heading error
        // Dùng Max(|actualVel|, minLinVel) để đảm bảo correction không bằng 0 khi vừa bắt đầu advance
        (double linearVelActual, _) = VelController.ActualVelocity;
        double effectiveVel = Math.Max(Math.Abs(linearVelActual), minLinVel);
        double headingGain = DockToController.DockConfig.AdvanceHeadingCorrectionGain;
        double dockAdvanceMaxAngVel = DockToController.DockConfig.DockToAdvanceMaxAngularVelocity;
        double advanceAngularCorrection = Math.Clamp(headingError * headingGain * effectiveVel, -dockAdvanceMaxAngVel, dockAdvanceMaxAngVel);

        VelController.SetVelocity(linearCmd, advanceAngularCorrection);
        Console.WriteLine(
            $"FinePos-Advance({_finePositioningDirection}): LinVel={linearCmd:F4}, AngCorr={advanceAngularCorrection:F4}, " +
            $"Dist={distanceToGoal:F4}m, HeadErr={headingError * 180 / Math.PI:F1}°");
    }

    private NavigationNode? TryUpdateDockGoalFromSession(double speed)
    {
        if (_dockSession is null || DockToController?.DockConfig is not { } dockCfg)
            return null;

        var snapshot = _dockSession.Goal;
        if (!snapshot.HasValue) return null;

        var pose = snapshot.Value.Pose;

        var oldGoal = DockToController!.Goal;
        double dxGoal = pose.Position.X - oldGoal.X;
        double dyGoal = pose.Position.Y - oldGoal.Y;
        double distShift = Math.Sqrt(dxGoal * dxGoal + dyGoal * dyGoal);

        double newTheta = pose.Orientation.ToYawRadian();
        double oldTheta = oldGoal.Theta ?? 0;
        double angleShift = Math.Abs(SpaceCompute.NormalizeRadianAngle(newTheta - oldTheta));
        double maxAngleShiftRad = dockCfg.MaxGoalAngleShiftDegrees * Math.PI / 180.0;

        if (distShift > dockCfg.MaxGoalPositionShift || angleShift > maxAngleShiftRad)
            return null;

        var updatedGoal = new NavigationNode()
        {
            Id = Guid.NewGuid(),
            X = pose.Position.X,
            Y = pose.Position.Y,
            Speed = speed,
            Theta = newTheta,
        };

        var newStartNode = GetDockToStartNode(
            Localization.X, Localization.Y,
            updatedGoal.X, updatedGoal.Y,
            updatedGoal.Theta ?? 0,
            dockCfg.DockToLength);

        DockToController.WithPath(newStartNode, updatedGoal);
        return updatedGoal;
    }

    private void NavigationHandler()
    {
        try
        {
            switch (NavState)
            {
                case NavigationState.Rotating:
                    Rotating();
                    break;
                case NavigationState.Moving:
                    Moving();
                    break;
                case NavigationState.Docking:
                    Docking();
                    break;
                case NavigationState.MovingStraight:
                    MovingStraight();
                    break;
                case NavigationState.FinePositioning:
                    FinePositioning();
                    break;
                case NavigationState.Paused:
                case NavigationState.SafetyStop:
                    if (!_idleVelocityZeroSent)
                    {
                        VelController.SetVelocity(0, 0);
                        _idleVelocityZeroSent = true;
                    }
                    break;
                default:
                    break;
            }
        }
        catch (Exception ex)
        {
            NavState = NavigationState.Error;
            Dispose();
            Logger.LogError($"Error in DifferentialNavigation: {ex.Message}");
        }
    }
}