using LessokajiWeaverUtilities.Utilities;
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.BasicProps;
using SimpleCore.Library;
using SimpleCore.PropType;
using SimpleCore.Traffic;
using SimpleLite.Props;
using SimpleLite.RCS;
using SimpleLite.RCS.CarTypes;
using SimpleLite.UI;
using StandardScene.Magnetic.Protocol;
using StandardScene.Magnetic.Tasking;
using StandardScene;
using System;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.IO;
using System.Linq;
using System.Net.Sockets;
using System.Threading;
using System.Threading.Tasks;

namespace StandardScene.CarTypes
{
    /// <summary>
    /// 基于 FASS 2.0 通用控制接口实现的磁导航小车类型。
    /// 支持 PLC/TCP（主动查询）与 PCB/UDP（被动收状态 + 0x10 应答）两种通讯模式。
    /// 路径段通过 0xB1 下发，业务逻辑对齐 <c>FASS.Scheduler</c> Fairyland 调度实现。
    /// </summary>
    [TemplateTrackCoderSettings(
        priority = 1,
        templateString = "agv.Mag2Go(${src.id},${dst.id},${track.Speed});",
        blockVerb = "true",
        trackFields = typeof(BasicTrackFields))]
    [CarType(Name = "Mag2Car")]
    [I18N.DocumentTranslation(Name = "Mag2Car", locale = "en")]
    [EnvelopConfig(lengthX = 1200, lengthY = 800, centerX = 0, centerY = 0)]
    public class Mag2Car : GhostCar, IFass2UdpCar, IFass2LoopCar
    {
        private readonly object _syncRoot = new object();
        private TcpClient _persistentClient;
        private DateTime _lastPollTime = DateTime.MinValue;
        private DateTime _lastUdpReportTime = DateTime.MinValue;
        private bool _udpRegistered;
        private ushort _registeredVehicleCode = ushort.MaxValue;
        private long _nextTaskId = 1;
        private long _nextActionId = 1;
        private Fass2TaskStateMachine _taskStateMachine;
        private Fass2TrafficLocker _trafficLocker;
        private bool _taskRestoreAttempted;
        private bool _wasOffline;
        private int _reconnectWaitSiteId = -1;
        private int _lastOffPathHandledSiteId = -1;
        private ushort _lastMappedNode;
        private ushort _lastMappedDistance;

        [FieldMember] public Fass2CommMode CommMode = Fass2CommMode.Udp;
        [FieldMember] public int Port = 5000;
        [FieldMember] public int ListenPort = 20103;
        [FieldMember] public ushort VehicleCode = 0;
        [FieldMember] public int ConnectTimeoutMs = 500;
        [FieldMember] public int SendTimeoutMs = 500;
        [FieldMember] public int ReceiveTimeoutMs = 500;
        [FieldMember] public int PollIntervalMs = 1000;
        [FieldMember] public int UdpOfflineTimeoutMs = 15000;
        [FieldMember] public int MoveTimeoutSeconds = 120;
        [FieldMember] public int LockCount = 4;
        [FieldMember] public bool EnableTrafficControl = true;
        [FieldMember] public bool VerifyTrajectoryFieldsOnStation = false;
        [FieldMember] public int TaskResendIntervalMs = 500;
        [FieldMember] public int ActionRetryIntervalMs = 1000;
        [FieldMember] public bool EnableTaskStateMachine = true;
        [FieldMember] public bool EnableLoopTaskDrive = true;
        [FieldMember] public bool StartBeforeMove = true;
        [FieldMember] public bool UseTagValueAsNode = false;
        [FieldMember] public bool UsePersistentConnection = true;
        [FieldMember] public bool EnableDetailLog = true;
        [FieldMember] public bool LogRawFrame = true;
        [FieldMember] public bool EnableFileLog = true;
        [FieldMember] public string LogDirectory = "logs";
        [FieldMember] public float CarLength = 1200;
        [FieldMember] public float CarWidth = 800;

        public Fass2StateReport LastReport { get; private set; } = new Fass2StateReport();

        public bool IsTaskIdle => _taskStateMachine?.IsIdle ?? true;

        public Fass2TaskPhase TaskPhase => _taskStateMachine?.Context.Phase ?? Fass2TaskPhase.Idle;

        public bool HasGoalSite => TryGetGoalSiteId(out _);

        public int? GoalSiteId => TryGetGoalSiteId(out var goalSiteId) ? goalSiteId : null;

        public event Action<int> TaskCompleted;

        string IFass2UdpCar.RemoteAddress => address;

        int IFass2UdpCar.RemotePort => Port;

        ushort IFass2UdpCar.VehicleCode => VehicleCode;

        void IFass2UdpCar.OnUdpStateReceived(Fass2StateReport report)
        {
            lock (_syncRoot)
            {
                _lastUdpReportTime = DateTime.Now;
                ApplyReport(report);
                TryRecoverAfterReconnectIfNeeded(report);
                SyncUdpOnlineTags(true);
            }

            TickTaskStateMachine(report);
        }

        public new static async Task<Mag2Car> Create()
        {
            var car = new Mag2Car
            {
                lstatus = "连接中",
                address = "127.0.0.1",
                name = "Mag2Car",
                haveCoordination = true,
                speed = 1
            };
            car.EnsureTaskStateMachine();
            Mag2CarFileLogger.Configure(car.LogDirectory, car.EnableFileLog);
            return car;
        }

        protected override void draw(Graphics eGraphics)
        {
            var halfLength = CarLength / 2;
            var halfWidth = CarWidth / 2;
            eGraphics.FillRectangle(Brushes.DimGray, -halfLength, -halfWidth, CarLength, CarWidth);
            eGraphics.DrawRectangle(Pens.White, -halfLength, -halfWidth, CarLength, CarWidth);

            using (var orientPen = new Pen(Color.Cyan, 3))
            {
                orientPen.CustomEndCap = new AdjustableArrowCap(6, 6, true);
                orientPen.StartCap = LineCap.RoundAnchor;
                eGraphics.DrawLine(orientPen, 0, 0, halfLength, 0);
            }
        }

        public override string SetDisplayInfo()
        {
            try
            {
                var report = LastReport;
                var state = Fass2Protocol.StateText(report.State);
                var alarm = report.Alarm == 0 ? "" : $"|alarm:0x{report.Alarm:X}";
                return $"{name}({id})\n{state}|site:{siteID}|soc:{report.BatteryCharge}|node:{report.Node.Node}{alarm}";
            }
            catch
            {
                return $"{name}({id})\n状态未知";
            }
        }

        public override void keepAlive()
        {
            if ((DateTime.Now - _lastPollTime).TotalMilliseconds < PollIntervalMs)
            {
                return;
            }

            _lastPollTime = DateTime.Now;

            if (CommMode == Fass2CommMode.Udp)
            {
                try
                {
                    EnsureUdpSession();
                    TryRestoreTaskFromTags();
                    if (_lastUdpReportTime != DateTime.MinValue &&
                        (DateTime.Now - _lastUdpReportTime).TotalMilliseconds <= UdpOfflineTimeoutMs)
                    {
                        haveCoordination = true;
                        SyncUdpOnlineTags(true);
                        TryLogLoopAssignmentToFile();
                        var report = LastReport;
                        DetailLog($"keepAlive udp ok, node={report.Node.Node}, state={Fass2Protocol.StateText(report.State)}, soc={report.BatteryCharge}, taskPhase={TaskPhase}");
                    }
                    else
                    {
                        lstatus = "离线";
                        _wasOffline = true;
                        SyncUdpOnlineTags(false);
                        DetailLog($"keepAlive udp waiting, listen={ListenPort}, remote={address}:{Port}");
                    }
                }
                catch (Exception ex)
                {
                    lstatus = "离线";
                    _wasOffline = true;
                    SyncUdpOnlineTags(false);
                    DetailLog($"keepAlive udp failed: {ex.Message}");
                }

                return;
            }

            try
            {
                DetailLog($"keepAlive poll begin, endpoint={address}:{Port}");
                var report = RefreshState();
                haveCoordination = true;
                DetailLog($"keepAlive ok, node={report.Node.Node}, state={Fass2Protocol.StateText(report.State)}, soc={report.BatteryCharge}");
            }
            catch (Exception ex)
            {
                lstatus = IsBenignSocketFault(ex) ? lstatus : "离线";
                if (!IsBenignSocketFault(ex))
                {
                    _wasOffline = true;
                    DetailLog($"keepAlive failed: {ex.Message}");
                }
            }
        }

