StandardSence/StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs

using System;

namespace StandardScene.Fass2Simulator
{
    /// <summary>
    /// 按 0xB1 节点序列模拟行驶：距离推进 → 到站更新 Node/字段 → State 切为停止供调度判定到站。
    /// </summary>
    public sealed class Fass2SimMotionEngine
    {
        private readonly Fass2SimVehicle _vehicle;
        private readonly Fass2SimConfig _config;
        private readonly Fass2SimActionEngine _actions;
        private readonly object _syncRoot = new object();

        private ulong _activeTaskId;
        private Fass2SimNodeMessage[] _windowNodes = Array.Empty<Fass2SimNodeMessage>();
        private Fass2SimNodeMessage _segmentTarget;
        private double _segmentProgress;
        private double _segmentLength;
        private bool _segmentActive;

        public Fass2SimMotionEngine(Fass2SimVehicle vehicle, Fass2SimConfig config, Fass2SimActionEngine actions)
        {
            _vehicle = vehicle;
            _config = config;
            _actions = actions;
        }

        public void OnNodesCommand(ulong taskId, Fass2SimNodeMessage[] nodes)
        {
            if (nodes == null || nodes.Length == 0)
            {
                Fass2SimLog.WriteLine($"[{Now()}]    -> 0xB1 节点为空，忽略");
                return;
            }

            lock (_syncRoot)
            {
                var taskChanged = _activeTaskId != taskId;
                _activeTaskId = taskId;
                _windowNodes = nodes;
                _vehicle.Task = taskId;

                if (taskChanged)
                {
                    _actions.ResetForNewTask();
                    ResetSegment();
                }

                ApplyCurrentNodeFields(nodes);
                LogNodes(nodes);

                if (_vehicle.State == 3)
                {
                    Fass2SimLog.WriteLine($"[{Now()}]    -> 急停中，不启动运动");
                    return;
                }

                if (_segmentActive)
                {
                    if (taskChanged)
                    {
                        Fass2SimLog.WriteLine($"[{Now()}]    -> 新 taskId，重置路段");
                        ResetSegment();
                    }
                    else
                    {
                        return;
                    }
                }

                if (!ContainsNode(nodes, _vehicle.Node))
                {
                    Fass2SimLog.WriteLine(
                        $"[{Now()}]    -> 0xB1 窗口不含当前 node={_vehicle.Node}，忽略启动路段（避免回退到窗口首站）");
                    return;
                }

                TryStartNextSegment();
            }
        }

        private static bool ContainsNode(Fass2SimNodeMessage[] nodes, ushort node)
        {
            if (nodes == null)
            {
                return false;
            }

            for (var i = 0; i < nodes.Length; i++)
            {
                if (nodes[i].Node == node)
                {
                    return true;
                }
            }

            return false;
        }

        public void OnStartCommand()
        {
            lock (_syncRoot)
            {
                if (_vehicle.State == 3)
                {
                    return;
                }

                TryStartNextSegment();
            }
        }

        public void OnStopCommand()
        {
            lock (_syncRoot)
            {
                ResetSegment();
            }
        }

        public void OnStationActionsCompleted()
        {
            lock (_syncRoot)
            {
                TryResumeFromStation();
            }
        }

        public void Tick(int deltaMs)
        {
            if (deltaMs <= 0)
            {
                return;
            }

            lock (_syncRoot)
            {
                if (!_segmentActive || _segmentTarget == null || _vehicle.State != 1)
                {
                    return;
                }

                var speed = ResolveSpeed(_segmentTarget);
                var deltaDistance = speed * deltaMs / 1000.0;
                _segmentProgress += deltaDistance;
                _vehicle.Distance = (ushort)Math.Min(ushort.MaxValue, Math.Round(_segmentProgress));

                if (_segmentProgress < _segmentLength)
                {
                    return;
                }

                ArriveAtTarget();
            }
        }

        private void TryStartNextSegment()
        {
            if (_vehicle.State != 1 && _vehicle.State != 2)
            {
                return;
            }

            if (_vehicle.State == 2 && !_actions.CanLeaveStation())
            {
                return;
            }

            var target = ResolveNextTarget(_windowNodes, _vehicle.Node);
            if (target == null)
            {
                if (_vehicle.State == 2)
                {
                    Fass2SimLog.WriteLine($"[{Now()}]    -> 窗口内无下一段，等待下一帧 0xB1");
                }
                return;
            }

            _segmentTarget = target;
            _segmentLength = ResolveSegmentLength(_vehicle.Node, target);
            _segmentProgress = 0;
            _segmentActive = true;
            _vehicle.State = 1;
            _vehicle.Distance = 0;

            if (target.Speed > 0)
            {
                _vehicle.Speed = target.Speed;
            }

            Fass2SimLog.WriteLine(
                $"[{Now()}]    -> 开始路段: {_vehicle.Node} -> {target.Node}, dist={_segmentLength:F0}mm, speed={ResolveSpeed(target):F0}mm/s");
        }

