TeamAAS-VP-HeShuo/TeamAAS-VM/Core/ModbusTcpMasterCommunicate.cs

using Modbus.Device;
using Prism.Events;
using System;
using System.Collections.Generic;
using System.IO;
using System.Net.Sockets;
using System.Threading;
using System.Threading.Tasks;
using TeamAAS_VP.Events;
using TeamAAS_VP.Models;

namespace TeamAAS_VP.Core
{
    /// <summary>
    /// Modbus TCP 主站：轮询 MDC V2 电批，锁付过程中累积实时曲线，完成后推送结果
    /// 地址默认值参考 MDC V2 Appendix A（Input Register 功能码 0x04）
    /// </summary>
    public class ModbusTcpMasterCommunicate : IDisposable
    {
        // MDC V2 事件数据区起始地址（3201~3213 一次读完，保证结果一致性）
        private const ushort MdcEventDataStartAddress = 3201;
        private const int MdcEventDataRegisterCount = 13;

        private readonly ModbusTcpMasterConfig _config;
        private readonly object _masterLock = new object();
        private readonly IEventAggregator _eventAggregator;

        private TcpClient _tcpClient;
        private IModbusMaster _master;
        private CancellationTokenSource _pollCts;

        // 轮询状态
        private bool _lastFinishComplete;
        private bool _lastLockInProgress;
        private int _lastWaveCount;
        /// <summary>本次锁付周期内是否检测到 Motor RUN，防止启动时误触发完成事件</summary>
        private bool _finishArmed;

        // 实时累积波形（MDC 无波形缓冲区，靠轮询 3300/3301 累积）
        private readonly List<WaveData> _accumulatedWave = new List<WaveData>();
        private DateTime _lockStartTime = DateTime.Now;
        private double _accumulatedTurns;

        public ModbusTcpMasterConfig Config { get { return _config; } }
        public bool IsEnabled { get { return _config.IsEnabled; } }
        public bool IsConnected { get; private set; }

        public ModbusTcpMasterCommunicate(ModbusTcpMasterConfig config, IEventAggregator eventAggregator)
        {
            _config = config;
            _eventAggregator = eventAggregator;
        }

        public void Start()
        {
            if (!_config.IsEnabled)
            {
                return;
            }

            Connect();
            SyncInitialPollState();
            _pollCts = new CancellationTokenSource();
            Task.Run(() => PollLoop(_pollCts.Token));
        }

        public void Stop()
        {
            if (_pollCts != null)
            {
                _pollCts.Cancel();
                _pollCts.Dispose();
                _pollCts = null;
            }
            Disconnect();
        }

        public void Dispose()
        {
            Stop();
        }

        #region 连接

        private void Connect()
        {
            Disconnect();
            _tcpClient = new TcpClient();
            _tcpClient.Connect(_config.IP, _config.Port);
            _master = ModbusIpMaster.CreateIp(_tcpClient);
            _master.Transport.ReadTimeout = 3000;
            _master.Transport.WriteTimeout = 3000;
            IsConnected = true;
        }

        private void Disconnect()
        {
            IsConnected = false;
            if (_master != null)
            {
                _master.Dispose();
                _master = null;
            }
            if (_tcpClient != null)
            {
                _tcpClient.Close();
                _tcpClient = null;
            }
        }

        private bool TryReconnect()
        {
            try
            {
                Connect();
                SyncInitialPollState();
                return true;
            }
            catch (Exception ex)
            {
                IsConnected = false;
                LogHelper.WriteLogError("Modbus TCP 主站重连失败", ex);
                return false;
            }
        }

        /// <summary>
        /// 连接/启动时同步电批当前状态，避免 3212 仍为「完成」时误触发锁付完成
        /// </summary>
        private void SyncInitialPollState()
        {
            _lastFinishComplete = ReadFinishComplete();
            _lastLockInProgress = ReadLockInProgress();
            _finishArmed = false;
            _lastWaveCount = 0;
            ClearAccumulatedWave();
        }

        #endregion

        #region 轮询