        private bool _running;
        private int _lastSyncedTrafficSiteId = -1;
        private int _lastFileLoggedGoalSite = -1;

        public override async Task actualSendScript(string script)
        {
            if (_running)
            {
                throw new InvalidOperationException($"Mag2Car {id} already running script");
            }

            if (ShouldDriveLoopGoalAsTrackedTask(script))
            {
                _running = true;
                try
                {
                    DetailLog($"loop goal drive intercept, goal={GoalSiteId}, scriptLen={(script ?? string.Empty).Length}");
                    await ExecuteGoalTaskAsync();
                    DetailLog("loop goal drive completed");
                }
                catch (Exception ex)
                {
                    DetailLog($"loop goal drive failed: {ExceptionFormatter.FormatEx(ex)}");
                    RecoverAfterGoalDriveFailure();
                    throw;
                }
                finally
                {
                    _running = false;
                }

                return;
            }

            _running = true;
            try
            {
                DetailLog($"script begin, length={(script ?? string.Empty).Length}");
                var agv = new Mag2CarInterface(id);
                var tcs = new TaskCompletionSource<int>();
                new Thread(() =>
                {
                    try
                    {
                        SelfEvaluating(agv, script);
                        tcs.SetResult(1);
                    }
                    catch (Exception ex)
                    {
                        tcs.SetException(ex);
                    }
                })
                { Name = $"eva_{name}({id}):{status.programs.now.name}" }.Start();

                await tcs.Task;
                await agv.WaitAsync();
                DetailLog("script completed");
            }
            catch (Exception ex)
            {
                DetailLog($"script failed: {ExceptionFormatter.FormatEx(ex)}");
                throw;
            }
            finally
            {
                _running = false;
            }
        }

        [MethodMember(Name = "查询状态", Description = "按FASS 2.0协议查询AGV状态")]
        public void QueryState()
        {
            RefreshState();
        }

        [MethodMember(Name = "启动", Description = "发送启动指令(0x01)")]
        public void StartMag2Car()
        {
            SendControl(Fass2Protocol.CmdStart, 0);
        }

        [MethodMember(Name = "停止", Description = "发送停止指令(0x02)")]
        public void StopMag2Car()
        {
            SendControl(Fass2Protocol.CmdStop, 0);
        }

        [MethodMember(Name = "急停", Description = "发送急停指令(0x03)")]
        public void EmergencyStopMag2Car()
        {
            SendControl(Fass2Protocol.CmdEmergencyStop, 0);
        }

        [MethodMember(Name = "进入小车远程", Description = "进入小车远程桌面")]
        [I18N.DocumentTranslation(Name = "Open remote desktop", Description = "Open the car's remote desktop", locale = "en")]
        public void Mstsc()
        {
            Process.Start(new ProcessStartInfo
            {
                FileName = "mstsc",
                Arguments = $"/v:{address}",
                UseShellExecute = false,
                CreateNoWindow = true
            });
        }

        public void EmergencyStop(string reason)
        {
            Diagnosis.Post($"car{name}:Mag2Car EmergencyStop {reason}");
            SendControl(Fass2Protocol.CmdEmergencyStop, 0);
        }

        public void EmergencyRelease()
        {
            Diagnosis.Post($"car{name}:Mag2Car EmergencyRelease");
            SendControl(Fass2Protocol.CmdStart, 0);
        }

        [MethodMember(Name = "重置UDP监听", Description = "重新注册 UDP 会话（PCB 模式）")]
        public void ResetUdpSession()
        {
            lock (_syncRoot)
            {
                if (_udpRegistered)
                {
                    Fass2UdpHub.Unregister(_registeredVehicleCode, this);
                    _udpRegistered = false;
                    _registeredVehicleCode = ushort.MaxValue;
                }

                _lastUdpReportTime = DateTime.MinValue;
            }

            EnsureUdpSession();
            DetailLog($"UDP session reset, listen={ListenPort}, remote={address}:{Port}");
        }

        [MethodMember(Name = "重置TCP连接", Description = "关闭当前长连接，下次命令自动重连（PLC 模式）")]
        public void ResetTcpConnection()
        {
            lock (_syncRoot)
            {
                ClosePersistentConnection("manual reset");
            }
        }

        [MethodMember(Name = "重置环线联调", Description = "清空 Loop/FASS2 标签、交管锁和脚本错误，恢复可再分配")]
        public void ResetLoopDriveState()
        {
            lock (_syncRoot)
            {
                _running = false;
                _lastSyncedTrafficSiteId = -1;
                _taskRestoreAttempted = false;
                _wasOffline = false;
                _reconnectWaitSiteId = -1;
                _lastOffPathHandledSiteId = -1;

                if (_taskStateMachine != null && !_taskStateMachine.IsIdle)
                {
                    _taskStateMachine.Cancel("manual loop reset");
                }

                ClearTaskPersistence();
                ClearLoopDriveTags();
                _lastFileLoggedGoalSite = -1;

                var site = ResolveSite(LastReport.Node.Node);
                if (site == null && GetLastSite() > 0)
                {
                    site = SimpleLib.GetSite(GetLastSite());
                }

                if (site == null && siteID > 0)
                {
                    site = SimpleLib.GetSite(siteID);
                }

                if (site != null)
                {
                    TrafficReset(site, true, strict: false);
                    siteID = site.id;
                    x = site.x;
                    y = site.y;
                    _lastSyncedTrafficSiteId = site.id;
                    haveCoordination = true;
                }

                status.programs.task = Task.CompletedTask;
                SyncUdpOnlineTags(IsUdpSessionOnline());
                DetailLog(
                    $"loop drive state reset, site={(site?.id.ToString() ?? "-")}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]");
            }
        }

        private void ClearLoopDriveTags()
        {
            if (tags == null)
            {
                return;
            }

            Commons.DeleteTag(tags, "goalSite");
            Commons.DeleteTag(tags, "occupied");
            Commons.DeleteTag(tags, "loopAssigned");
            Commons.DeleteTag(tags, "dest");
            Commons.DeleteTag(tags, "deliver");
            Commons.DeleteTag(tags, "changePriority");
            Commons.DeleteTag(tags, "priority");
            Commons.DeleteTag(tags, "currentWorkStep");
            Fass2TaskPersistence.Clear(tags);
        }

