StandardSence/StandardScene.Magnetic/Tasking/Fass2TaskStateMachine.cs

using SimpleCore;
using SimpleCore.PropType;
using StandardScene.Magnetic.Protocol;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;

namespace StandardScene.Magnetic.Tasking
{
    public sealed class Fass2TaskCallbacks
    {
        public Func<int, int, double, Fass2TaskPlan> BuildPlan { get; set; }
        public Action<Fass2NodeMessage[], ulong> SendNodes { get; set; }
        public Action<Fass2NodeMessage, ulong> SendAction { get; set; }
        public Action<byte, ushort> SendControl { get; set; }
        public Func<int, ushort> ResolveNodeId { get; set; }
        public Func<ushort, Site> ResolveSite { get; set; }
        public Action<string> Log { get; set; }
        public Action<Fass2TaskContext> Persist { get; set; }
        public Action ClearPersisted { get; set; }
        public Func<ulong> AllocateTaskId { get; set; }
        public Func<ulong> AllocateActionId { get; set; }

        /// <summary>规划完成后 Forecast 建序（seqScope/pendingLocks）。</summary>
        public Action<IReadOnlyList<int>, int> PrepareTraffic { get; set; }

        /// <summary>返回当前可下发窗口长度（已锁站点数，含当前站）。</summary>
        public Func<IReadOnlyList<int>, int, int, int> EnsureTrafficWindow { get; set; }

        /// <summary>段完成后释放刚离开的站点。</summary>
        public Action<int> LeaveTraffic { get; set; }

        /// <summary>按路径索引释放后方锁点（keepFromIndex 之前）。</summary>
        public Action<IReadOnlyList<int>, int> ReleaseTrafficBehind { get; set; }

        /// <summary>重连对齐：Reset 当前站并从 fromIndex 重建前方窗口。</summary>
        public Action<IReadOnlyList<int>, int> RebaseTraffic { get; set; }

        /// <summary>全程完成，保留终点 holding。</summary>
        public Action<int> FinalizeTraffic { get; set; }

        /// <summary>管控停止到站后，判断同区他车占用是否已允许放行本车。</summary>
        public Func<int, Fass2ControlReleaseCheck> CheckControlRelease { get; set; }
    }

    public sealed class Fass2TaskTickResult
    {
        public Fass2TaskPhase Phase { get; set; }
        public bool Dispatched { get; set; }
        public bool Rebuilt { get; set; }
        public bool ActionSent { get; set; }
        public bool Advanced { get; set; }
        public bool Completed { get; set; }
        public bool Faulted { get; set; }
        public string Message { get; set; }
    }

    /// <summary>
    /// FASS 2.0 任务状态机：规划 → 滑动窗口下发 → UDP/TCP 状态驱动推进 → 动作闭环。
    /// </summary>
    public sealed class Fass2TaskStateMachine
    {
        private readonly Fass2TaskCallbacks _callbacks;
        private readonly object _syncRoot = new object();
        private TaskCompletionSource<int> _completionSource;

        public Fass2TaskStateMachine(Fass2TaskCallbacks callbacks)
        {
            _callbacks = callbacks ?? throw new ArgumentNullException(nameof(callbacks));
            Context = new Fass2TaskContext();
        }

        public Fass2TaskContext Context { get; }

        public int LockCount { get; set; } = 4;

        public int ResendIntervalMs { get; set; } = 500;

        public int ActionRetryIntervalMs { get; set; } = 1000;

        /// <summary>交管等锁 / 管控等放行上限。0 表示不限制。</summary>
        public int TrafficWaitTimeoutMs { get; set; } = 180_000;

        public bool StartBeforeMove { get; set; } = true;

        public ushort HeadingAngle { get; set; }

        public bool IsIdle
        {
            get
            {
                lock (_syncRoot)
                {
                    return Context.Phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete
                        or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault;
                }
            }
        }

        public bool IsRunning
        {
            get
            {
                lock (_syncRoot)
                {
                    return Context.Phase is Fass2TaskPhase.Planning or Fass2TaskPhase.Dispatching
                        or Fass2TaskPhase.Moving or Fass2TaskPhase.AtStation or Fass2TaskPhase.SegmentDone;
                }
            }
        }

        public event Action<Fass2TaskContext> Completed;
        public event Action<Fass2TaskContext, string> Faulted;

        public void Begin(int startSiteId, int goalSiteId, double defaultSpeed = -1)
        {
            lock (_syncRoot)
            {
                if (!IsIdle)
                {
                    throw new InvalidOperationException($"task state machine busy, phase={Context.Phase}");
                }

                ResetCompletionSource();
                Context.Phase = Fass2TaskPhase.Planning;
                Context.StartSiteId = startSiteId;
                Context.GoalSiteId = goalSiteId;
                Context.CurrentIndex = 0;
                Context.DefaultSpeed = defaultSpeed;
                Context.StartedAt = DateTime.Now;
                Context.FaultReason = null;
                Context.Plan = null;
                Context.FieldsSignature = string.Empty;
                Context.LastDispatchAt = DateTime.MinValue;
                Context.LastActionSentAt = DateTime.MinValue;
                Context.LastActionId = 0;
                Context.WaitingForTraffic = false;
                Context.WaitingForRelease = false;
                Context.ControlReleasedIndex = -1;
                Context.RouteSiteIds = Array.Empty<int>();
                Log($"task begin start={startSiteId}, goal={goalSiteId}, speed={defaultSpeed}");
                Persist();
            }
        }

