StandardSence/StandardScene.Magnetic/CarTypes/MagCar.cs

using LessokajiWeaverUtilities.Utilities;
using SimpleLite;
using SimpleLite.RCS;
using SimpleLite.RCS.CarTypes;
using SimpleLite.Props;
using SimpleLite.UI;
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.Library;
using SimpleCore.PropType;
using StandardScene.Magnetic.Coders;
using StandardScene.Utils;
using System;
using System.Diagnostics;
using System.Drawing;
using System.Net.Sockets;
using System.Threading;
using System.Threading.Tasks;

namespace StandardScene.CarTypes
{
    /// <summary>
    /// 磁导航专用车型（scene.mag 平台）。
    /// <para>与激光/二维码车（Kiva、叉车等走 HTTP:8008）不同：磁导航车与下位机通过 <b>UDP 协议</b> 交互；
    /// 路径由 <see cref="MagneticTrackCoder"/> 生成 <c>agv.MagGo</c> / <c>agv.NaiveMagGo</c> 脚本
    /// （按 <c>track.Magnet</c> 触发）。本类只承载磁导航相关能力。</para>
    /// <para>⚠ UDP 报文的端口 / 字节布局 / 命令码因下位机协议而异，下方以常量 + TODO 标注，
    /// 待按实际磁导航 AGV 协议填充 <see cref="ParseStatus"/> 与各控制命令的编码。</para>
    /// </summary>
    [ProgramTrackCoderSettings(priority = 19, program = typeof(MagneticTrackCoder))]
    [CarType(Name = "磁导航车", editor = typeof(MagCar))]
    [I18N.DocumentTranslation(Name = "Magnetic AGV", locale = "en")]
    [EnvelopConfig(centerX = 0, centerY = 0, lengthX = 1000, lengthY = 600)]
    public class MagCar : GhostCar
    {
        // ── 磁导航 UDP 通信参数（默认值待按实际下位机协议确认）──────────────────
        /// <summary>下位机 UDP 命令端口（RCS → AGV 下发）。</summary>
        [FieldMember] public int UdpCommandPort = 5000;

        /// <summary>本机 UDP 状态监听端口（AGV → RCS 上报）。</summary>
        [FieldMember] public int UdpStatusPort = 5001;

        [FieldMember] public float CarLength = 1000;
        [FieldMember] public float CarWidth = 600;

        private UdpClient _udp;
        private CancellationTokenSource _cts;
        private readonly object _sendLock = new object();

        public static async Task<MagCar> Create()
        {
            var car = new MagCar
            {
                lstatus = "连接中",
                address = "127.0.0.1",
                name = "磁导航车",
                haveCoordination = true,
                speed = 1,
            };
            car.StartUdp();
            return car;
        }

        // ── UDP 通信：协作式停止 + 单帧异常隔离（参考 ChargeUdpService 范式）─────────
        /// <summary>启动磁导航 UDP 通信（幂等：会先停止已有连接）。</summary>
        public void StartUdp()
        {
            StopUdp();
            _cts = new CancellationTokenSource();
            var token = _cts.Token;
            Task.Run(() => ReceiveLoop(token), token);
        }

        /// <summary>停止磁导航 UDP 通信并释放资源。</summary>
        public void StopUdp()
        {
            try { _cts?.Cancel(); } catch { /* 取消令牌释放兜底 */ }
            try { _udp?.Close(); } catch { /* 关闭套接字兜底 */ }
            _udp = null;
            _cts = null;
        }

        private async Task ReceiveLoop(CancellationToken token)
        {
            try
            {
                _udp = new UdpClient(UdpStatusPort);
            }
            catch (Exception ex)
            {
                // 多车共用监听端口时此处会绑定失败——若实际协议为单端口多车，应改为单例监听 + 按来源 IP 分发
                Diagnosis.Log($"磁导航车{name}({id}) UDP 监听端口 {UdpStatusPort} 绑定失败: {ex.Message}", "MagCar", true);
                return;
            }

            while (!token.IsCancellationRequested)
            {
                try
                {
                    var result = await _udp.ReceiveAsync();
                    // 多车共用监听端口时按来源 IP 过滤，仅处理本车报文
                    if (!string.IsNullOrEmpty(address) &&
                        !string.Equals(result.RemoteEndPoint.Address.ToString(), address, StringComparison.Ordinal))
                        continue;
                    ParseStatus(result.Buffer);
                }
                catch (Exception e)
                {
                    // 单帧异常（越界 / 半包 / 套接字关闭）不得中断本车 UDP 接收循环
                    if (token.IsCancellationRequested) break;
                    Diagnosis.Log($"磁导航车{name}({id}) UDP 接收异常: {e.Message}", "MagCar", true);
                }
            }
        }