        /// <summary>
        /// 任务失败/超时后清 pending、复位本站交管锁，去掉 occupied，保留 goalSite 供环线重派。
        /// </summary>
        private void RecoverAfterGoalDriveFailure()
        {
            try
            {
                if (_taskStateMachine != null && !_taskStateMachine.IsIdle)
                {
                    _taskStateMachine.Cancel("recover after failure");
                }
            }
            catch
            {
                // ignore
            }

            try
            {
                ClearTaskPersistence();
                Commons.DeleteTag(tags, "occupied");

                var site = ResolveSite(LastReport.Node.Node);
                if (site == null && siteID > 0)
                {
                    site = SimpleLib.GetSite(siteID);
                }

                if (site == null && GetLastSite() > 0)
                {
                    site = SimpleLib.GetSite(GetLastSite());
                }

                if (site != null)
                {
                    TrafficReset(site, true, strict: false);
                    siteID = site.id;
                    _lastSyncedTrafficSiteId = site.id;
                    haveCoordination = true;
                }
                else
                {
                    status.pendingLocks = Array.Empty<int>();
                    status.seqScope = Array.Empty<int>();
                    status.seqPtr = 0;
                }

                SyncUdpOnlineTags(IsUdpSessionOnline());
                DetailLog(
                    $"goal drive recover done, keepGoal={GoalSiteId}, site={(site?.id.ToString() ?? "-")}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]");
            }
            catch (Exception ex)
            {
                DetailLog($"goal drive recover failed: {ex.Message}");
            }
        }

        private bool IsUdpSessionOnline()
        {
            return CommMode == Fass2CommMode.Udp &&
                   _lastUdpReportTime != DateTime.MinValue &&
                   (DateTime.Now - _lastUdpReportTime).TotalMilliseconds <= UdpOfflineTimeoutMs;
        }

        public Fass2StateReport RefreshState()
        {
            if (CommMode == Fass2CommMode.Udp)
            {
                EnsureUdpSession();
                if (_lastUdpReportTime == DateTime.MinValue ||
                    (DateTime.Now - _lastUdpReportTime).TotalMilliseconds > UdpOfflineTimeoutMs)
                {
                    throw new TimeoutException(
                        $"Mag2Car {name}({id}) UDP state timeout ({UdpOfflineTimeoutMs}ms), listen={ListenPort}");
                }

                return LastReport;
            }

            var request = Fass2Protocol.BuildControl(Fass2Protocol.CmdQuery, VehicleCode, 0);
            var response = SendRequest(request, Fass2Protocol.StateFrameLength, "Query(0x00)");
            var report = Fass2Protocol.ParseState(response);
            ApplyReport(report);
            TryRecoverAfterReconnectIfNeeded(report);
            return report;
        }

        public void SendControl(byte command, ushort param)
        {
            var request = Fass2Protocol.BuildControl(command, VehicleCode, param);
            SendRequest(request, Fass2Protocol.StateFrameLength, CommandName(command));
        }

        public void SendPathNodes(Fass2NodeMessage[] nodes)
        {
            var taskId = (ulong)Interlocked.Increment(ref _nextTaskId);
            SendPathNodes(nodes, taskId);
        }

        public void SendPathNodes(Fass2NodeMessage[] nodes, ulong taskId)
        {
            var request = Fass2Protocol.BuildNodes(VehicleCode, taskId, nodes);
            DetailLog($"send nodes 0xB1, task={taskId}, count={nodes.Length}");
            SendRequest(request, Fass2Protocol.StateFrameLength, "Nodes(0xB1)");
        }

        public Fass2TaskPlan BuildTaskPlan(int startSiteId, int goalSiteId, double defaultSpeed = -1)
        {
            return Fass2TaskBuilder.BuildPath(startSiteId, goalSiteId, CreateTaskBuildOptions(defaultSpeed), ResolveNodeId, this);
        }

        public void SendTaskPlan(Fass2TaskPlan plan)
        {
            if (plan == null || plan.Nodes == null || plan.Nodes.Count == 0)
            {
                throw new ArgumentException("Fass2TaskPlan requires at least one node");
            }

            var taskId = (ulong)Interlocked.Increment(ref _nextTaskId);
            DetailLog(
                $"send task plan 0xB1, task={taskId}, sites={plan.SiteIds.Count}, nodes={plan.Nodes.Count}, batches={plan.Batches.Count}, sig={plan.FieldsSignature}");

            foreach (var batch in plan.Batches)
            {
                SendPathNodes(batch, taskId);
            }
        }

        public void BeginTrackedTask(int startSiteId, int goalSiteId, double defaultSpeed = -1)
        {
            EnsureTaskStateMachine();
            _taskStateMachine.LockCount = LockCount;
            _taskStateMachine.ResendIntervalMs = TaskResendIntervalMs;
            _taskStateMachine.ActionRetryIntervalMs = ActionRetryIntervalMs;
            _taskStateMachine.StartBeforeMove = StartBeforeMove;
            Fass2ActionResolver.VerifyTrajectoryFields = VerifyTrajectoryFieldsOnStation;
            _taskStateMachine.Begin(startSiteId, goalSiteId, defaultSpeed);

            var tick = _taskStateMachine.Tick(LastReport);
            DetailLog($"tracked task bootstrap tick: {tick.Message}");
        }

        public async Task WaitTrackedTaskAsync(int timeoutMs)
        {
            EnsureTaskStateMachine();
            var waitMs = timeoutMs <= 0 ? MoveTimeoutSeconds * 1000 : timeoutMs;
            if (CommMode == Fass2CommMode.Udp)
            {
                await _taskStateMachine.WaitForCompletionAsync(waitMs);
                return;
            }

            await _taskStateMachine.WaitAsync(
                waitMs,
                RefreshState,
                Math.Max(50, Math.Min(PollIntervalMs, 500)));
        }

        async Task IFass2LoopCar.ExecuteGoalTaskAsync(int? goalSiteId, double defaultSpeed,
            CancellationToken cancellationToken)
        {
            await ExecuteGoalTaskAsync(goalSiteId, defaultSpeed, cancellationToken);
        }

        public async Task ExecuteGoalTaskAsync(int? goalSiteId = null, double defaultSpeed = -1,
            CancellationToken cancellationToken = default)
        {
            if (!EnableTaskStateMachine)
            {
                throw new InvalidOperationException("ExecuteGoalTaskAsync requires EnableTaskStateMachine=true");
            }

            var goalId = goalSiteId ?? GoalSiteId;
            if (goalId == null || goalId.Value <= 0)
            {
                throw new InvalidOperationException("Mag2Car goalSite is not set");
            }

            var startId = ResolvePhysicalSiteId();
            if (startId <= 0)
            {
                throw new InvalidOperationException($"Mag2Car {name}({id}) cannot resolve start site for loop task");
            }

            var lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1;
            if (lockedSite > 0 && lockedSite != startId)
            {
                DetailLog($"ExecuteGoalTaskAsync normalize start, holding={lockedSite}, reportStart={startId}");
            }

            DetailLog($"ExecuteGoalTaskAsync start={startId}, goal={goalId.Value}, speed={defaultSpeed}, traffic={EnableTrafficControl}");

            BeginTrackedTask(startId, goalId.Value, defaultSpeed);
            await WaitTrackedTaskAsync(MoveTimeoutSeconds * 1000);
            TaskCompleted?.Invoke(goalId.Value);
        }

        /// <summary>
        /// 环线联调以 UDP 上报节点映射的站点为准，避免 FindRoute/forecast 污染 holdingLocks 后 GetLastSite 变成目标站。
        /// </summary>
        private int ResolvePhysicalSiteId()
        {
            if (LastReport.Node.Node != 0)
            {
                var fromReport = ResolveSite(LastReport.Node.Node);
                if (fromReport != null)
                {
                    return fromReport.id;
                }
            }

            if (GetLastSite() > 0)
            {
                return GetLastSite();
            }

            return siteID > 0 ? siteID : -1;
        }