        private void PollLoop(CancellationToken token)
        {
            while (!token.IsCancellationRequested)
            {
                try
                {
                    PollOnce();
                }
                catch (Exception ex)
                {
                    LogHelper.WriteLogError("Modbus TCP 主站轮询出错", ex);
                    Disconnect();
                    Thread.Sleep(2000);
                    TryReconnect();
                }

                Thread.Sleep(Math.Max(10, _config.PollIntervalMs));
            }
        }

        private void PollOnce()
        {
            if (!IsConnected)
            {
                TryReconnect();
                return;
            }

            bool isLocking = ReadLockInProgress();
            bool finishComplete = ReadFinishComplete();

            HandleLockStartEdge(isLocking);

            if (isLocking)
            {
                AppendRealtimeWavePoint();
            }

            bool needCurve = isLocking || finishComplete;
            if (!needCurve)
            {
                ResetPollStateIfIdle();
                return;
            }

            List<WaveData> waveDatas = GetCurrentWaveDatas();
            PublishCurveUpdateIfChanged(waveDatas, finishComplete);

            // 必须：先检测到 Motor RUN（_finishArmed），再检测 3212 上升沿，才推送完成
            if (finishComplete && !_lastFinishComplete && _finishArmed)
            {
                PublishLockFinish(waveDatas);
                ClearAccumulatedWave();
                _finishArmed = false;
            }

            _lastFinishComplete = finishComplete;
            _lastLockInProgress = isLocking;
        }

        /// <summary>检测到锁付开始时清空累积曲线并允许后续完成判定</summary>
        private void HandleLockStartEdge(bool isLocking)
        {
            if (!isLocking || _lastLockInProgress)
            {
                return;
            }

            ClearAccumulatedWave();
            _lockStartTime = DateTime.Now;
            _finishArmed = true;
            _lastWaveCount = 0;
        }

        private void ResetPollStateIfIdle()
        {
            _lastFinishComplete = false;
            _lastLockInProgress = false;
            _finishArmed = false;
            _lastWaveCount = 0;
            ClearAccumulatedWave();
        }

        private void ClearAccumulatedWave()
        {
            _accumulatedWave.Clear();
            _accumulatedTurns = 0;
        }

        private List<WaveData> GetCurrentWaveDatas()
        {
            if (_config.WaveDataMode == WaveDataSourceMode.RegisterBlock)
            {
                return ReadWaveDatasFromRegisters();
            }
            return new List<WaveData>(_accumulatedWave);
        }

        private void PublishCurveUpdateIfChanged(List<WaveData> waveDatas, bool finishComplete)
        {
            // 完成态已处理过则不再刷曲线，避免 3212 保持为 1 时反复推送空曲线
            if (finishComplete && _lastFinishComplete)
            {
                return;
            }

            int waveCount = waveDatas.Count;
            if (waveCount == _lastWaveCount && !finishComplete)
            {
                return;
            }

            _lastWaveCount = waveCount;
            _eventAggregator.GetEvent<LockCurveUpdateNotification>().Publish(waveDatas);
        }

        private void PublishLockFinish(List<WaveData> waveDatas)
        {
            LockResult result = BuildLockResult(waveDatas);
            result.LockCurveImagePath = BuildLockCurveImagePath(result.Timestamp);
            result.CurveCsvPath = BuildLockCurveCsvPath(result.Timestamp);
            _eventAggregator.GetEvent<LockFinishNotification>().Publish(result);
        }

        #endregion

        #region MDC 实时波形累积

        /// <summary>
        /// 锁付进行中：读取 3300 扭矩、3301 转速，累积为曲线点
        /// </summary>
        private void AppendRealtimeWavePoint()
        {
            float torque = ReadScaledTorque(_config.RealtimeTorqueAddress);
            float speed = ReadRegister(_config.RealtimeSpeedAddress);
            double intervalSec = _config.PollIntervalMs / 1000.0;
            _accumulatedTurns += (speed / 60.0) * intervalSec;

            _accumulatedWave.Add(new WaveData
            {
                Torque = torque,
                Speed = speed,
                Turns = _accumulatedTurns,
                Time = (DateTime.Now - _lockStartTime).TotalSeconds
            });
        }

