StandardSence/StandardScene.Magnetic/Protocol/Fass2UdpHub.cs

using SimpleCore.Library;
using System;
using System.Collections.Generic;
using System.Net;
using System.Net.Sockets;
using System.Threading;

namespace StandardScene.Magnetic.Protocol
{
    /// <summary>
    /// FASS 2.0 PCB/UDP 共享监听枢纽。
    /// AGV 主动上报 100B 状态 → 解析并分发给对应车辆 → 回 0x10 时间戳应答到车辆 address:Port。
    /// 下发 0xB1/0xA1/0x01 等命令亦通过本枢纽单播到车辆 address:Port。
    /// </summary>
    internal static class Fass2UdpHub
    {
        private static readonly object SyncRoot = new object();
        private static readonly Dictionary<ushort, IFass2UdpCar> CarsByCode = new Dictionary<ushort, IFass2UdpCar>();

        private static UdpClient _listener;
        private static Thread _receiveThread;
        private static volatile bool _running;
        private static int _listenPort = 20103;
        private static readonly Dictionary<ushort, long> _lastUnknownCarLogUtcTicks = new Dictionary<ushort, long>();

        internal static bool IsRunning => _running;

        internal static int ListenPort => _listenPort;

        public static bool Register(IFass2UdpCar car)
        {
            if (car == null)
            {
                return false;
            }

            if (car.VehicleCode == 0)
            {
                Diagnosis.Post("Fass2UdpHub 拒绝注册 VehicleCode=0，请为每台 Mag2Car 配置唯一车号", "Fass2UdpHub", true);
                return false;
            }

            lock (SyncRoot)
            {
                if (CarsByCode.TryGetValue(car.VehicleCode, out var existing) &&
                    !ReferenceEquals(existing, car))
                {
                    Diagnosis.Post(
                        $"Fass2UdpHub 拒绝覆盖 VehicleCode={car.VehicleCode}（已被另一台车占用）",
                        "Fass2UdpHub",
                        true);
                    return false;
                }

                CarsByCode[car.VehicleCode] = car;
                return true;
            }
        }

        public static void Unregister(IFass2UdpCar car)
        {
            if (car == null)
            {
                return;
            }

            Unregister(car.VehicleCode, car);
        }

        public static void Unregister(ushort vehicleCode, IFass2UdpCar car)
        {
            if (car == null)
            {
                return;
            }

            lock (SyncRoot)
            {
                if (CarsByCode.TryGetValue(vehicleCode, out var existing) && ReferenceEquals(existing, car))
                {
                    CarsByCode.Remove(vehicleCode);
                }
            }
        }

        public static void EnsureStarted(int listenPort)
        {
            if (listenPort <= 0)
            {
                listenPort = _listenPort;
            }

            var needRestart = false;
            lock (SyncRoot)
            {
                if (_running && listenPort != _listenPort)
                {
                    needRestart = true;
                }
                else if (_running)
                {
                    return;
                }
            }

            if (needRestart)
            {
                Diagnosis.Post(
                    $"Fass2UdpHub 监听口 {_listenPort} → {listenPort}，重启枢纽",
                    "Fass2UdpHub",
                    true);
                Stop();
            }

            lock (SyncRoot)
            {
                if (_running)
                {
                    return;
                }

                _listenPort = listenPort;
                _listener = new UdpClient(_listenPort);
                _running = true;
                _receiveThread = new Thread(ReceiveLoop)
                {
                    IsBackground = true,
                    Name = $"Fass2UdpHub:{_listenPort}"
                };
                _receiveThread.Start();
                Diagnosis.Post($"Fass2UdpHub 已启动，监听 UDP {_listenPort}", "Fass2UdpHub", true);
            }
        }

        public static void Stop()
        {
            lock (SyncRoot)
            {
                _running = false;
                try
                {
                    _listener?.Close();
                }
                catch
                {
                }

                _listener = null;
            }

            try
            {
                _receiveThread?.Join(1000);
            }
            catch
            {
            }

            _receiveThread = null;
        }

        public static void SendToCar(IFass2UdpCar car, byte[] payload)
        {
            if (car == null || payload == null || payload.Length == 0)
            {
                return;
            }

            if (string.IsNullOrWhiteSpace(car.RemoteAddress) || car.RemotePort <= 0)
            {
                throw new InvalidOperationException(
                    $"Fass2UdpHub send failed: car VehicleCode={car.VehicleCode} remote endpoint not configured");
            }

            using var sender = new UdpClient();
            var remote = new IPEndPoint(IPAddress.Parse(car.RemoteAddress), car.RemotePort);
            sender.Send(payload, payload.Length, remote);
        }

        private static void ReceiveLoop()
        {
            while (_running)
            {
                try
                {
                    var remote = new IPEndPoint(IPAddress.Any, 0);
                    var packet = _listener.Receive(ref remote);
                    ProcessPacket(packet, remote);
                }
                catch (SocketException) when (!_running)
                {
                    break;
                }
                catch (ObjectDisposedException) when (!_running)
                {
                    break;
                }
                catch (Exception ex)
                {
                    Diagnosis.Post($"Fass2UdpHub receive error: {ex.Message}", "Fass2UdpHub", true);
                }
            }
        }

        private static void ProcessPacket(byte[] packet, IPEndPoint remote)
        {
            if (!Fass2Protocol.TryExtractStateFrame(packet, packet.Length, out var frame))
            {
                return;
            }

            Fass2StateReport report;
            try
            {
                report = Fass2Protocol.ParseState(frame);
            }
            catch (Exception ex)
            {
                Diagnosis.Post($"Fass2UdpHub parse error from {remote}: {ex.Message}", "Fass2UdpHub", true);
                return;
            }

            IFass2UdpCar car;
            lock (SyncRoot)
            {
                if (!CarsByCode.TryGetValue(report.Car, out car))
                {
                    LogUnknownCar(report.Car);
                    return;
                }
            }

            try
            {
                car.OnUdpStateReceived(report);
            }
            catch (Exception ex)
            {
                Diagnosis.Post($"Fass2UdpHub dispatch error car={report.Car}: {ex.Message}", "Fass2UdpHub", true);
            }

            try
            {
                var ack = Fass2Protocol.BuildStateResponse(report.Car, Fass2Protocol.BeijingUnixTimeMs());
                SendToCar(car, ack);
            }
            catch (Exception ex)
            {
                Diagnosis.Post($"Fass2UdpHub 0x10 reply failed car={report.Car}: {ex.Message}", "Fass2UdpHub", true);
            }
        }

        private static void LogUnknownCar(ushort carCode)
        {
            var nowTicks = DateTime.UtcNow.Ticks;
            string registered;
            lock (SyncRoot)
            {
                if (_lastUnknownCarLogUtcTicks.TryGetValue(carCode, out var lastTicks)
                    && nowTicks - lastTicks < TimeSpan.FromSeconds(5).Ticks)
                {
                    return;
                }

                _lastUnknownCarLogUtcTicks[carCode] = nowTicks;
                registered = CarsByCode.Count == 0
                    ? "(无)"
                    : string.Join(",", CarsByCode.Keys);
            }

            Diagnosis.Post(
                $"Fass2UdpHub 收到 Car={carCode} 的状态，但未注册该编号（已注册: {registered}）。请检查 Mag2Car.VehicleCode 与模拟器车号是否一致，并重新启动场景。",
                "Fass2UdpHub",
                true);
        }
    }
}