        private bool ShouldDriveLoopGoalAsTrackedTask(string script)
        {
            if (!EnableLoopTaskDrive || !EnableTaskStateMachine || !HasGoalSite)
            {
                return false;
            }

            return !string.IsNullOrEmpty(script) &&
                   script.Contains("Mag2Go", StringComparison.Ordinal);
        }

        private bool TryGetGoalSiteId(out int goalSiteId)
        {
            goalSiteId = 0;
            if (tags == null || !tags.TryGetValue("goalSite", out var goalText) ||
                !int.TryParse(goalText, out goalSiteId))
            {
                return false;
            }

            return goalSiteId > 0;
        }

        private void EnsureTaskStateMachine()
        {
            if (_taskStateMachine != null)
            {
                return;
            }

            _trafficLocker = new Fass2TrafficLocker(this, message => DetailLog(message));

            _taskStateMachine = new Fass2TaskStateMachine(new Fass2TaskCallbacks
            {
                BuildPlan = BuildTaskPlan,
                SendNodes = (nodes, taskId) => SendPathNodes(nodes, taskId),
                SendAction = SendAction,
                SendControl = SendControl,
                ResolveNodeId = ResolveNodeId,
                ResolveSite = ResolveSite,
                AllocateTaskId = () => (ulong)Interlocked.Increment(ref _nextTaskId),
                AllocateActionId = () => (ulong)Interlocked.Increment(ref _nextActionId),
                Persist = PersistTaskContext,
                ClearPersisted = ClearTaskPersistence,
                Log = message => DetailLog($"task-sm {message}"),
                PrepareTraffic = (siteIds, fromIndex) =>
                {
                    if (!EnableTrafficControl)
                    {
                        return;
                    }

                    _trafficLocker.PrepareSequence(siteIds, fromIndex);
                },
                EnsureTrafficWindow = (siteIds, fromIndex, wantWindow) =>
                {
                    if (!EnableTrafficControl)
                    {
                        return wantWindow;
                    }

                    return _trafficLocker.EnsureWindowLocked(siteIds, fromIndex, wantWindow);
                },
                LeaveTraffic = siteId =>
                {
                    if (!EnableTrafficControl)
                    {
                        return;
                    }

                    _trafficLocker.LeavePassed(siteId);
                },
                ReleaseTrafficBehind = (siteIds, keepFromIndex) =>
                {
                    if (!EnableTrafficControl)
                    {
                        return;
                    }

                    _trafficLocker.ReleaseLocksBehind(siteIds, keepFromIndex);
                },
                RebaseTraffic = (siteIds, fromIndex) =>
                {
                    if (!EnableTrafficControl)
                    {
                        return;
                    }

                    _trafficLocker.RebaseFrom(siteIds, fromIndex, LockCount);
                },
                FinalizeTraffic = siteId =>
                {
                    if (!EnableTrafficControl)
                    {
                        return;
                    }

                    _trafficLocker.FinalizeAtSite(siteId);
                },
                CheckControlRelease = siteId =>
                {
                    try
                    {
                        return Fass2ControlAreaOccupancy.Check(this, siteId);
                    }
                    catch (Exception ex)
                    {
                        DetailLog($"control occupancy failed: site={siteId}, {ex.Message}");
                        return new Fass2ControlReleaseCheck
                        {
                            CanRelease = false,
                            Reason = ex.Message
                        };
                    }
                }
            });
        }

        private void TickTaskStateMachine(Fass2StateReport report)
        {
            if (!EnableTaskStateMachine || _taskStateMachine == null || !_taskStateMachine.IsRunning)
            {
                return;
            }

            var tick = _taskStateMachine.Tick(report);
            if (!string.IsNullOrEmpty(tick.Message) &&
                (tick.Dispatched || tick.Rebuilt || tick.ActionSent || tick.Advanced || tick.Completed || tick.Faulted))
            {
                DetailLog($"task-sm tick phase={tick.Phase}, {tick.Message}");
            }

            UpdateTaskStatusEnums();
        }

        private void TryRestoreTaskFromTags()
        {
            if (!EnableTaskStateMachine || _taskRestoreAttempted || tags == null)
            {
                return;
            }

            _taskRestoreAttempted = true;
            if (!Fass2TaskPersistence.TryLoad(tags, out var saved))
            {
                return;
            }

            EnsureTaskStateMachine();
            if (_taskStateMachine.TryRestore(saved))
            {
                DetailLog(
                    $"task restored from tags, phase={saved.Phase}, goal={saved.GoalSiteId}, index={saved.CurrentIndex}, task={saved.TaskId}");
                UpdateTaskStatusEnums();
            }
        }

        private void TryRecoverAfterReconnectIfNeeded(Fass2StateReport report)
        {
            var site = report?.Node != null ? ResolveSite(report.Node.Node) : null;
            if (site == null)
            {
                if (_wasOffline || _reconnectWaitSiteId > 0)
                {
                    DetailLog($"reconnect skip, unmapped node={report?.Node.Node}");
                }

                return;
            }

            EnsureTaskStateMachine();
            var running = _taskStateMachine != null && _taskStateMachine.IsRunning;
            var onRoute = running && _taskStateMachine.TryFindIndexOnRoute(site.id, out _);
            var offlineRecover = _wasOffline || _reconnectWaitSiteId > 0;
            var offPathRecover = running && !onRoute && _lastOffPathHandledSiteId != site.id;

            if (!offlineRecover && !offPathRecover)
            {
                return;
            }

            if (_reconnectWaitSiteId > 0 && _reconnectWaitSiteId != site.id)
            {
                _reconnectWaitSiteId = -1;
            }

            _wasOffline = false;

            var holding = status.holdingLocks == null || status.holdingLocks.Length == 0
                ? string.Empty
                : string.Join(",", status.holdingLocks);
            var routeText = FormatRouteSites();
            DetailLog(
                $"reconnect site={site.id}, onRoute={onRoute}, offPath={offPathRecover}, taskPhase={TaskPhase}, holding=[{holding}], goal={GoalSiteId?.ToString() ?? "-"}, route=[{routeText}]");

            if (!running)
            {
                _reconnectWaitSiteId = -1;
                DetailLog("reconnect idle, keep SyncTrafficFromReport");
                return;
            }

            if (!TryClaimReportedSite(site))
            {
                _reconnectWaitSiteId = site.id;
                DetailLog($"reconnect wait, site={site.id} held by another car");
                return;
            }

            _reconnectWaitSiteId = -1;

            try
            {
                if (onRoute)
                {
                    if (_taskStateMachine.IsAtRouteEnd(site.id))
                    {
                        TrafficResetToReportedSite(site);
                        _taskStateMachine.TryCompleteAtSite(site.id);
                        DetailLog($"reconnect rebase complete at end site={site.id}");
                        return;
                    }

                    if (_taskStateMachine.RebaseToSite(site.id))
                    {
                        _lastSyncedTrafficSiteId = site.id;
                        DetailLog($"reconnect rebase site={site.id}");
                    }

                    return;
                }

                _lastOffPathHandledSiteId = site.id;
                RecoverOffOriginalRoute(site);
            }
            catch (Exception ex)
            {
                DetailLog($"reconnect recover failed: {ex.Message}");
            }
        }