        #endregion

        #region 寄存器读取

        private bool ReadLockInProgress()
        {
            if (_config.LockInProgressAddress == 0)
            {
                return false;
            }

            // MDC：3307 Motor RUN = 1 表示正在锁付
            return ReadRegister(_config.LockInProgressAddress) == _config.LockInProgressActiveValue;
        }

        /// <summary>MDC 3212：1=完成，2=NG，均视为锁付结束</summary>
        private bool ReadFinishComplete()
        {
            ushort status = ReadRegister(_config.FinishFlagAddress);
            return status == _config.FinishFlagCompleteValue
                || status == _config.FinishFlagNgValue;
        }

        /// <summary>
        /// 组装锁付结果。优先批量读取 MDC 3201~3213 事件区，保证各字段同一时刻快照。
        /// </summary>
        private LockResult BuildLockResult(List<WaveData> waveDatas)
        {
            if (TryReadMdcEventBlock(out ushort durationMs, out ushort presetNo, out ushort torqueRaw,
                out ushort status, out ushort snugAngle))
            {
                return CreateLockResult(waveDatas, durationMs, presetNo, torqueRaw, status, snugAngle);
            }

            // 非标准地址布局：逐寄存器读取
            ushort fallbackStatus = ReadRegister(_config.FinishFlagAddress);
            return CreateLockResult(
                waveDatas,
                ReadRegister(_config.LockDurationAddress),
                ReadRegister(_config.ProgramNumberAddress),
                ReadRegister(_config.LockTorqueAddress),
                fallbackStatus,
                ReadRegister(_config.LockTurnsAddress));
        }

        private LockResult CreateLockResult(
            List<WaveData> waveDatas,
            ushort durationRaw,
            ushort presetNo,
            ushort torqueRaw,
            ushort status,
            ushort turnsRaw)
        {
            // MDC 3212：1=OK，2=NG（以事件 Status 为准，比 3305 实时位更可靠）
            bool lockPassed = status == _config.FinishFlagCompleteValue;

            float durationSec = _config.LockDurationIsMilliseconds
                ? durationRaw / 1000f
                : durationRaw;

            float lockTurns = _config.TurnsScaleFactor > 0
                ? turnsRaw / _config.TurnsScaleFactor
                : turnsRaw;

            float lockTorque = _config.TorqueScaleFactor > 0
                ? torqueRaw / _config.TorqueScaleFactor
                : torqueRaw;

            return new LockResult
            {
                Timestamp = DateTime.Now,
                LockTurns = lockTurns,
                LockTorque = lockTorque,
                LockDuration = durationSec,
                LockPassed = lockPassed,
                ScrewdriverProgramNumber = presetNo,
                WaveDatas = waveDatas
            };
        }

        /// <summary>
        /// 一次读取 MDC 事件数据块 3201~3213（说明书 Appendix A，功能码 0x04）
        /// </summary>
        private bool TryReadMdcEventBlock(
            out ushort durationMs,
            out ushort presetNo,
            out ushort torqueRaw,
            out ushort status,
            out ushort snugAngle)
        {
            durationMs = presetNo = torqueRaw = status = snugAngle = 0;

            if (!_config.UseInputRegisters || !IsStandardMdcEventLayout())
            {
                return false;
            }

            ushort[] block = ReadRegisters(MdcEventDataStartAddress, MdcEventDataRegisterCount);
            if (block == null || block.Length < MdcEventDataRegisterCount)
            {
                return false;
            }

            // 相对 3201 的偏移：3201=0, 3202=1, 3204=3, 3212=11, 3213=12
            durationMs = block[0];
            presetNo = block[1];
            torqueRaw = block[3];
            status = block[11];
            snugAngle = block[12];
            return true;
        }

