StandardSence/StandardScene.MagCarSimulator/Runtime/MagCarSimVehicle.cs

using System;
using System.Collections.Generic;
using System.Net;

namespace StandardScene.MagCarSimulator
{
    public sealed class MagCarSimVehicleSnapshot
    {
        public string Name { get; set; }
        public ushort VehicleCode { get; set; }
        public int ListenPort { get; set; }
        public int StartSiteId { get; set; }
        public int EndSiteId { get; set; }
        public int IntervalMs { get; set; }
        public int CurrentSiteId { get; set; }
        public ushort Node { get; set; }
        public byte State { get; set; }
        public string StateText { get; set; }
        public bool WaitingRelease { get; set; }
        public bool HoldingForStop { get; set; }
        public bool Looping { get; set; }
        public bool Listening { get; set; }
        public int ClientCount { get; set; }
        public string PathText { get; set; }
    }

    /// <summary>
    /// 单车循环：普通站可叠车；停止点与交管点同时只允许一辆，后车停在上一站并保持运行中；仅 Mag_NeedStop 停车等 0x01。
    /// </summary>
    public sealed class MagCarSimVehicle : IDisposable
    {
        private readonly object _syncRoot = new object();
        private readonly MagCarSimConfig _hostConfig;
        private MagCarSimTcpServer _server;
        private SimpleLiteMap _map;
        private List<int> _loopSiteIds = new List<int>();
        private int _index;
        private DateTime _nextHopAt = DateTime.MaxValue;
        private DateTime _dispatchAt = DateTime.MinValue;
        private DateTime _lastHoldLogAt = DateTime.MinValue;
        private bool _looping;
        private bool _dispatched;
        private bool _waitingRelease;
        private bool _pausedByStopCommand;
        private bool _holdingForStop;

        public MagCarSimVehicle(MagCarSimVehicleConfig config, MagCarSimConfig hostConfig)
        {
            Config = config ?? throw new ArgumentNullException(nameof(config));
            _hostConfig = hostConfig ?? throw new ArgumentNullException(nameof(hostConfig));
            Name = string.IsNullOrWhiteSpace(config.Name) ? $"AGV-{config.VehicleCode}" : config.Name;
            Charge = 100;
            Voltage = 24f;
            Speed = 50;
        }

        public MagCarSimVehicleConfig Config { get; }
        public string Name { get; }
        public ushort VehicleCode => Config.VehicleCode;
        public int ListenPort => Config.ListenPort;
        public int CurrentSiteId { get; private set; }
        public ushort Node { get; private set; }
        public byte State { get; private set; }
        public byte Charge { get; private set; }
        public float Current { get; private set; }
        public float Voltage { get; private set; }
        public byte Speed { get; private set; }
        public ushort Angle { get; private set; }
        public byte Task { get; private set; }
        public byte Lift { get; private set; }
        public byte Roll { get; private set; }
        public bool WaitingRelease => _waitingRelease;
        public bool HoldingForStop => _holdingForStop;
        public bool Looping => _looping;

        public bool Listening => _server != null && _server.IsRunning;

        public void AttachMap(SimpleLiteMap map)
        {
            lock (_syncRoot)
            {
                _map = map;
                if (!_looping)
                {
                    PlaceAtStartUnlocked();
                }
            }
        }

        public void StartListen()
        {
            MagCarSimTcpServer server;
            lock (_syncRoot)
            {
                if (_server != null && _server.IsRunning)
                {
                    return;
                }

                _server?.Dispose();
                var address = string.IsNullOrWhiteSpace(_hostConfig.ListenAddress)
                    ? "0.0.0.0"
                    : _hostConfig.ListenAddress;
                server = new MagCarSimTcpServer(IPAddress.Parse(address), Config.ListenPort, HandleFrame);
                _server = server;
                PlaceAtStartUnlocked();
                Log($"监听 {address}:{Config.ListenPort}，当前站={CurrentSiteId} node={Node} {MagCarSimProtocol.StateText(State)}");
            }

            server.Start();
        }