        /// <summary>
        /// 上报站不在当前任务 Plan/RouteSiteIds 上：按 tasklist 归属链路重下，不再用原 goal 续跑。
        /// </summary>
        private void RecoverOffOriginalRoute(Site site)
        {
            var originalGoal = _taskStateMachine.Context.GoalSiteId > 0
                ? _taskStateMachine.Context.GoalSiteId
                : GoalSiteId ?? 0;

            var matches = Mag2LoopMission.FindChainsContaining(site.id);
            if (originalGoal > 0 && matches.Count > 0)
            {
                matches = matches.Where(m => m != null && m.TargetSiteId != originalGoal).ToList();
            }

            if (Fass2ReconnectChainSelector.TrySelect(matches, preferGoalSiteId: null, out var chain, out var reason)
                && chain != null)
            {
                DetailLog($"reconnect off-path chain={chain}, reason={reason}, dropGoal={originalGoal}");
                if (chain.IsAtEndPoint || chain.TargetSiteId == site.id)
                {
                    HandOffToAutoLoop(site);
                    _taskStateMachine.TryCompleteAtSite(site.id);
                    DetailLog($"reconnect chain end site={site.id}, goal={chain.TargetSiteId}");
                    return;
                }

                SetGoalSiteTag(chain.TargetSiteId);
                if (_taskStateMachine.RestartFromSite(site.id, chain.TargetSiteId))
                {
                    TrafficResetToReportedSite(site);
                    _lastSyncedTrafficSiteId = site.id;
                    DetailLog($"reconnect restart start={site.id}, goal={chain.TargetSiteId}");
                    return;
                }
            }

            DetailLog(
                $"reconnect off-path unmatched site={site.id}, dropGoal={originalGoal}, matches={matches.Count}, {reason ?? "no chain"}");

            if (!EnableLoopTaskDrive && originalGoal > 0 && originalGoal != site.id)
            {
                try
                {
                    var probe = BuildTaskPlan(site.id, originalGoal);
                    if (probe?.SiteIds != null && probe.SiteIds.Count > 0)
                    {
                        SetGoalSiteTag(originalGoal);
                        if (_taskStateMachine.RestartFromSite(site.id, originalGoal))
                        {
                            TrafficResetToReportedSite(site);
                            _lastSyncedTrafficSiteId = site.id;
                            DetailLog($"reconnect restart keepGoal (non-loop) start={site.id}, goal={originalGoal}");
                            return;
                        }
                    }
                }
                catch (Exception ex)
                {
                    DetailLog($"reconnect keepGoal plan failed: {ex.Message}");
                }
            }

            DetailLog($"reconnect unmatched site={site.id}, handoff AutoLoop");
            HandOffToAutoLoop(site);
            if (!_taskStateMachine.IsIdle)
            {
                _taskStateMachine.Cancel("reconnect unmatched site");
            }
        }

        private string FormatRouteSites()
        {
            var sites = _taskStateMachine?.Context?.Plan?.SiteIds;
            if (sites == null || sites.Count == 0)
            {
                sites = _taskStateMachine?.Context?.RouteSiteIds;
            }

            if (sites == null || sites.Count == 0)
            {
                return string.Empty;
            }

            return string.Join(",", sites);
        }

        private bool TryClaimReportedSite(Site site)
        {
            if (site == null)
            {
                return false;
            }

            var holder = FindOtherCarHoldingSite(site.id);
            if (holder == null)
            {
                return true;
            }

            if (TryReleaseStaleHolder(holder, site.id))
            {
                DetailLog($"reconnect released stale holder car={holder.id}, site={site.id}");
                return true;
            }

            return false;
        }

        private void TrafficResetToReportedSite(Site site)
        {
            if (site == null)
            {
                return;
            }

            try
            {
                TrafficReset(site, true, strict: false);
                siteID = site.id;
                _lastSyncedTrafficSiteId = site.id;
                haveCoordination = true;
            }
            catch (Exception ex)
            {
                DetailLog($"reconnect TrafficReset failed: site={site.id}, {ex.Message}");
            }
        }

        private void HandOffToAutoLoop(Site site)
        {
            Commons.DeleteTag(tags, "occupied");
            Commons.DeleteTag(tags, "goalSite");
            Commons.DeleteTag(tags, "loopAssigned");
            ClearTaskPersistence();
            TrafficResetToReportedSite(site);
        }

        private void SetGoalSiteTag(int goalSiteId)
        {
            if (tags == null || goalSiteId <= 0)
            {
                return;
            }

            Commons.AddOrUpdateTag(tags, "goalSite", goalSiteId.ToString());
        }

        private void PersistTaskContext(Fass2TaskContext context)
        {
            if (tags == null || context == null)
            {
                return;
            }

            Fass2TaskPersistence.Save(tags, context);
            UpdateTaskStatusEnums();
        }

        private void ClearTaskPersistence()
        {
            if (tags == null)
            {
                return;
            }

            Fass2TaskPersistence.Clear(tags);
            UpdateTaskStatusEnums();
        }

        private void UpdateTaskStatusEnums()
        {
            if (_taskStateMachine == null)
            {
                status.enums["TaskPhase"] = Fass2TaskPhase.Idle.ToString();
                return;
            }

            var context = _taskStateMachine.Context;
            status.enums["TaskPhase"] = context.Phase.ToString();
            status.enums["TaskGoal"] = context.GoalSiteId.ToString();
            status.enums["TaskIndex"] = context.CurrentIndex.ToString();
            status.enums["TaskFieldsSig"] = context.FieldsSignature ?? string.Empty;
            if (!string.IsNullOrEmpty(context.FaultReason))
            {
                status.enums["TaskFault"] = context.FaultReason;
            }
            else
            {
                status.enums.Remove("TaskFault");
            }
        }

        private Fass2TaskBuildOptions CreateTaskBuildOptions(double defaultSpeed = -1)
        {
            return new Fass2TaskBuildOptions
            {
                UseTagValueAsNode = UseTagValueAsNode,
                DefaultSpeed = defaultSpeed > 0 ? defaultSpeed : 12,
                CarSpeed = speed
            };
        }

        public void SendAction(Fass2NodeMessage node, ulong actionId = 0)
        {
            var request = Fass2Protocol.BuildAction(VehicleCode, actionId, node);
            DetailLog($"send action 0xA1, actionId={actionId}, node={node.Node}");
            SendRequest(request, Fass2Protocol.ActionFrameLength, "Action(0xA1)");
        }

        private byte[] SendRequest(byte[] request, int responseLength, string commandName)
        {
            DetailLog($"TX {commandName}, len={request.Length}, mode={CommMode}");
            if (LogRawFrame)
            {
                DetailLog($"TX raw {Fass2Protocol.ToHex(request)}");
            }

            if (CommMode == Fass2CommMode.Udp)
            {
                EnsureUdpSession();
                Fass2UdpHub.SendToCar(this, request);

                DetailLog($"UDP TX {commandName} done (async, no sync RX)");
                return Array.Empty<byte>();
            }

            byte[] response;
            lock (_syncRoot)
            {
                response = SendAndReceive(request, responseLength);
            }

            DetailLog($"RX {commandName}, bytes={response.Length}");
            if (LogRawFrame)
            {
                DetailLog($"RX raw {Fass2Protocol.ToHex(response)}");
            }

            if (response.Length >= Fass2Protocol.StateFrameLength)
            {
                ApplyReport(Fass2Protocol.ParseState(response));
            }

            return response;
        }