        private void ArriveAtTarget()
        {
            var target = _segmentTarget;
            _vehicle.Node = target.Node;
            _vehicle.Distance = 0;
            ResetSegment();
            _actions.OnStationArrival(target);

            Fass2SimLog.WriteLine(
                $"[{Now()}]    -> 到站 node={target.Node}, StartStop={target.StartStop}, state={Fass2SimProtocol.StateText(_vehicle.State)}");

            TryResumeFromStation();
        }

        private void TryResumeFromStation()
        {
            if (!_config.AutoContinueOnPass)
            {
                return;
            }

            var expected = _actions.ExpectedStation;
            if (expected == null || !Fass2SimActionResolver.AllowsAutoContinue(expected.StartStop))
            {
                return;
            }

            if (_vehicle.State != 2 || !_actions.CanLeaveStation() || !HasNextTarget(_vehicle.Node))
            {
                return;
            }

            _vehicle.State = 1;
            TryStartNextSegment();
        }

        private void ResetSegment()
        {
            _segmentTarget = null;
            _segmentProgress = 0;
            _segmentLength = 0;
            _segmentActive = false;
        }

        private bool HasNextTarget(ushort currentNode)
        {
            return ResolveNextTarget(_windowNodes, currentNode) != null;
        }

        private static Fass2SimNodeMessage ResolveNextTarget(Fass2SimNodeMessage[] nodes, ushort currentNode)
        {
            if (nodes == null || nodes.Length == 0)
            {
                return null;
            }

            // 必须先在窗口内定位当前站，再取其后一站。
            // 旧逻辑在窗口不含当前站时会落到 nodes[0]，把更早的站当成下一目标 → 车体 Node 瞬间回退/瞬移。
            var foundCurrent = false;
            var startIndex = 0;
            for (var i = 0; i < nodes.Length; i++)
            {
                if (nodes[i].Node == currentNode)
                {
                    foundCurrent = true;
                    startIndex = i + 1;
                }
            }

            if (!foundCurrent)
            {
                return null;
            }

            for (var i = startIndex; i < nodes.Length; i++)
            {
                var node = nodes[i];
                if (node.Node == 0 || node.Node == currentNode)
                {
                    continue;
                }

                return node;
            }

            return null;
        }

        private void ApplyCurrentNodeFields(Fass2SimNodeMessage[] nodes)
        {
            foreach (var node in nodes)
            {
                if (node.Node == _vehicle.Node && node.StartStop > 0)
                {
                    _vehicle.StartStop = node.StartStop;
                    _actions.MergeExpected(node);
                }
            }
        }

        private double ResolveSegmentLength(ushort fromNode, Fass2SimNodeMessage target)
        {
            foreach (var node in _windowNodes)
            {
                if (node.Node == fromNode && node.Distance > 0)
                {
                    return node.Distance;
                }
            }

            if (target.Distance > 0)
            {
                return target.Distance;
            }

            if (_config.DefaultSegmentDistance > 0)
            {
                return _config.DefaultSegmentDistance;
            }

            var speed = ResolveSpeed(target);
            return Math.Max(100, speed * _config.SecondsPerSegment);
        }

        private double ResolveSpeed(Fass2SimNodeMessage target)
        {
            if (target.Speed > 0)
            {
                return Fass2SimProtocol.ProtocolSpeedToMmPerSec(target.Speed);
            }

            if (_vehicle.Speed > 0)
            {
                return Fass2SimProtocol.ProtocolSpeedToMmPerSec(_vehicle.Speed);
            }

            return _config.DefaultSpeed;
        }

        private void LogNodes(Fass2SimNodeMessage[] nodes)
        {
            var summary = string.Join(" -> ", Array.ConvertAll(nodes, n => n.Node.ToString()));
            Fass2SimLog.WriteLine($"[{Now()}]    -> 0xB1 taskId={_activeTaskId}, nodes=[{summary}]");
        }

        private static string Now()
        {
            return DateTime.Now.ToString("HH:mm:ss.fff");
        }

        /// <summary>
        /// 供界面展示路段进度：行驶中返回目标节点与路段长度，到站后 node 才会在协议里跳变。
        /// </summary>
        public bool TryGetSegmentStatus(out ushort targetNode, out int segmentLengthMm, out int progressMm)
        {
            lock (_syncRoot)
            {
                if (!_segmentActive || _segmentTarget == null)
                {
                    targetNode = 0;
                    segmentLengthMm = 0;
                    progressMm = 0;
                    return false;
                }

                targetNode = _segmentTarget.Node;
                segmentLengthMm = (int)Math.Round(_segmentLength);
                progressMm = (int)Math.Round(_segmentProgress);
                return true;
            }
        }
    }
}