        public bool TryRestore(Fass2TaskContext saved)
        {
            if (saved == null)
            {
                return false;
            }

            lock (_syncRoot)
            {
                if (IsRunning)
                {
                    return false;
                }

                ResetCompletionSource();
                Context.Phase = saved.Phase;
                Context.StartSiteId = saved.StartSiteId;
                Context.GoalSiteId = saved.GoalSiteId;
                Context.CurrentIndex = saved.CurrentIndex;
                Context.TaskId = saved.TaskId;
                Context.DefaultSpeed = saved.DefaultSpeed;
                Context.FieldsSignature = saved.FieldsSignature ?? string.Empty;
                Context.StartedAt = DateTime.Now;
                Context.LastDispatchAt = DateTime.MinValue;
                Context.LastActionSentAt = DateTime.MinValue;
                Context.Plan = null;
                Context.FaultReason = null;
                Context.RouteSiteIds = saved.RouteSiteIds ?? Array.Empty<int>();
                Context.WaitingForTraffic = false;
                Context.WaitingForRelease = false;
                Context.ControlReleasedIndex = -1;

                if (Context.Phase == Fass2TaskPhase.Planning)
                {
                    Log($"task restored phase={Context.Phase}, goal={Context.GoalSiteId}, index={Context.CurrentIndex}");
                    Persist();
                    return true;
                }

                Context.Phase = Fass2TaskPhase.Planning;
                Log($"task restored and normalized to Planning, goal={Context.GoalSiteId}, index={Context.CurrentIndex}");
                Persist();
                return true;
            }
        }

        public bool TryFindIndexOnRoute(int siteId, out int index)
        {
            lock (_syncRoot)
            {
                return TryFindIndexOnRouteUnlocked(siteId, out index);
            }
        }

        public bool IsAtRouteEnd(int siteId)
        {
            lock (_syncRoot)
            {
                var sites = GetRouteSiteIds();
                return sites.Count > 0 && sites[sites.Count - 1] == siteId;
            }
        }

        /// <summary>
        /// 上报站仍在原任务链路上：对齐 index，从该站重建后方锁窗口，继续同一 goal。
        /// 不取消等待中的任务。
        /// </summary>
        public bool RebaseToSite(int siteId)
        {
            lock (_syncRoot)
            {
                if (!IsRunning)
                {
                    return false;
                }

                if (!TryEnsurePlanUnlocked())
                {
                    return false;
                }

                if (!TryFindIndexOnRouteUnlocked(siteId, out var index))
                {
                    return false;
                }

                Context.CurrentIndex = index;
                RememberRoute(Context.Plan);
                try
                {
                    if (_callbacks.RebaseTraffic != null)
                    {
                        _callbacks.RebaseTraffic(Context.Plan.SiteIds, index);
                    }
                    else
                    {
                        _callbacks.PrepareTraffic?.Invoke(Context.Plan.SiteIds, index);
                    }
                }
                catch (Exception ex)
                {
                    Log($"reconnect rebase traffic wait: {ex.Message}");
                    Context.WaitingForTraffic = true;
                    Context.Phase = Fass2TaskPhase.Moving;
                    Context.LastDispatchAt = DateTime.MinValue;
                    Context.StartedAt = DateTime.Now;
                    Persist();
                    return true;
                }

                Context.WaitingForTraffic = false;
                Context.WaitingForRelease = false;
                Context.Phase = Fass2TaskPhase.Moving;
                Context.LastDispatchAt = DateTime.MinValue;
                Context.StartedAt = DateTime.Now;
                Log($"reconnect rebase site={siteId}, index={index}, goal={Context.GoalSiteId}");
                Persist();
                return true;
            }
        }

        /// <summary>
        /// 不在原链路：保留等待句柄，从新站重开同一/新 goal。
        /// </summary>
        public bool RestartFromSite(int startSiteId, int goalSiteId)
        {
            lock (_syncRoot)
            {
                if (!IsRunning)
                {
                    return false;
                }

                Context.Phase = Fass2TaskPhase.Planning;
                Context.StartSiteId = startSiteId;
                Context.GoalSiteId = goalSiteId;
                Context.CurrentIndex = 0;
                Context.Plan = null;
                Context.RouteSiteIds = Array.Empty<int>();
                Context.TaskId = 0;
                Context.FieldsSignature = string.Empty;
                Context.LastDispatchAt = DateTime.MinValue;
                Context.LastActionSentAt = DateTime.MinValue;
                Context.LastActionId = 0;
                Context.StartedAt = DateTime.Now;
                Context.WaitingForTraffic = false;
                Context.WaitingForRelease = false;
                Context.ControlReleasedIndex = -1;
                Context.FaultReason = null;
                Log($"reconnect restart start={startSiteId}, goal={goalSiteId}");
                Persist();
                return true;
            }
        }

        public bool TryCompleteAtSite(int siteId)
        {
            lock (_syncRoot)
            {
                if (IsIdle)
                {
                    return false;
                }

                Context.Phase = Fass2TaskPhase.Complete;
                TryFinalizeTraffic();
                ClearPersisted();
                Log($"reconnect complete at site={siteId}");
                Completed?.Invoke(Context);
                _completionSource?.TrySetResult(1);
                return true;
            }
        }