        private void EnsureUdpSession()
        {
            if (string.IsNullOrWhiteSpace(address))
            {
                throw new InvalidOperationException("Mag2Car address is empty for UDP mode");
            }

            if (!_udpRegistered || _registeredVehicleCode != VehicleCode)
            {
                if (_udpRegistered && _registeredVehicleCode != VehicleCode)
                {
                    Fass2UdpHub.Unregister(_registeredVehicleCode, this);
                    _udpRegistered = false;
                }

                Fass2UdpHub.Register(this);
                Fass2UdpHub.EnsureStarted(ListenPort);
                _udpRegistered = true;
                _registeredVehicleCode = VehicleCode;
                DetailLog($"UDP registered, VehicleCode={VehicleCode}, listen={ListenPort}, remote={address}:{Port}");
            }
        }

        private byte[] SendAndReceive(byte[] request, int responseLength)
        {
            return UsePersistentConnection
                ? SendAndReceivePersistent(request, responseLength)
                : SendAndReceiveShort(request, responseLength);
        }

        private byte[] SendAndReceivePersistent(byte[] request, int responseLength)
        {
            try
            {
                EnsurePersistentConnection();
                var stream = _persistentClient.GetStream();
                stream.Write(request, 0, request.Length);
                stream.Flush();
                return ReadExact(stream, responseLength);
            }
            catch (Exception ex)
            {
                ClosePersistentConnection($"persistent send/read failed: {ex.Message}");
                throw;
            }
        }

        private void EnsurePersistentConnection()
        {
            if (IsPersistentConnected())
            {
                return;
            }

            ClosePersistentConnection("prepare reconnect");
            DetailLog($"persistent connect begin {address}:{Port}, timeout={ConnectTimeoutMs}ms");
            var client = new TcpClient();
            try
            {
                ConnectWithTimeout(client);
                client.SendTimeout = SendTimeoutMs;
                client.ReceiveTimeout = ReceiveTimeoutMs;
                _persistentClient = client;
                DetailLog($"persistent connect ok {address}:{Port}");
            }
            catch
            {
                try { client.Close(); } catch { }
                _persistentClient = null;
                throw;
            }
        }

        private bool IsPersistentConnected()
        {
            try
            {
                return _persistentClient != null && _persistentClient.Connected;
            }
            catch
            {
                return false;
            }
        }

        private void ClosePersistentConnection(string reason)
        {
            var client = _persistentClient;
            if (client == null)
            {
                return;
            }

            _persistentClient = null;
            DetailLog($"persistent connection closed, reason={reason}");
            try
            {
                if (client.Connected)
                {
                    client.Client?.Shutdown(SocketShutdown.Both);
                }
            }
            catch (Exception ex) when (IsBenignSocketFault(ex)) { }
            catch { }

            try { client.Close(); }
            catch (Exception ex) when (IsBenignSocketFault(ex)) { }
            catch { }
        }

        private byte[] SendAndReceiveShort(byte[] request, int responseLength)
        {
            using (var client = new TcpClient())
            {
                DetailLog($"short connect begin {address}:{Port}, timeout={ConnectTimeoutMs}ms");
                ConnectWithTimeout(client);
                DetailLog($"short connect ok {address}:{Port}");

                client.SendTimeout = SendTimeoutMs;
                client.ReceiveTimeout = ReceiveTimeoutMs;

                using (var stream = client.GetStream())
                {
                    stream.Write(request, 0, request.Length);
                    stream.Flush();
                    return ReadExact(stream, responseLength);
                }
            }
        }

        private void ConnectWithTimeout(TcpClient client)
        {
            if (string.IsNullOrWhiteSpace(address))
            {
                throw new InvalidOperationException("Mag2Car address is empty");
            }

            using var cts = new CancellationTokenSource(ConnectTimeoutMs);
            try
            {
                client.ConnectAsync(address, Port, cts.Token).GetAwaiter().GetResult();
            }
            catch (OperationCanceledException) when (cts.IsCancellationRequested)
            {
                AbortClientQuietly(client);
                throw new TimeoutException($"connect {address}:{Port} timeout ({ConnectTimeoutMs}ms)");
            }
            catch (Exception ex) when (IsBenignSocketFault(ex) && cts.IsCancellationRequested)
            {
                AbortClientQuietly(client);
                throw new TimeoutException($"connect {address}:{Port} timeout ({ConnectTimeoutMs}ms)", ex);
            }
        }

        private static void AbortClientQuietly(TcpClient client)
        {
            if (client == null) return;
            try { client.Client?.Close(); } catch (Exception ex) when (IsBenignSocketFault(ex)) { } catch { }
            try { client.Close(); } catch (Exception ex) when (IsBenignSocketFault(ex)) { } catch { }
        }

        private static bool IsBenignSocketFault(Exception ex)
        {
            for (var current = ex; current != null; current = current.InnerException)
            {
                if (current is ObjectDisposedException) return true;
                if (current is SocketException socketEx)
                {
                    switch (socketEx.SocketErrorCode)
                    {
                        case SocketError.OperationAborted:
                        case SocketError.Interrupted:
                        case SocketError.ConnectionAborted:
                        case SocketError.ConnectionReset:
                        case SocketError.Shutdown:
                            return true;
                    }
                }
                if (current is IOException ioEx)
                {
                    var message = ioEx.Message;
                    if (!string.IsNullOrEmpty(message) &&
                        (message.Contains("已中止 I/O", StringComparison.Ordinal) ||
                         message.Contains("I/O operation", StringComparison.OrdinalIgnoreCase) ||
                         message.Contains("operation was aborted", StringComparison.OrdinalIgnoreCase)))
                    {
                        return true;
                    }
                }
            }
            return false;
        }

        private static byte[] ReadExact(NetworkStream stream, int length)
        {
            var buffer = new byte[length];
            var offset = 0;
            try
            {
                while (offset < length)
                {
                    var read = stream.Read(buffer, offset, length - offset);
                    if (read == 0)
                    {
                        throw new IOException("remote closed before full FASS2 response");
                    }
                    offset += read;
                }
                return buffer;
            }
            catch (Exception ex) when (IsBenignSocketFault(ex))
            {
                throw new IOException("remote closed before full FASS2 response", ex);
            }
        }

        /// <summary>
        /// 按 FASS2 调度器 <c>GetSendNodes</c> 规则构建 src→dst 路径段（含中间站与站点/边 fields）。
        /// </summary>
        internal Fass2NodeMessage[] BuildMoveNodes(int srcId, int dstId, double trackSpeed = -1)
        {
            return Fass2TaskBuilder.BuildSegment(srcId, dstId, CreateTaskBuildOptions(trackSpeed), ResolveNodeId, this);
        }

        private void WaitUntilArrived(ushort targetNode, int timeoutMs)
        {
            var deadline = DateTime.Now.AddMilliseconds(timeoutMs <= 0 ? MoveTimeoutSeconds * 1000 : timeoutMs);
            while (DateTime.Now < deadline)
            {
                var report = RefreshState();
                if (report.Node.Node == targetNode && report.State != 1)
                {
                    DetailLog($"arrived node={targetNode}, state={Fass2Protocol.StateText(report.State)}");
                    return;
                }

                Thread.Sleep(Math.Max(50, Math.Min(PollIntervalMs, 500)));
            }

            throw new TimeoutException(
                $"Mag2Car {name}({id}) move timeout, target node={targetNode}, current={LastReport.Node.Node}, state={Fass2Protocol.StateText(LastReport.State)}");
        }