        public void StopListen()
        {
            MagCarSimTcpServer server;
            lock (_syncRoot)
            {
                server = _server;
                _server = null;
            }

            server?.Dispose();
        }

        public void StartLoop(DateTime dispatchAt)
        {
            lock (_syncRoot)
            {
                if (_map == null)
                {
                    throw new InvalidOperationException($"{Name} 尚未加载地图");
                }

                _loopSiteIds = MapPathFinder.ResolveLoop(_map, Config);
                _index = 0;
                if (Config.StartSiteId > 0)
                {
                    var found = _loopSiteIds.IndexOf(Config.StartSiteId);
                    if (found >= 0)
                    {
                        _index = found;
                    }
                }

                _looping = true;
                _dispatched = false;
                _holdingForStop = false;
                _pausedByStopCommand = false;
                _waitingRelease = false;
                _dispatchAt = dispatchAt;
                PlaceAtStartUnlocked();
                // 待命阶段若起点是停止点，先报运行中，避免交管误放行并占住 pending，导致真发车后长时间不放。
                if (_map != null && _map.NeedStop(CurrentSiteId))
                {
                    State = MagCarSimProtocol.StateRunning;
                    _waitingRelease = false;
                }

                var delayMs = Math.Max(0, (int)(dispatchAt - DateTime.Now).TotalMilliseconds);
                Log($"待命发车，环线 {MapPathFinder.FormatPath(_loopSiteIds, 16)}，间隔 {IntervalOrDefault()}ms" +
                    (delayMs > 50 ? $"，{delayMs}ms 后启动" : "，立即启动"));
            }
        }

        public void StopLoop()
        {
            lock (_syncRoot)
            {
                _looping = false;
                _dispatched = false;
                _waitingRelease = false;
                _holdingForStop = false;
                _pausedByStopCommand = false;
                _nextHopAt = DateTime.MaxValue;
                State = MagCarSimProtocol.StateStopping;
                Log("已停止循环");
            }
        }

        public void Release()
        {
            lock (_syncRoot)
            {
                ApplyStartUnlocked("界面放行");
            }
        }

        public void Tick(DateTime now, Dictionary<int, ushort> occupancy)
        {
            lock (_syncRoot)
            {
                if (!_looping || _pausedByStopCommand)
                {
                    return;
                }

                if (!_dispatched)
                {
                    TryDispatchUnlocked(now, occupancy);
                    return;
                }

                if (_waitingRelease)
                {
                    return;
                }

                if (State != MagCarSimProtocol.StateRunning)
                {
                    return;
                }

                if (now < _nextHopAt)
                {
                    return;
                }

                AdvanceUnlocked(now, occupancy);
            }
        }

        public MagCarSimVehicleSnapshot Snapshot()
        {
            lock (_syncRoot)
            {
                return new MagCarSimVehicleSnapshot
                {
                    Name = Name,
                    VehicleCode = VehicleCode,
                    ListenPort = ListenPort,
                    StartSiteId = Config.StartSiteId,
                    EndSiteId = Config.EndSiteId,
                    IntervalMs = Config.IntervalMs,
                    CurrentSiteId = CurrentSiteId,
                    Node = Node,
                    State = State,
                    StateText = MagCarSimProtocol.StateText(State),
                    WaitingRelease = _waitingRelease,
                    HoldingForStop = _holdingForStop,
                    Looping = _looping,
                    Listening = Listening,
                    ClientCount = _server?.ClientCount ?? 0,
                    PathText = MapPathFinder.FormatPath(_loopSiteIds, 12)
                };
            }
        }

        public void Dispose()
        {
            lock (_syncRoot)
            {
                _looping = false;
            }

            StopListen();
        }