        /// <summary>
        /// 解析下位机 UDP 状态报文并写入 <c>status.enums</c>。
        /// TODO（待协议确认）：下方字节偏移为占位，请按实际磁导航 AGV 协议替换。
        /// </summary>
        private void ParseStatus(byte[] data)
        {
            if (data == null || data.Length < 4) return; // 长度校验，避免半包 / 越界
            // —— 按实际协议填充，例如：——
            // status.enums["ChassisMode"]    = data[?].ToString();
            // status.enums["Soc"]            = data[?].ToString();
            // status.enums["AlarmLevel"]     = data[?].ToString();
            // status.enums["AlarmInfo"]      = ...;
            // 若磁导航上报绝对位姿，可在此回填 x / y / th（坐标系需与地图一致）。
        }

        /// <summary>向下位机发送 UDP 命令。TODO：命令字节编码按实际协议实现。</summary>
        private void SendUdp(byte[] payload)
        {
            if (payload == null || payload.Length == 0) return;
            try
            {
                lock (_sendLock)
                {
                    using var sender = new UdpClient();
                    sender.Send(payload, payload.Length, address, UdpCommandPort);
                }
            }
            catch (Exception e)
            {
                Diagnosis.Log($"磁导航车{name}({id}) UDP 发送失败: {e.Message}", "MagCar", true);
            }
        }

        protected override void draw(Graphics eGraphics)
        {
            try
            {
                var w = CarLength;
                var h = CarWidth;
                var rect = new Rectangle((int)(-w / 2), (int)(-h / 2), (int)w, (int)h);
                eGraphics.FillRectangle(Brushes.SteelBlue, rect);
                eGraphics.DrawRectangle(Pens.White, rect);
                using var orientPen = new Pen(Color.White, 3);
                eGraphics.DrawLine(orientPen, 0, 0, (int)(w / 2), 0); // 朝向指示
            }
            catch (Exception e)
            {
                Diagnosis.Post("绘制磁导航车异常" + ExceptionFormatter.FormatEx(e), "绘制小车异常", true);
            }
        }

        public override string SetDisplayInfo()
        {
            try
            {
                var str = $"{name}({id})\n";
                status.enums.TryGetValue("ChassisMode", out var manual);
                var ms = manual == "0" ? "自动" : "手动";
                status.enums.TryGetValue("Soc", out var soc);
                str = $"{str}{ms}|soc:{soc}";
                var alarmStr = Commons.GetCarStatus(this, "AlarmInfo");
                if (!string.IsNullOrEmpty(alarmStr))
                    str = $"{str}|{alarmStr}";
                return str;
            }
            catch (Exception e)
            {
                Console.WriteLine(e);
                return "bad car";
            }
        }

        [MethodMember(Name = "进入小车管理界面", Description = "在浏览器中打开小车 Web 页面")]
        [I18N.DocumentTranslation(Name = "Open vehicle management page", Description = "Open the car's management page in browser", locale = "en")]
        public void OpenVehicleManagementPage()
        {
            CarRemoteHelper.OpenVehicleWebPage(address);
        }

        [MethodMember(Name = "显示车辆监控", Description = "打开车辆状态监控窗口")]
        public void ShowVehicleMonitor() => CarRemoteHelper.OpenVehicleWebPage(address);

        [MethodMember(Name = "重连通信", Description = "重启磁导航 UDP 通信")]
        [I18N.DocumentTranslation(Name = "Reconnect", Description = "Restart magnetic UDP link", locale = "en")]
        public void ReconnectUdp() => StartUdp();
    }
}