        public Fass2TaskTickResult Tick(Fass2StateReport report)
        {
            lock (_syncRoot)
            {
                if (Context.Phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete
                    or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault)
                {
                    return new Fass2TaskTickResult { Phase = Context.Phase };
                }

                var result = new Fass2TaskTickResult { Phase = Context.Phase };

                if (report?.State == 3)
                {
                    return Fault("vehicle emergency stop", result);
                }

                if (report?.State == 4)
                {
                    return Fault("vehicle fault", result);
                }

                switch (Context.Phase)
                {
                    case Fass2TaskPhase.Planning:
                        return PlanAndDispatch(report, result);
                    case Fass2TaskPhase.Dispatching:
                        Context.Phase = Fass2TaskPhase.Moving;
                        result.Phase = Context.Phase;
                        return result;
                    case Fass2TaskPhase.Moving:
                        return HandleMoving(report, result);
                    case Fass2TaskPhase.AtStation:
                        return HandleAtStation(report, result);
                    case Fass2TaskPhase.SegmentDone:
                        return HandleSegmentDone(report, result);
                    default:
                        return result;
                }
            }
        }

        public void Cancel(string reason)
        {
            lock (_syncRoot)
            {
                if (IsIdle)
                {
                    return;
                }

                Context.Phase = Fass2TaskPhase.Cancelled;
                Context.FaultReason = reason;
                Log($"task cancelled: {reason}");
                TryFinalizeTraffic();
                ClearPersisted();
                _completionSource?.TrySetException(new OperationCanceledException(reason));
            }
        }

        public async Task WaitAsync(int timeoutMs, Func<Fass2StateReport> poll, int pollIntervalMs,
            CancellationToken cancellationToken = default)
        {
            if (poll == null)
            {
                throw new ArgumentNullException(nameof(poll));
            }

            TaskCompletionSource<int> waiter;
            lock (_syncRoot)
            {
                waiter = _completionSource ?? ResetCompletionSource();
            }

            var pollTask = Task.Run(async () =>
            {
                while (!cancellationToken.IsCancellationRequested)
                {
                    Fass2StateReport report;
                    lock (_syncRoot)
                    {
                        if (!IsRunning)
                        {
                            return;
                        }
                    }

                    report = poll();
                    Tick(report);

                    lock (_syncRoot)
                    {
                        if (!IsRunning)
                        {
                            return;
                        }
                    }

                    await Task.Delay(Math.Max(20, pollIntervalMs), cancellationToken);
                }
            }, cancellationToken);

            await WaitWithTrafficPauseAsync(waiter, timeoutMs, cancellationToken);
        }

        /// <summary>
        /// UDP 模式由 Hub 回调驱动 Tick，此处只等待完成信号，避免与 OnUdpStateReceived 争用锁导致死锁。
        /// 等锁（WaitingForTraffic）期间不消耗 timeoutMs。
        /// </summary>
        public async Task WaitForCompletionAsync(int timeoutMs, CancellationToken cancellationToken = default)
        {
            TaskCompletionSource<int> waiter;
            string failReason;
            lock (_syncRoot)
            {
                if (Context.Phase == Fass2TaskPhase.Complete)
                {
                    return;
                }

                waiter = _completionSource;
                failReason = Context.FaultReason;
                if (Context.Phase is not Fass2TaskPhase.Cancelled and not Fass2TaskPhase.Fault)
                {
                    waiter = waiter ?? ResetCompletionSource();
                }
            }

            if (waiter == null)
            {
                throw new InvalidOperationException(failReason ?? "task ended without completion source");
            }

            await WaitWithTrafficPauseAsync(waiter, timeoutMs, cancellationToken);
        }

        /// <summary>
        /// 移动超时倒计时；交管等锁或管控等待放行时暂停扣减。
        /// </summary>
        private async Task WaitWithTrafficPauseAsync(TaskCompletionSource<int> waiter, int timeoutMs,
            CancellationToken cancellationToken)
        {
            if (waiter == null)
            {
                throw new ArgumentNullException(nameof(waiter));
            }

            var remainingMs = timeoutMs <= 0 ? -1 : timeoutMs;
            const int sliceMs = 500;
            var lastPauseLog = DateTime.MinValue;
            var pauseElapsedMs = 0;

            while (true)
            {
                cancellationToken.ThrowIfCancellationRequested();

                if (waiter.Task.IsCompleted)
                {
                    await waiter.Task;
                    return;
                }

                var delayMs = remainingMs < 0 ? sliceMs : Math.Min(sliceMs, Math.Max(1, remainingMs));
                var delayTask = Task.Delay(delayMs, cancellationToken);
                var finished = await Task.WhenAny(waiter.Task, delayTask);
                if (finished == waiter.Task)
                {
                    await waiter.Task;
                    return;
                }

                bool waitingPaused;
                string pauseKind;
                Fass2TaskPhase phase;
                int index;
                int goal;
                lock (_syncRoot)
                {
                    phase = Context.Phase;
                    index = Context.CurrentIndex;
                    goal = Context.GoalSiteId;
                    waitingPaused = Context.WaitingForTraffic || Context.WaitingForRelease;
                    pauseKind = Context.WaitingForRelease ? "control wait" : "traffic wait";

                    if (phase is Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault)
                    {
                        break;
                    }
                }

                if (waitingPaused)
                {
                    if (TrafficWaitTimeoutMs > 0)
                    {
                        pauseElapsedMs += delayMs;
                        if (pauseElapsedMs >= TrafficWaitTimeoutMs)
                        {
                            Cancel($"{pauseKind} timeout after {TrafficWaitTimeoutMs}ms");
                            throw new TimeoutException(
                                $"FASS2 {pauseKind} timeout after {TrafficWaitTimeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}");
                        }
                    }

                    if ((DateTime.Now - lastPauseLog).TotalSeconds >= 5)
                    {
                        lastPauseLog = DateTime.Now;
                        Log($"timeout paused ({pauseKind}), remain={(remainingMs < 0 ? "inf" : remainingMs + "ms")}, index={index}, goal={goal}");
                    }

                    continue;
                }

                pauseElapsedMs = 0;

                if (remainingMs < 0)
                {
                    continue;
                }

                remainingMs -= delayMs;
                if (remainingMs <= 0)
                {
                    Cancel("wait timeout");
                    throw new TimeoutException(
                        $"FASS2 task timeout after {timeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}");
                }
            }

            await waiter.Task;
        }

