StandardSence/StandardScene.Fass2Simulator/Fass2SimCommandHandler.cs

using System;

namespace StandardScene.Fass2Simulator
{
    public sealed class Fass2SimCommandHandler
    {
        private readonly Fass2SimVehicle _vehicle;
        private readonly Fass2SimMotionEngine _motion;
        private readonly Fass2SimActionEngine _actions;
        private readonly bool _logRawFrames;
        private long _stateResponseCount;

        public Fass2SimCommandHandler(
            Fass2SimVehicle vehicle,
            Fass2SimMotionEngine motion,
            Fass2SimActionEngine actions,
            bool logRawFrames)
        {
            _vehicle = vehicle;
            _motion = motion;
            _actions = actions;
            _logRawFrames = logRawFrames;
        }

        public void Handle(byte[] packet, string remote)
        {
            if (!Fass2SimProtocol.TryGetCommand(packet, out var command, out var expectedLength))
            {
                Fass2SimLog.WriteLine($"[{Now()}] RX 非法报文 from {remote}, len={packet?.Length ?? 0}");
                return;
            }

            if (packet.Length != expectedLength)
            {
                Fass2SimLog.WriteLine(
                    $"[{Now()}] RX 长度不匹配 from {remote}: cmd={Fass2SimProtocol.CommandName(command)} actual={packet.Length} expected={expectedLength}");
                return;
            }

            if (packet[expectedLength - 1] != Fass2SimProtocol.End)
            {
                Fass2SimLog.WriteLine($"[{Now()}] RX 帧尾错误 from {remote}: cmd={Fass2SimProtocol.CommandName(command)}");
                return;
            }

            _vehicle.RecordCommandReceived();
            _vehicle.LastCommand = command;

            if (command == Fass2SimProtocol.CmdStateResponse)
            {
                _vehicle.RecordAckReceived();
                LogStateResponseAck(remote);
                return;
            }

            if (_logRawFrames)
            {
                Fass2SimLog.WriteLine($"[{Now()}] RX {Fass2SimProtocol.CommandName(command)} from {remote}: {Fass2SimProtocol.ToHex(packet)}");
            }
            else
            {
                Fass2SimLog.WriteLine($"[{Now()}] RX {Fass2SimProtocol.CommandName(command)} from {remote}");
            }

            switch (command)
            {
                case Fass2SimProtocol.CmdStart:
                    _vehicle.State = 1;
                    _motion.OnStartCommand();
                    Fass2SimLog.WriteLine($"[{Now()}]    -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
                    break;
                case Fass2SimProtocol.CmdStop:
                    _vehicle.State = 2;
                    _motion.OnStopCommand();
                    Fass2SimLog.WriteLine($"[{Now()}]    -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
                    break;
                case Fass2SimProtocol.CmdEmergencyStop:
                    _vehicle.State = 3;
                    _motion.OnStopCommand();
                    Fass2SimLog.WriteLine($"[{Now()}]    -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
                    break;
                case Fass2SimProtocol.CmdQuery:
                    Fass2SimLog.WriteLine($"[{Now()}]    -> TCP 查询（UDP 模式通常不发 0x00）");
                    break;
                case Fass2SimProtocol.CmdNodes:
                    if (Fass2SimProtocol.TryParseNodes(packet, out var taskId, out var nodes))
                    {
                        _motion.OnNodesCommand(taskId, nodes);
                    }
                    else
                    {
                        Fass2SimLog.WriteLine($"[{Now()}]    -> 0xB1 解析失败");
                    }
                    break;
                case Fass2SimProtocol.CmdAction:
                    if (Fass2SimProtocol.TryParseAction(packet, out var actionId, out var patch))
                    {
                        _actions.OnActionCommand(actionId, patch);
                    }
                    else
                    {
                        Fass2SimLog.WriteLine($"[{Now()}]    -> 0xA1 解析失败");
                    }
                    break;
                default:
                    Fass2SimLog.WriteLine($"[{Now()}]    -> 暂未处理");
                    break;
            }
        }

        private void LogStateResponseAck(string remote)
        {
            _stateResponseCount++;
            if (_stateResponseCount == 1)
            {
                Fass2SimLog.WriteLine($"[{Now()}] RX StateResponse(0x10) from {remote}，调度在线（后续 0x10 仅写文件日志）");
                return;
            }

            if (_stateResponseCount % 200 == 0)
            {
                Fass2SimLog.WriteFileOnly($"[{Now()}] 0x10 累计应答 {_stateResponseCount} 次");
            }
        }

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