        private byte[] HandleFrame(byte[] request)
        {
            lock (_syncRoot)
            {
                if (!MagCarSimProtocol.TryParse(request, out var parsed))
                {
                    Log("收到非法帧");
                    return BuildStatusUnlocked(MagCarSimProtocol.CmdQueryState);
                }

                if (parsed.VehicleCode != 0 && parsed.VehicleCode != VehicleCode)
                {
                    Log($"帧车号 {parsed.VehicleCode} 与本车 {VehicleCode} 不一致，仍按本车应答");
                }

                if (_hostConfig.LogRawFrames)
                {
                    Log($"RX {MagCarSimProtocol.CommandName(parsed.Command)} {MagCarSimProtocol.ToHex(request)}");
                }
                else if (parsed.Command != MagCarSimProtocol.CmdQueryState || _hostConfig.LogQueries)
                {
                    Log($"RX {MagCarSimProtocol.CommandName(parsed.Command)}");
                }

                switch (parsed.Command)
                {
                    case MagCarSimProtocol.CmdStart:
                        ApplyStartUnlocked("0x01");
                        break;
                    case MagCarSimProtocol.CmdStop:
                        _pausedByStopCommand = true;
                        State = MagCarSimProtocol.StateStopping;
                        Log("收到停止，暂停循环");
                        break;
                    case MagCarSimProtocol.CmdSpeed:
                        Speed = (byte)Math.Clamp((int)parsed.Data0 >> 8, 0, 100);
                        break;
                    case MagCarSimProtocol.CmdAngle:
                        Angle = parsed.Data0;
                        break;
                    case MagCarSimProtocol.CmdLegacyNode:
                    case MagCarSimProtocol.CmdTask:
                        Log($"忽略路径命令 {MagCarSimProtocol.CommandName(parsed.Command)}（本模拟器按 WinForm 循环跑）");
                        break;
                }

                return BuildStatusUnlocked(parsed.Command);
            }
        }

        private void ApplyStartUnlocked(string source)
        {
            if (!_dispatched)
            {
                Log($"待命中忽略放行（{source}）");
                return;
            }

            if (_waitingRelease)
            {
                _waitingRelease = false;
                _pausedByStopCommand = false;
                State = MagCarSimProtocol.StateRunning;
                _nextHopAt = DateTime.Now.AddMilliseconds(IntervalOrDefault());
                Log($"放行成功（{source}），站点 {CurrentSiteId} 改为运行中，{IntervalOrDefault()}ms 后前往下一站");
                return;
            }

            if (_pausedByStopCommand && _looping)
            {
                _pausedByStopCommand = false;
                State = MagCarSimProtocol.StateRunning;
                _nextHopAt = DateTime.Now.AddMilliseconds(IntervalOrDefault());
                Log($"从暂停恢复（{source}）");
                return;
            }

            if (_looping && State != MagCarSimProtocol.StateRunning)
            {
                State = MagCarSimProtocol.StateRunning;
                _nextHopAt = DateTime.Now.AddMilliseconds(IntervalOrDefault());
                Log($"启动运行（{source}）");
            }
        }

        private void PlaceAtStartUnlocked()
        {
            CurrentSiteId = Config.StartSiteId;
            Node = _map == null ? (ushort)Math.Clamp(Config.StartSiteId, 0, ushort.MaxValue) : _map.ResolveNode(Config.StartSiteId);
            State = MagCarSimProtocol.StateStopping;
            _waitingRelease = false;
            _pausedByStopCommand = false;
        }

        private void ArriveUnlocked(int siteId, bool isStart)
        {
            CurrentSiteId = siteId;
            Node = _map.ResolveNode(siteId);
            var needStop = _map.NeedStop(siteId);
            if (needStop)
            {
                State = MagCarSimProtocol.StateStopping;
                _waitingRelease = true;
                _nextHopAt = DateTime.MaxValue;
                Log($"{(isStart ? "起点" : "到站")} {siteId} node={Node} Mag_NeedStop=true → 停止中，等待放行");
                return;
            }

            State = MagCarSimProtocol.StateRunning;
            _waitingRelease = false;
            _nextHopAt = DateTime.Now.AddMilliseconds(IntervalOrDefault());
            if (isStart)
            {
                Log($"起点 {siteId} node={Node} 过站，{IntervalOrDefault()}ms 后下一地标");
            }
        }