        private Fass2TaskTickResult PlanAndDispatch(Fass2StateReport report, Fass2TaskTickResult result)
        {
            var startSiteId = ResolveCurrentSiteId(report);
            Context.Plan = _callbacks.BuildPlan(startSiteId, Context.GoalSiteId, Context.DefaultSpeed);
            if (Context.Plan == null || Context.Plan.Nodes.Count == 0)
            {
                return Fault($"empty task plan from {startSiteId} to {Context.GoalSiteId}", result);
            }

            if (Context.CurrentIndex >= Context.Plan.Nodes.Count)
            {
                Context.CurrentIndex = Math.Max(0, Context.Plan.Nodes.Count - 1);
            }

            Context.FieldsSignature = Context.Plan.FieldsSignature;
            Context.TaskId = _callbacks.AllocateTaskId();
            Context.ControlReleasedIndex = -1;
            Context.WaitingForRelease = false;
            RememberRoute(Context.Plan);

            try
            {
                _callbacks.PrepareTraffic?.Invoke(Context.Plan.SiteIds, Context.CurrentIndex);
            }
            catch (Exception ex)
            {
                Context.Phase = Fass2TaskPhase.Moving;
                Context.WaitingForTraffic = true;
                result.Phase = Context.Phase;
                result.Message = $"traffic prepare wait: {ex.Message}";
                Log(result.Message);
                Persist();
                return result;
            }

            if (StartBeforeMove && Context.CurrentIndex == 0)
            {
                _callbacks.SendControl(Fass2Protocol.CmdStart, 0);
            }

            if (!TryDispatchCurrentWindow(out var waitReason))
            {
                result.Message = waitReason;
                Context.Phase = Fass2TaskPhase.Moving;
                result.Phase = Context.Phase;
                Persist();
                return result;
            }

            Context.Phase = Fass2TaskPhase.Moving;
            result.Phase = Context.Phase;
            result.Dispatched = true;
            result.Message = $"planned nodes={Context.Plan.Nodes.Count}, task={Context.TaskId}";
            Log(result.Message);
            Persist();
            return result;
        }

        private Fass2TaskTickResult HandleMoving(Fass2StateReport report, Fass2TaskTickResult result)
        {
            if (TryRebuildForFieldsChange(report, result))
            {
                return result;
            }

            if (ShouldResend())
            {
                if (TryDispatchCurrentWindow(out var waitReason))
                {
                    result.Dispatched = true;
                }
                else if (string.IsNullOrEmpty(result.Message))
                {
                    result.Message = waitReason;
                }
            }

            // 滑动窗口一次下发多站时，过站点可能被车体直接飞过；上报已到更后站则追赶 Leave + 推进 index。
            TryCatchUpPassedSites(report, result);
            ReleaseLocksBehindVehicle(report);

            var expected = GetExpectedNode();
            if (!Fass2ActionResolver.IsAtNode(report, expected.Node))
            {
                result.Message = $"moving node={report.Node.Node}, expect={expected.Node}";
                return result;
            }

            if (Fass2ActionResolver.IsVehicleMoving(report.State))
            {
                // 仍报到本站时不要 Leave/推进：模拟器过站常保持 node=当前站并增加 dist。
                // 真正飞过到更后站时由 TryCatchUpPassedSites 推进并放锁。
                result.Message = Fass2ActionResolver.RequiresActionWait(expected)
                    ? $"at node={expected.Node}, still running"
                    : $"pass node={expected.Node}, still running (wait leave)";
                return result;
            }

            Context.Phase = Fass2TaskPhase.AtStation;
            result.Phase = Context.Phase;
            result.Message = $"arrived site index={Context.CurrentIndex}, node={expected.Node}";
            Log(result.Message);
            Persist();
            return result;
        }