        private void ApplyReport(Fass2StateReport report)
        {
            LastReport = report;
            lstatus = report.Alarm != 0 ? "报警" : report.State switch
            {
                0 => "未准备",
                4 => "故障",
                3 => "急停",
                _ => "上线"
            };
            th = report.HeadingAngle;

            status.enums["Protocol"] = CommMode == Fass2CommMode.Udp ? "FASS2.0/UDP" : "FASS2.0/TCP";
            status.enums["CurrentNode"] = report.Node.Node.ToString();
            status.enums["Alarm"] = $"0x{report.Alarm:X}";
            status.enums["State"] = Fass2Protocol.StateText(report.State);
            status.enums["Soc"] = report.BatteryCharge.ToString();
            status.enums["BatteryHealth"] = report.BatteryHealth.ToString();
            status.enums["ElectricCurrent"] = report.BatteryCurrent.ToString();
            status.enums["Voltage"] = report.BatteryVoltage.ToString();
            status.enums["HeadingAngle"] = report.HeadingAngle.ToString();
            status.enums["Task"] = report.Task.ToString();
            status.enums["StartStop"] = report.Node.StartStop.ToString();
            status.enums["Lift"] = report.Node.Lift.ToString();
            status.enums["Roll"] = report.Node.Roll.ToString();
            UpdateTaskStatusEnums();

            var site = ResolveSite(report.Node.Node);
            if (site == null)
            {
                haveCoordination = false;
                DetailLog($"report applied without mapped site, node={report.Node.Node}, state={Fass2Protocol.StateText(report.State)}");
                return;
            }

            haveCoordination = true;
            ApplyMapCoordinates(site, report);
            SyncTrafficFromReport(site, report);
            DetailLog(
                $"report applied, node={report.Node.Node}, site={siteID}, x={x:0.###}, y={y:0.###}, dist={report.Node.Distance}, state={Fass2Protocol.StateText(report.State)}, soc={report.BatteryCharge}, task={report.Task}");
        }

        /// <summary>
        /// 路段内按 Distance 在起止站点间插值，避免地图只在 node 跳变时才移动。
        /// <para>
        /// 协议行驶中 Node 一直是出发站、Distance 递增；到站才改 Node。
        /// 若中途 0xB1 重下/task 重建把 Distance 清 0，旧逻辑会把车坐标打回出发站中心 → 看起来“闪回上一站”。
        /// </para>
        /// </summary>
        private void ApplyMapCoordinates(Site site, Fass2StateReport report)
        {
            siteID = site.id;

            var node = report.Node.Node;
            var dist = report.Node.Distance;
            var moving = Fass2ActionResolver.IsVehicleMoving(report.State);

            // 同站行驶中 Distance 被清零：保留当前插值坐标，不要拽回站心。
            if (dist <= 0 && moving &&
                _lastMappedNode == node && _lastMappedDistance > 100)
            {
                DetailLog(
                    $"map coord keep (dist cleared while moving), node={node}, lastDist={_lastMappedDistance}, site={site.id}");
                _lastMappedDistance = 0;
                return;
            }

            if (dist <= 0 ||
                !TryResolveMotionSegment(node, out var fromSite, out var toSite, out var segmentLenMm))
            {
                x = site.x;
                y = site.y;
                _lastMappedNode = node;
                _lastMappedDistance = dist;
                return;
            }

            var progress = Math.Min(1.0, dist / Math.Max(1.0, segmentLenMm));
            var dx = toSite.x - fromSite.x;
            var dy = toSite.y - fromSite.y;
            var mapLen = Math.Sqrt(dx * dx + dy * dy);

            if (mapLen >= 1.0)
            {
                x = (float)(fromSite.x + dx * progress);
                y = (float)(fromSite.y + dy * progress);
                _lastMappedNode = node;
                _lastMappedDistance = dist;
                return;
            }

            var track = FindTrackBetween(fromSite.id, toSite.id);
            if (track != null)
            {
                var trackDx = toSite.x - fromSite.x;
                var trackDy = toSite.y - fromSite.y;
                if (Math.Abs(trackDx) + Math.Abs(trackDy) >= 1.0)
                {
                    x = (float)(fromSite.x + trackDx * progress);
                    y = (float)(fromSite.y + trackDy * progress);
                    _lastMappedNode = node;
                    _lastMappedDistance = dist;
                    return;
                }
            }

            // 地图起止站点坐标重合时，用协议距离沿 X 轴做最小可视化偏移。
            x = (float)(fromSite.x + dist);
            y = fromSite.y;
            _lastMappedNode = node;
            _lastMappedDistance = dist;
        }

        private bool TryResolveMotionSegment(ushort currentNode, out Site fromSite, out Site toSite, out double segmentLenMm)
        {
            fromSite = null;
            toSite = null;
            segmentLenMm = 0;

            fromSite = ResolveSite(currentNode);
            if (fromSite == null)
            {
                return false;
            }

            var plan = _taskStateMachine?.Context?.Plan;
            if (plan?.Nodes == null || plan.Nodes.Count < 2)
            {
                return false;
            }

            for (var i = 0; i < plan.Nodes.Count - 1; i++)
            {
                if (plan.Nodes[i].Node != currentNode)
                {
                    continue;
                }

                var targetNode = plan.Nodes[i + 1].Node;
                toSite = ResolveSite(targetNode);
                if (toSite == null)
                {
                    return false;
                }

                // Distance 挂在出发站节点上（边长），不是下一站。
                segmentLenMm = plan.Nodes[i].Distance;
                if (segmentLenMm <= 0)
                {
                    var dx = toSite.x - fromSite.x;
                    var dy = toSite.y - fromSite.y;
                    segmentLenMm = Math.Max(1.0, Math.Sqrt(dx * dx + dy * dy));
                }

                return true;
            }

            return false;
        }

        private static Track FindTrackBetween(int fromSiteId, int toSiteId)
        {
            foreach (var track in SimpleLib.GetAllTracks())
            {
                if (track.direction == 0)
                {
                    if ((track.siteA == fromSiteId && track.siteB == toSiteId) ||
                        (track.siteB == fromSiteId && track.siteA == toSiteId))
                    {
                        return track;
                    }

                    continue;
                }

                if (track.direction == 1 && track.siteA == fromSiteId && track.siteB == toSiteId)
                {
                    return track;
                }

                if (track.direction == 2 && track.siteB == fromSiteId && track.siteA == toSiteId)
                {
                    return track;
                }
            }

            return null;
        }

        private void SyncUdpOnlineTags(bool isOnline)
        {
            if (CommMode != Fass2CommMode.Udp)
            {
                return;
            }

            if (isOnline)
            {
                Commons.AddOrUpdateTag(tags, "Online", "true");
                Commons.DeleteTag(tags, "agvOffline");
                return;
            }

            Commons.DeleteTag(tags, "Online");
        }

        private void SyncTrafficFromReport(Site site, Fass2StateReport report)
        {
            if (site == null)
            {
                return;
            }

            var taskRunning = _running || (_taskStateMachine?.IsRunning ?? false);
            if (taskRunning)
            {
                var lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1;
                if (lockedSite == site.id)
                {
                    _lastSyncedTrafficSiteId = site.id;
                }
                else
                {
                    DetailLog(
                        $"traffic sync skipped during task, lock={lockedSite}, reportSite={site.id}, node={report.Node.Node}");
                }

                return;
            }

            var idleLockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1;
            if (idleLockedSite == site.id)
            {
                _lastSyncedTrafficSiteId = site.id;
                return;
            }

            if (_lastSyncedTrafficSiteId == site.id && idleLockedSite == site.id)
            {
                return;
            }

            // 双车叠在同一站时，后到车不能 TrafficReset 抢锁；离线/错站的陈旧占锁则回收后重占。
            var holder = FindOtherCarHoldingSite(site.id);
            if (holder != null)
            {
                if (TryReleaseStaleHolder(holder, site.id))
                {
                    DetailLog(
                        $"traffic sync released stale holder car={holder.id}, site={site.id}, node={report.Node.Node}");
                }
                else
                {
                    if (_lastSyncedTrafficSiteId != -site.id)
                    {
                        DetailLog(
                            $"traffic sync skip, site={site.id} held by car={holder.id}, selfLock={idleLockedSite}, node={report.Node.Node}");
                        _lastSyncedTrafficSiteId = -site.id;
                    }

                    return;
                }
            }

            try
            {
                DetailLog($"traffic sync TrafficReset, lock={idleLockedSite}, targetSite={site.id}, node={report.Node.Node}");
                TrafficReset(site, true, strict: false);
                _lastSyncedTrafficSiteId = site.id;
            }
            catch (Exception ex)
            {
                DetailLog($"traffic sync failed: {ex.Message}");
            }
        }