        /// <summary>判断配置是否为 MDC V2 标准事件区地址布局</summary>
        private bool IsStandardMdcEventLayout()
        {
            return _config.LockDurationAddress == 3201
                && _config.ProgramNumberAddress == 3202
                && _config.LockTorqueAddress == 3204
                && _config.FinishFlagAddress == 3212
                && _config.LockTurnsAddress == 3213;
        }

        /// <summary>RegisterBlock 模式：从连续寄存器块批量读波形</summary>
        private List<WaveData> ReadWaveDatasFromRegisters()
        {
            int pointCount = ReadRegister(_config.WaveDataCountAddress);
            if (pointCount <= 0)
            {
                return new List<WaveData>();
            }

            pointCount = Math.Min(pointCount, _config.MaxWavePoints);
            int totalRegisters = pointCount * _config.WavePointRegisterCount;
            ushort[] registers = ReadRegisters(_config.WaveDataStartAddress, totalRegisters);

            List<WaveData> waveDatas = new List<WaveData>(pointCount);
            for (int i = 0; i < pointCount; i++)
            {
                waveDatas.Add(ParseWavePointFromBlock(registers, i * _config.WavePointRegisterCount));
            }
            return waveDatas;
        }

        private WaveData ParseWavePointFromBlock(ushort[] registers, int baseOffset)
        {
            return new WaveData
            {
                Turns = ReadFloatFromRegisters(registers, baseOffset + 0),
                Torque = ReadFloatFromRegisters(registers, baseOffset + 2),
                LockAngle = ReadFloatFromRegisters(registers, baseOffset + 4),
                Speed = ReadFloatFromRegisters(registers, baseOffset + 6),
                Slopes = ReadFloatFromRegisters(registers, baseOffset + 8),
                Time = ReadFloatFromRegisters(registers, baseOffset + 10)
            };
        }

        private float ReadScaledTorque(ushort address)
        {
            return ScaleTorque(ReadRegister(address));
        }

        private float ScaleTorque(ushort raw)
        {
            if (_config.TorqueScaleFactor <= 0)
            {
                return raw;
            }
            return raw / _config.TorqueScaleFactor;
        }

        private ushort ReadRegister(ushort address)
        {
            lock (_masterLock)
            {
                if (_config.UseInputRegisters)
                {
                    return _master.ReadInputRegisters(_config.SlaveID, address, 1)[0];
                }
                return _master.ReadHoldingRegisters(_config.SlaveID, address, 1)[0];
            }
        }

        private ushort[] ReadRegisters(ushort startAddress, int count)
        {
            lock (_masterLock)
            {
                if (_config.UseInputRegisters)
                {
                    return _master.ReadInputRegisters(_config.SlaveID, startAddress, (ushort)count);
                }
                return _master.ReadHoldingRegisters(_config.SlaveID, startAddress, (ushort)count);
            }
        }

        private static float ReadFloatFromRegisters(ushort[] registers, int index)
        {
            if (registers == null || index + 1 >= registers.Length)
            {
                return 0f;
            }
            byte[] bytes = new byte[4];
            BitConverter.GetBytes(registers[index + 1]).CopyTo(bytes, 0);
            BitConverter.GetBytes(registers[index + 0]).CopyTo(bytes, 2);
            return BitConverter.ToSingle(bytes, 0);
        }

        private string BuildLockCurveImagePath(DateTime timestamp)
        {
            string folder = _config.LockCurveImageFolder;
            if (!Directory.Exists(folder))
            {
                Directory.CreateDirectory(folder);
            }
            return Path.Combine(folder, $"LockCurve_{timestamp:yyyyMMdd_HHmmss_fff}.png");
        }

        private string BuildLockCurveCsvPath(DateTime timestamp)
        {
            string folder = Path.Combine(_config.LockCurveImageFolder, "Csv");
            if (!Directory.Exists(folder))
            {
                Directory.CreateDirectory(folder);
            }
            return Path.Combine(folder, $"LockCurve_{timestamp:yyyyMMdd_HHmmss_fff}.csv");
        }

        #endregion
    }
}