        private Fass2TaskTickResult HandleAtStation(Fass2StateReport report, Fass2TaskTickResult result)
        {
            if (TryRebuildForFieldsChange(report, result))
            {
                return result;
            }

            if (ShouldResend())
            {
                if (TryDispatchCurrentWindow(out var waitReason))
                {
                    result.Dispatched = true;
                }
                else if (string.IsNullOrEmpty(result.Message))
                {
                    result.Message = waitReason;
                }
            }

            if (TryCatchUpPassedSites(report, result))
            {
                Context.WaitingForRelease = false;
                ReleaseLocksBehindVehicle(report);
                Context.Phase = Fass2TaskPhase.Moving;
                result.Phase = Context.Phase;
                result.Message = $"catch-up from AtStation to index={Context.CurrentIndex}, node={report.Node.Node}";
                return result;
            }

            ReleaseLocksBehindVehicle(report);

            var expected = GetExpectedNode();
            if (!Fass2ActionResolver.IsAtNode(report, expected.Node))
            {
                Context.WaitingForRelease = false;
                Context.Phase = Fass2TaskPhase.Moving;
                result.Phase = Context.Phase;
                result.Message = $"left station node={report.Node.Node}, expect={expected.Node}";
                return result;
            }

            if (TryHandleControlStop(report, expected, result))
            {
                return result;
            }

            Context.WaitingForRelease = false;

            if (!Fass2ActionResolver.IsStationActionComplete(expected, report.Node, report.State))
            {
                TrySendActionPatch(expected, report, result);
                return result;
            }

            Context.Phase = Fass2TaskPhase.SegmentDone;
            result.Phase = Context.Phase;
            result.Message = $"station done index={Context.CurrentIndex}, node={expected.Node}";
            Log(result.Message);
            Persist();
            return result;
        }

        private bool TryHandleControlStop(Fass2StateReport report, Fass2NodeMessage expected, Fass2TaskTickResult result)
        {
            var unreleased = Fass2ControlAreaGate.IsControlStop(expected.StartStop)
                             && Context.ControlReleasedIndex != Context.CurrentIndex;
            var releasedHere = Context.ControlReleasedIndex == Context.CurrentIndex;
            if (!unreleased && !releasedHere)
            {
                return false;
            }

            Context.WaitingForRelease = true;
            var isLast = Context.Plan != null && Context.CurrentIndex >= Context.Plan.Nodes.Count - 1;

            if (Fass2ActionResolver.IsVehicleMoving(report.State))
            {
                Context.Phase = Fass2TaskPhase.Moving;
                result.Phase = Context.Phase;
                result.Message = $"control node={expected.Node}, vehicle running";
                return true;
            }

            if (unreleased)
            {
                if (report.Node.StartStop != Fass2TaskBuilder.StartStopControlStop)
                {
                    TrySendActionPatch(expected, report, result);
                    result.Message = $"control stop pending StartStop, node={expected.Node}";
                    return true;
                }

                var siteId = ResolveControlSiteId();
                var check = _callbacks.CheckControlRelease?.Invoke(siteId)
                            ?? new Fass2ControlReleaseCheck
                            {
                                CanRelease = true,
                                ReleaseStartStop = Fass2TaskBuilder.StartStopControlStart,
                                Reason = "no occupancy callback"
                            };

                if (!check.CanRelease)
                {
                    result.Message =
                        $"control wait site={siteId}, area={check.AreaId}, others={check.OthersInArea}/{check.Capacity}, {check.Reason}";
                    return true;
                }

                var releaseSs = check.ReleaseStartStop == 0
                    ? Fass2TaskBuilder.StartStopControlStart
                    : check.ReleaseStartStop;
                expected.StartStop = releaseSs;
                Context.ControlReleasedIndex = Context.CurrentIndex;
                SendControlReleasePatch(expected, releaseSs, result);
                if (TryDispatchCurrentWindow(out var waitReason))
                {
                    result.Dispatched = true;
                }
                else if (string.IsNullOrEmpty(result.Message))
                {
                    result.Message = waitReason;
                }

                result.Message =
                    $"control released site={siteId}, 12->{releaseSs}, area={check.AreaId}, others={check.OthersInArea}/{check.Capacity}";
                Log(result.Message);
                Persist();
                return true;
            }

            if (report.Node.StartStop != expected.StartStop)
            {
                TrySendActionPatch(expected, report, result);
                result.Message = $"control release pending StartStop={expected.StartStop}, node={expected.Node}";
                return true;
            }

            if (isLast)
            {
                Context.WaitingForRelease = false;
                Context.Phase = Fass2TaskPhase.SegmentDone;
                result.Phase = Context.Phase;
                result.Message = $"control last-station done index={Context.CurrentIndex}, node={expected.Node}";
                Log(result.Message);
                Persist();
                return true;
            }

            result.Message = $"control released waiting leave, node={expected.Node}";
            return true;
        }

        private void TrySendActionPatch(Fass2NodeMessage expected, Fass2StateReport report, Fass2TaskTickResult result)
        {
            if (!ShouldSendActionPatch())
            {
                return;
            }

            var patch = Fass2ActionResolver.BuildActionPatch(expected, report?.Node);
            if (patch == null)
            {
                return;
            }

            Context.LastActionId = _callbacks.AllocateActionId();
            Context.LastActionSentAt = DateTime.Now;
            _callbacks.SendAction(patch, Context.LastActionId);
            result.ActionSent = true;
            result.Message =
                $"action pending [{Fass2ActionResolver.DescribePending(expected, report?.Node)}], sent 0xA1={Context.LastActionId}";
            Log(result.Message);
        }