        private Car FindOtherCarHoldingSite(int siteId)
        {
            foreach (var other in SimpleLib.GetAllCars().OfType<Car>())
            {
                if (other == null || other.id == id)
                {
                    continue;
                }

                if (other.status?.holdingLocks != null &&
                    Array.IndexOf(other.status.holdingLocks, siteId) >= 0)
                {
                    return other;
                }
            }

            return null;
        }

        /// <summary>
        /// 他车离线，或上报站点已离开，却仍占着本站锁 → 视为陈旧占锁，允许回收。
        /// </summary>
        private bool TryReleaseStaleHolder(Car other, int siteId)
        {
            if (other == null)
            {
                return false;
            }

            var online = other.tags?.Contains("Online") == true;
            var physicalSiteId = other is Mag2Car mag2 && mag2.siteID > 0
                ? mag2.siteID
                : other.GetLastSite();

            var stale = !online || (physicalSiteId > 0 && physicalSiteId != siteId);
            if (!stale)
            {
                return false;
            }

            try
            {
                TrafficControl.Leave(other, siteId);
                return Array.IndexOf(other.status.holdingLocks ?? Array.Empty<int>(), siteId) < 0;
            }
            catch (Exception ex)
            {
                DetailLog($"traffic sync release stale holder failed: car={other.id}, site={siteId}, {ex.Message}");
                return false;
            }
        }

        private Site ResolveSite(ushort node)
        {
            if (node == 0)
            {
                return null;
            }

            if (UseTagValueAsNode)
            {
                var byTagValue = SimpleLib.GetAllSites().FirstOrDefault(site =>
                    site.fields.TryGetValue("TagValue", out var tag) &&
                    int.TryParse(tag, out var tagValue) &&
                    tagValue == node);
                if (byTagValue != null)
                {
                    return byTagValue;
                }
            }

            return SimpleLib.GetAllSites().FirstOrDefault(site => site.id == node);
        }

        internal ushort ResolveNodeId(int siteId)
        {
            var site = SimpleLib.GetSite(siteId);
            if (UseTagValueAsNode &&
                site.fields.TryGetValue("TagValue", out var tag) &&
                ushort.TryParse(tag, out var tagValue))
            {
                return tagValue;
            }

            return (ushort)Clamp(siteId, 0, ushort.MaxValue);
        }

        private static int Clamp(int value, int min, int max)
        {
            if (value < min) return min;
            return value > max ? max : value;
        }

        private void DetailLog(string message)
        {
            WriteFileLog(message);

            if (!EnableDetailLog)
            {
                return;
            }

            var text = $"[Mag2Car:{name}({id})] {DateTime.Now:HH:mm:ss.fff} {message}";
            AppendDebug(text);
            Diagnosis.Post(text, "Mag2Car", true);
        }

        private void WriteFileLog(string message)
        {
            if (!EnableFileLog)
            {
                return;
            }

            var logVehicleCode = ResolveLogVehicleCode();
            if (logVehicleCode == 0)
            {
                return;
            }

            Mag2CarFileLogger.Configure(LogDirectory, true);
            Mag2CarFileLogger.Write(logVehicleCode, message);
        }

        private ushort ResolveLogVehicleCode()
        {
            if (VehicleCode != 0)
            {
                return VehicleCode;
            }

            if (id <= 0 || id > ushort.MaxValue)
            {
                return 0;
            }

            return (ushort)id;
        }

        private void TryLogLoopAssignmentToFile()
        {
            if (!EnableFileLog || !TryGetGoalSiteId(out var goalSiteId))
            {
                _lastFileLoggedGoalSite = -1;
                return;
            }

            if (_lastFileLoggedGoalSite == goalSiteId)
            {
                return;
            }

            _lastFileLoggedGoalSite = goalSiteId;
            var startSite = GetLastSite() > 0 ? GetLastSite() : siteID;
            WriteFileLog($"AutoLoop: 车辆 {name} 从站点 {startSite} 自动分配到目标站点 {goalSiteId}");
        }

        private static string CommandName(byte command)
        {
            switch (command)
            {
                case Fass2Protocol.CmdQuery: return "Query(0x00)";
                case Fass2Protocol.CmdStart: return "Start(0x01)";
                case Fass2Protocol.CmdStop: return "Stop(0x02)";
                case Fass2Protocol.CmdEmergencyStop: return "EmergencyStop(0x03)";
                case Fass2Protocol.CmdReset: return "Reset(0x04)";
                case Fass2Protocol.CmdRest: return "Rest(0x05)";
                case Fass2Protocol.CmdShutdown: return "Shutdown(0x06)";
                default: return $"Unknown(0x{command:X2})";
            }
        }

        internal sealed class Mag2CarInterface : AGVInterface
        {
            private readonly Mag2Car _car;

            public Mag2CarInterface(int id)
            {
                _car = (Mag2Car)SimpleLib.GetCar(id);
            }

            public override bool TryLock(int siteId)
            {
                if (!_car.status.usage.Get().scheduling)
                {
                    throw new Exception("abandoned");
                }

                if (_car.status.pendingLocks.Length > 0 && _car.status.pendingLocks[0] != siteId)
                {
                    throw new Exception("lock not according to sequence");
                }

                return TrafficControl.TryLock(_car, siteId);
            }

            public override void Leave(int siteId)
            {
                TrafficControl.Leave(_car, siteId);
            }

            public void Mag2Go(int srcId, int dstId, double speed = -1)
            {
                var promise = new TaskCompletionSource<int>();
                Queue(async () =>
                {
                    _car.DetailLog($"Mag2Go src={srcId}, dst={dstId}, speed={speed}, traffic={_car.EnableTrafficControl}");

                    if (_car.EnableTaskStateMachine)
                    {
                        _car.BeginTrackedTask(srcId, dstId, speed);
                        await _car.WaitTrackedTaskAsync(_car.MoveTimeoutSeconds * 1000);
                    }
                    else
                    {
                        if (_car.StartBeforeMove)
                        {
                            _car.SendControl(Fass2Protocol.CmdStart, 0);
                        }

                        var plan = _car.BuildTaskPlan(srcId, dstId, speed);
                        _car.DetailLog(
                            $"Mag2Go plan sites={plan.SiteIds.Count}, nodes={plan.Nodes.Count}, batches={plan.Batches.Count}");
                        _car.SendTaskPlan(plan);
                        _car.WaitUntilArrived(_car.ResolveNodeId(dstId), _car.MoveTimeoutSeconds * 1000);
                    }

                    promise.SetResult(1);
                }, async () =>
                {
                    await promise.Task;
                });
            }
        }
    }
}