        private void TryDispatchUnlocked(DateTime now, Dictionary<int, ushort> occupancy)
        {
            if (now < _dispatchAt)
            {
                return;
            }

            if (_loopSiteIds.Count == 0)
            {
                return;
            }

            var startSite = _loopSiteIds[_index];
            if (IsOccupiedUnlocked(startSite, occupancy))
            {
                HoldForOccupiedUnlocked(now, startSite, "发车");
                return;
            }

            OccupyUnlocked(occupancy, startSite);
            _dispatched = true;
            _holdingForStop = false;
            ArriveUnlocked(startSite, isStart: true);
        }

        private void AdvanceUnlocked(DateTime now, Dictionary<int, ushort> occupancy)
        {
            if (_loopSiteIds.Count < 2)
            {
                return;
            }

            var nextIndex = (_index + 1) % _loopSiteIds.Count;
            var nextSite = _loopSiteIds[nextIndex];
            if (IsOccupiedUnlocked(nextSite, occupancy))
            {
                HoldForOccupiedUnlocked(now, nextSite, "进站");
                return;
            }

            MoveOccupancyUnlocked(occupancy, CurrentSiteId, nextSite);
            _holdingForStop = false;
            _index = nextIndex;
            ArriveUnlocked(nextSite, isStart: false);
        }

        private bool IsOccupiedUnlocked(int siteId, Dictionary<int, ushort> occupancy)
        {
            if (_map == null || occupancy == null || !_map.SingleOccupancy(siteId))
            {
                return false;
            }

            return occupancy.TryGetValue(siteId, out var occupier) && occupier != VehicleCode;
        }

        private void OccupyUnlocked(Dictionary<int, ushort> occupancy, int siteId)
        {
            if (occupancy == null || siteId <= 0 || _map == null || !_map.SingleOccupancy(siteId))
            {
                return;
            }

            occupancy[siteId] = VehicleCode;
        }

        private void MoveOccupancyUnlocked(Dictionary<int, ushort> occupancy, int fromSiteId, int toSiteId)
        {
            if (occupancy == null)
            {
                return;
            }

            if (fromSiteId > 0 &&
                occupancy.TryGetValue(fromSiteId, out var occupier) &&
                occupier == VehicleCode)
            {
                occupancy.Remove(fromSiteId);
            }

            OccupyUnlocked(occupancy, toSiteId);
        }

        private void HoldForOccupiedUnlocked(DateTime now, int occupiedSiteId, string action)
        {
            _holdingForStop = true;
            State = MagCarSimProtocol.StateRunning;
            _waitingRelease = false;
            _nextHopAt = now.AddMilliseconds(200);
            if ((now - _lastHoldLogAt).TotalMilliseconds >= 5000)
            {
                _lastHoldLogAt = now;
                Log($"停止/交管点 {occupiedSiteId} 已有车，{action}受阻，保持站点 {CurrentSiteId} 运行中");
            }
        }

        private int IntervalOrDefault()
        {
            var value = Config.IntervalMs > 0 ? Config.IntervalMs : _hostConfig.DefaultIntervalMs;
            return Math.Max(200, value);
        }

        private byte[] BuildStatusUnlocked(byte command)
        {
            return MagCarSimProtocol.BuildStatus(
                command,
                VehicleCode,
                Node,
                State,
                Charge,
                Current,
                Voltage,
                Speed,
                Angle,
                Task,
                Lift,
                Roll);
        }

        private void Log(string message)
        {
            MagCarSimLog.WriteLine($"[{DateTime.Now:HH:mm:ss.fff}] [{Name}/{VehicleCode}] {message}");
        }
    }
}