        private void SendControlReleasePatch(Fass2NodeMessage expected, byte releaseStartStop, Fass2TaskTickResult result)
        {
            var patch = new Fass2NodeMessage
            {
                Node = expected.Node,
                StartStop = releaseStartStop
            };
            Context.LastActionId = _callbacks.AllocateActionId();
            Context.LastActionSentAt = DateTime.Now;
            _callbacks.SendAction(patch, Context.LastActionId);
            result.ActionSent = true;
        }

        private int ResolveControlSiteId()
        {
            if (Context.Plan?.SiteIds != null &&
                Context.CurrentIndex >= 0 &&
                Context.CurrentIndex < Context.Plan.SiteIds.Count)
            {
                return Context.Plan.SiteIds[Context.CurrentIndex];
            }

            return Context.StartSiteId;
        }

        private Fass2TaskTickResult HandleSegmentDone(Fass2StateReport report, Fass2TaskTickResult result)
        {
            var finishedIndex = Context.CurrentIndex;
            Context.CurrentIndex++;
            result.Advanced = true;
            Context.WaitingForRelease = false;

            if (Context.Plan == null || Context.CurrentIndex >= Context.Plan.Nodes.Count)
            {
                if (Context.Plan?.SiteIds != null && finishedIndex < Context.Plan.SiteIds.Count)
                {
                    _callbacks.FinalizeTraffic?.Invoke(Context.Plan.SiteIds[finishedIndex]);
                }

                Context.Phase = Fass2TaskPhase.Complete;
                result.Phase = Context.Phase;
                result.Completed = true;
                result.Message = $"task complete goal={Context.GoalSiteId}";
                Log(result.Message);
                ClearPersisted();
                Completed?.Invoke(Context);
                _completionSource?.TrySetResult(1);
                return result;
            }

            if (Context.Plan.SiteIds != null && finishedIndex < Context.Plan.SiteIds.Count)
            {
                _callbacks.LeaveTraffic?.Invoke(Context.Plan.SiteIds[finishedIndex]);
            }

            ReleaseLocksBehindVehicle(report);

            Context.Phase = Fass2TaskPhase.Dispatching;
            if (!TryDispatchCurrentWindow(out var waitReason))
            {
                Context.Phase = Fass2TaskPhase.Moving;
                result.Phase = Context.Phase;
                result.Message = waitReason;
                Persist();
                return result;
            }

            Context.Phase = Fass2TaskPhase.Moving;
            result.Phase = Context.Phase;
            result.Dispatched = true;
            result.Message = $"segment advanced index={Context.CurrentIndex}";
            Log(result.Message);
            Persist();
            return result;
        }

        private bool TryRebuildForFieldsChange(Fass2StateReport report, Fass2TaskTickResult result)
        {
            if (Context.Plan?.SiteIds == null || Context.Plan.SiteIds.Count == 0)
            {
                return false;
            }

            var newSignature = Fass2SiteFieldReader.BuildFieldsSignature(
                Context.Plan.SiteIds, 0, SimpleLib.GetSite);
            if (string.Equals(newSignature, Context.FieldsSignature, StringComparison.Ordinal))
            {
                return false;
            }

            // 优先用车辆实际报到站重建，避免逻辑 index 超前时 Reset 到他车正占用的前方站。
            var currentSiteId = ResolveRebuildStartSiteId(report);
            Log($"fields changed at index={Context.CurrentIndex}, rebuild from site={currentSiteId}");
            Context.Plan = _callbacks.BuildPlan(currentSiteId, Context.GoalSiteId, Context.DefaultSpeed);
            if (Context.Plan == null || Context.Plan.Nodes.Count == 0)
            {
                Fault($"rebuild failed from {currentSiteId} to {Context.GoalSiteId}", result);
                return true;
            }

            Context.CurrentIndex = 0;
            Context.FieldsSignature = Context.Plan.FieldsSignature;
            Context.TaskId = _callbacks.AllocateTaskId();
            Context.ControlReleasedIndex = -1;
            Context.WaitingForRelease = false;
            RememberRoute(Context.Plan);
            try
            {
                _callbacks.PrepareTraffic?.Invoke(Context.Plan.SiteIds, Context.CurrentIndex);
            }
            catch (Exception ex)
            {
                // 起点被他车占用等：等待，不要把整单打成 Fault
                Context.Phase = Fass2TaskPhase.Moving;
                result.Phase = Context.Phase;
                result.Rebuilt = true;
                result.Message = $"traffic rebuild prepare wait: {ex.Message}";
                Log(result.Message);
                Persist();
                return true;
            }

            if (!TryDispatchCurrentWindow(out var waitReason))
            {
                Context.Phase = Fass2TaskPhase.Moving;
                result.Phase = Context.Phase;
                result.Rebuilt = true;
                result.Message = waitReason;
                Persist();
                return true;
            }

            Context.Phase = Fass2TaskPhase.Moving;
            result.Phase = Context.Phase;
            result.Rebuilt = true;
            result.Dispatched = true;
            result.Message = $"rebuilt nodes={Context.Plan.Nodes.Count}, task={Context.TaskId}";
            Persist();
            return true;
        }

