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,
        };
    }
}