        private int ResolveRebuildStartSiteId(Fass2StateReport report)
        {
            if (report?.Node != null && _callbacks.ResolveSite != null)
            {
                var physical = _callbacks.ResolveSite(report.Node.Node);
                if (physical != null && physical.id > 0)
                {
                    return physical.id;
                }
            }

            if (Context.Plan?.SiteIds != null &&
                Context.CurrentIndex >= 0 &&
                Context.CurrentIndex < Context.Plan.SiteIds.Count)
            {
                return Context.Plan.SiteIds[Context.CurrentIndex];
            }

            return Context.StartSiteId;
        }

        private bool TryDispatchCurrentWindow(out string waitReason)
        {
            waitReason = null;
            var window = BuildDispatchWindow(out var lockedCount, out var wantWindow);
            if (window.Length == 0)
            {
                waitReason = "dispatch window is empty";
                Context.WaitingForTraffic = true;
                return false;
            }

            if (_callbacks.EnsureTrafficWindow != null && lockedCount < wantWindow && lockedCount <= 1)
            {
                waitReason = $"traffic wait, locked={lockedCount}/{wantWindow}, index={Context.CurrentIndex}";
                Context.WaitingForTraffic = true;
                return false;
            }

            _callbacks.SendNodes(window, Context.TaskId);
            Context.LastDispatchAt = DateTime.Now;
            Context.WaitingForTraffic = false;
            Log($"dispatch window count={window.Length}, locked={lockedCount}/{wantWindow}, fromIndex={Context.CurrentIndex}, task={Context.TaskId}");
            return true;
        }

        private Fass2NodeMessage[] BuildDispatchWindow(out int lockedCount, out int wantWindow)
        {
            var nodes = Context.Plan.Nodes;
            var start = Context.CurrentIndex;
            var remaining = nodes.Count - start;
            wantWindow = Math.Max(1, Math.Min(LockCount, remaining));
            wantWindow = Math.Min(wantWindow, 10);
            wantWindow = Fass2ControlAreaGate.LimitWindowCount(
                nodes, start, wantWindow, Context.ControlReleasedIndex);

            lockedCount = wantWindow;
            if (_callbacks.EnsureTrafficWindow != null && Context.Plan?.SiteIds != null)
            {
                lockedCount = _callbacks.EnsureTrafficWindow(Context.Plan.SiteIds, start, wantWindow);
            }

            lockedCount = Fass2ControlAreaGate.LimitWindowCount(
                nodes, start, lockedCount, Context.ControlReleasedIndex);
            lockedCount = Math.Max(0, Math.Min(lockedCount, wantWindow));
            if (lockedCount <= 0)
            {
                return Array.Empty<Fass2NodeMessage>();
            }

            var window = new Fass2NodeMessage[lockedCount];
            for (var i = 0; i < lockedCount; i++)
            {
                window[i] = nodes[start + i];
            }

            return window;
        }

        private Fass2NodeMessage GetExpectedNode()
        {
            return Context.Plan.Nodes[Context.CurrentIndex];
        }

        /// <summary>
        /// 当上报节点已是计划中更后方站点时，释放中间站交管锁并推进 CurrentIndex。
        /// </summary>
        private bool TryCatchUpPassedSites(Fass2StateReport report, Fass2TaskTickResult result)
        {
            if (Context.Plan?.Nodes == null || report?.Node == null)
            {
                return false;
            }

            var reportIndex = FindPlanIndexByNode(report.Node.Node, Context.CurrentIndex);
            if (reportIndex <= Context.CurrentIndex)
            {
                return false;
            }

            var catchTo = reportIndex;
            for (var i = Context.CurrentIndex; i < reportIndex; i++)
            {
                var node = Context.Plan.Nodes[i];
                var unreleasedControl = Fass2ControlAreaGate.IsControlStop(node.StartStop) &&
                                        Context.ControlReleasedIndex != i;
                var actionStation = Fass2ActionResolver.RequiresActionWait(node) &&
                                    node.StartStop != Fass2TaskBuilder.StartStopControlStart;
                if (unreleasedControl || actionStation)
                {
                    catchTo = i;
                    break;
                }
            }

            if (catchTo <= Context.CurrentIndex)
            {
                return false;
            }

            while (Context.CurrentIndex < catchTo)
            {
                var finishedIndex = Context.CurrentIndex;
                if (Context.Plan.SiteIds != null && finishedIndex < Context.Plan.SiteIds.Count)
                {
                    _callbacks.LeaveTraffic?.Invoke(Context.Plan.SiteIds[finishedIndex]);
                }

                Context.CurrentIndex++;
                result.Advanced = true;
                Log(
                    $"catch-up passed index={finishedIndex} -> {Context.CurrentIndex}, reportNode={report.Node.Node}");
            }

            Persist();
            return true;
        }

        /// <summary>
        /// 以车辆实际报到站为准释放身后锁；逻辑 index 若超前于物理站，不得放掉车辆仍在的站。
        /// </summary>
        private void ReleaseLocksBehindVehicle(Fass2StateReport report)
        {
            if (Context.Plan?.SiteIds == null || Context.Plan.SiteIds.Count == 0)
            {
                return;
            }

            var keepIndex = Context.CurrentIndex;
            if (report?.Node != null)
            {
                var reportIndex = FindPlanIndexByNode(report.Node.Node, 0);
                if (reportIndex >= 0)
                {
                    // 取更靠后的“尚未离开”位置，避免 CurrentIndex 超前时把物理站 Leave 掉
                    keepIndex = Math.Min(keepIndex, reportIndex);
                }
            }

            _callbacks.ReleaseTrafficBehind?.Invoke(Context.Plan.SiteIds, keepIndex);
        }

        private int FindPlanIndexByNode(ushort node, int fromIndex)
        {
            if (Context.Plan?.Nodes == null)
            {
                return -1;
            }

            fromIndex = Math.Max(0, fromIndex);
            for (var i = fromIndex; i < Context.Plan.Nodes.Count; i++)
            {
                if (Context.Plan.Nodes[i].Node == node)
                {
                    return i;
                }
            }

            return -1;
        }

        private int ResolveCurrentSiteId(Fass2StateReport report)
        {
            if (Context.CurrentIndex > 0 && Context.Plan?.SiteIds != null &&
                Context.CurrentIndex < Context.Plan.SiteIds.Count)
            {
                return Context.Plan.SiteIds[Context.CurrentIndex];
            }

            if (report?.Node != null && report.Node.Node != 0)
            {
                var site = _callbacks.ResolveSite?.Invoke(report.Node.Node);
                if (site != null)
                {
                    return site.id;
                }
            }

            return Context.StartSiteId;
        }

        private bool ShouldResend()
        {
            return Context.LastDispatchAt == DateTime.MinValue ||
                   (DateTime.Now - Context.LastDispatchAt).TotalMilliseconds >= ResendIntervalMs;
        }

        private bool ShouldSendActionPatch()
        {
            return Context.LastActionSentAt == DateTime.MinValue ||
                   (DateTime.Now - Context.LastActionSentAt).TotalMilliseconds >= ActionRetryIntervalMs;
        }

        private Fass2TaskTickResult Fault(string reason, Fass2TaskTickResult result)
        {
            Context.Phase = Fass2TaskPhase.Fault;
            Context.FaultReason = reason;
            result.Phase = Context.Phase;
            result.Faulted = true;
            result.Message = reason;
            Log($"task fault: {reason}");
            TryFinalizeTraffic();
            Persist();
            Faulted?.Invoke(Context, reason);
            _completionSource?.TrySetException(new InvalidOperationException(reason));
            return result;
        }

        private void TryFinalizeTraffic()
        {
            if (Context.Plan?.SiteIds == null || Context.Plan.SiteIds.Count == 0)
            {
                return;
            }

            var index = Math.Max(0, Math.Min(Context.CurrentIndex, Context.Plan.SiteIds.Count - 1));
            _callbacks.FinalizeTraffic?.Invoke(Context.Plan.SiteIds[index]);
        }

        private void RememberRoute(Fass2TaskPlan plan)
        {
            if (plan?.SiteIds == null || plan.SiteIds.Count == 0)
            {
                Context.RouteSiteIds = Array.Empty<int>();
                return;
            }

            var copy = new int[plan.SiteIds.Count];
            for (var i = 0; i < plan.SiteIds.Count; i++)
            {
                copy[i] = plan.SiteIds[i];
            }

            Context.RouteSiteIds = copy;
        }

        private IReadOnlyList<int> GetRouteSiteIds()
        {
            if (Context.Plan?.SiteIds != null && Context.Plan.SiteIds.Count > 0)
            {
                return Context.Plan.SiteIds;
            }

            return Context.RouteSiteIds ?? Array.Empty<int>();
        }

        private bool TryFindIndexOnRouteUnlocked(int siteId, out int index)
        {
            index = -1;
            var sites = GetRouteSiteIds();
            if (sites == null || sites.Count == 0 || siteId <= 0)
            {
                return false;
            }

            var from = Math.Max(0, Math.Min(Context.CurrentIndex, sites.Count - 1));
            for (var i = from; i < sites.Count; i++)
            {
                if (sites[i] == siteId)
                {
                    index = i;
                    return true;
                }
            }

            for (var i = from - 1; i >= 0; i--)
            {
                if (sites[i] == siteId)
                {
                    index = i;
                    return true;
                }
            }

            return false;
        }

        private bool TryEnsurePlanUnlocked()
        {
            if (Context.Plan?.Nodes != null && Context.Plan.Nodes.Count > 0)
            {
                RememberRoute(Context.Plan);
                return true;
            }

            if (_callbacks.BuildPlan == null || Context.GoalSiteId <= 0)
            {
                return false;
            }

            var startId = Context.StartSiteId > 0 ? Context.StartSiteId : Context.GoalSiteId;
            try
            {
                Context.Plan = _callbacks.BuildPlan(startId, Context.GoalSiteId, Context.DefaultSpeed);
            }
            catch (Exception ex)
            {
                Log($"reconnect rebuild plan failed: {ex.Message}");
                return false;
            }

            if (Context.Plan?.Nodes == null || Context.Plan.Nodes.Count == 0)
            {
                return false;
            }

            RememberRoute(Context.Plan);
            return true;
        }

        private void Persist()
        {
            _callbacks.Persist?.Invoke(Context);
        }

        private void ClearPersisted()
        {
            _callbacks.ClearPersisted?.Invoke();
        }

        private void Log(string message)
        {
            _callbacks.Log?.Invoke(message);
        }

        private TaskCompletionSource<int> ResetCompletionSource()
        {
            _completionSource = new TaskCompletionSource<int>(TaskCreationOptions.RunContinuationsAsynchronously);
            return _completionSource;
        }
    }
}