COFTester/COFTester/Services/ModbusProcessDataReader.cs

using System;
using System.Threading;
using System.Threading.Tasks;
using COFTester.Models;
using NModbusAsync;

namespace COFTester.Services;

public sealed class ModbusProcessDataReader
{
    private const ushort CurrentReciprocatingStaticForceAddress = 1000;
    private const ushort CurrentReciprocatingStaticCoefficientAddress = 1002;
    private const ushort CurrentReciprocatingKineticForceAddress = 1010;
    private const ushort CurrentReciprocatingKineticCoefficientAddress = 1012;

    private static readonly PlcResultRegisterMap DefaultResultRegisterMap = new(
        SlaveAddress: 1,
        StartAddress: 460,
        RegisterCount: 12,
        StaticCoefficient1Offset: 0,
        KineticCoefficient1Offset: 2,
        StandardDeviation1Offset: 4,
        StaticCoefficient2Offset: 6,
        KineticCoefficient2Offset: 8,
        StandardDeviation2Offset: 10);
    private static readonly PlcTelemetryRegisterMap DefaultTelemetryRegisterMap = new(
        SlaveAddress: 1,
        ForceAddress: 1314,
        HorizontalSpeedAddress: 370,
        HorizontalDisplacementAddress: 380,
        LiftSpeedAddress: 310,
        LiftDisplacementAddress: 320,
        LiftPositionAddress: 12,
        HorizontalPositionAddress: 16);

    private readonly ModbusTcpConnectionService _connectionService;
    private readonly PlcResultRegisterMap _resultRegisterMap;
    private readonly PlcTelemetryRegisterMap _telemetryRegisterMap;
    private TestRecipe? _recipe;

    public ModbusProcessDataReader(
        ModbusTcpConnectionService connectionService,
        PlcResultRegisterMap? resultRegisterMap = null,
        PlcTelemetryRegisterMap? telemetryRegisterMap = null)
    {
        _connectionService = connectionService;
        _resultRegisterMap = resultRegisterMap ?? DefaultResultRegisterMap;
        _telemetryRegisterMap = telemetryRegisterMap ?? DefaultTelemetryRegisterMap;
    }

    public void Initialize(TestRecipe recipe)
    {
        _recipe = recipe;
    }

    public async Task<ProcessFrame> ReadFrameAsync(CancellationToken cancellationToken = default)
    {
        if (_recipe is null)
        {
            throw new InvalidOperationException("Modbus process data reader has not been initialized.");
        }

        var master = _connectionService.Master ?? throw new InvalidOperationException("Modbus master is not connected.");
        var resultRegisters = await master.ReadHoldingRegistersAsync(
            _resultRegisterMap.SlaveAddress,
            _resultRegisterMap.StartAddress,
            _resultRegisterMap.RegisterCount,
            cancellationToken);

        if (resultRegisters.Length < _resultRegisterMap.RegisterCount)
        {
            throw new InvalidOperationException("Incomplete PLC result register block received.");
        }

        var positionStartAddress = Min(_telemetryRegisterMap.LiftPositionAddress, _telemetryRegisterMap.HorizontalPositionAddress);
        var positionRegisterCount = GetRegisterCount(
            positionStartAddress,
            _telemetryRegisterMap.LiftPositionAddress,
            _telemetryRegisterMap.HorizontalPositionAddress);
        var positionRegisters = await ReadHoldingRegisterBlockAsync(
            master,
            _telemetryRegisterMap.SlaveAddress,
            positionStartAddress,
            positionRegisterCount,
            cancellationToken);

        var motionStartAddress = Min(
            _telemetryRegisterMap.LiftSpeedAddress,
            _telemetryRegisterMap.LiftDisplacementAddress,
            _telemetryRegisterMap.HorizontalSpeedAddress,
            _telemetryRegisterMap.HorizontalDisplacementAddress);
        var motionRegisterCount = GetRegisterCount(
            motionStartAddress,
            _telemetryRegisterMap.LiftSpeedAddress,
            _telemetryRegisterMap.LiftDisplacementAddress,
            _telemetryRegisterMap.HorizontalSpeedAddress,
            _telemetryRegisterMap.HorizontalDisplacementAddress);
        var motionRegisters = await ReadHoldingRegisterBlockAsync(
            master,
            _telemetryRegisterMap.SlaveAddress,
            motionStartAddress,
            motionRegisterCount,
            cancellationToken);

        var forceRegisters = await ReadHoldingRegisterBlockAsync(
            master,
            _telemetryRegisterMap.SlaveAddress,
            _telemetryRegisterMap.ForceAddress,
            PlcRegisterEncoding.FloatRegisterCount,
            cancellationToken);

        var force = ReadFloatAt(forceRegisters, _telemetryRegisterMap.ForceAddress, _telemetryRegisterMap.ForceAddress);
        var horizontalSpeed = ReadFloatAt(motionRegisters, motionStartAddress, _telemetryRegisterMap.HorizontalSpeedAddress);
        var horizontalDisplacement = ReadFloatAt(motionRegisters, motionStartAddress, _telemetryRegisterMap.HorizontalDisplacementAddress);
        var liftSpeed = ReadFloatAt(motionRegisters, motionStartAddress, _telemetryRegisterMap.LiftSpeedAddress);
        var liftDisplacement = ReadFloatAt(motionRegisters, motionStartAddress, _telemetryRegisterMap.LiftDisplacementAddress);
        var liftPosition = ReadFloatAt(positionRegisters, positionStartAddress, _telemetryRegisterMap.LiftPositionAddress);
        var horizontalPosition = ReadFloatAt(positionRegisters, positionStartAddress, _telemetryRegisterMap.HorizontalPositionAddress);
        var completed = false;

        return new ProcessFrame(
            horizontalPosition,
            Math.Max(force, 0.001),
            horizontalSpeed,
            completed,
            PlcRegisterEncoding.ReadFloat(resultRegisters, _resultRegisterMap.StaticCoefficient1Offset),
            PlcRegisterEncoding.ReadFloat(resultRegisters, _resultRegisterMap.KineticCoefficient1Offset),
            PlcRegisterEncoding.ReadFloat(resultRegisters, _resultRegisterMap.StandardDeviation1Offset),
            PlcRegisterEncoding.ReadFloat(resultRegisters, _resultRegisterMap.StaticCoefficient2Offset),
            PlcRegisterEncoding.ReadFloat(resultRegisters, _resultRegisterMap.KineticCoefficient2Offset),
            PlcRegisterEncoding.ReadFloat(resultRegisters, _resultRegisterMap.StandardDeviation2Offset),
            horizontalSpeed,
            horizontalDisplacement,
            liftSpeed,
            liftDisplacement,
            liftPosition,
            horizontalPosition);
    }

    public async Task<ReciprocatingFrictionRecord> ReadCurrentReciprocatingRecordAsync(
        int recordIndex,
        CancellationToken cancellationToken = default)
    {
        var master = _connectionService.Master ?? throw new InvalidOperationException("Modbus master is not connected.");
        var startAddress = Min(
            CurrentReciprocatingStaticForceAddress,
            CurrentReciprocatingStaticCoefficientAddress,
            CurrentReciprocatingKineticForceAddress,
            CurrentReciprocatingKineticCoefficientAddress);
        var registerCount = GetRegisterCount(
            startAddress,
            CurrentReciprocatingStaticForceAddress,
            CurrentReciprocatingStaticCoefficientAddress,
            CurrentReciprocatingKineticForceAddress,
            CurrentReciprocatingKineticCoefficientAddress);
        var registers = await ReadHoldingRegisterBlockAsync(
            master,
            _resultRegisterMap.SlaveAddress,
            startAddress,
            registerCount,
            cancellationToken);

        return new ReciprocatingFrictionRecord
        {
            Index = Math.Max(1, recordIndex),
            StaticCoefficient = ReadFloatAt(registers, startAddress, CurrentReciprocatingStaticCoefficientAddress),
            KineticCoefficient = ReadFloatAt(registers, startAddress, CurrentReciprocatingKineticCoefficientAddress),
            StaticForceN = ReadFloatAt(registers, startAddress, CurrentReciprocatingStaticForceAddress),
            KineticForceN = ReadFloatAt(registers, startAddress, CurrentReciprocatingKineticForceAddress)
        };
    }

    private static async Task<ushort[]> ReadHoldingRegisterBlockAsync(
        IModbusMaster master,
        byte slaveAddress,
        ushort startAddress,
        ushort registerCount,
        CancellationToken cancellationToken)
    {
        var registers = await master.ReadHoldingRegistersAsync(slaveAddress, startAddress, registerCount, cancellationToken);
        if (registers.Length < registerCount)
        {
            throw new InvalidOperationException($"PLC 寄存器 D{startAddress} 起始数据块返回不完整。");
        }

        return registers;
    }

    private static double ReadFloatAt(ushort[] registers, ushort startAddress, ushort address)
    {
        var offset = address - startAddress;
        return PlcRegisterEncoding.ReadFloat(registers, offset, $"D{address}");
    }

    private static ushort Min(params ushort[] values)
    {
        return values.Min();
    }

    private static ushort GetRegisterCount(ushort startAddress, params ushort[] addresses)
    {
        var lastAddress = addresses.Max();
        return checked((ushort)(lastAddress - startAddress + PlcRegisterEncoding.FloatRegisterCount));
    }
}

public sealed record PlcResultRegisterMap(
    byte SlaveAddress,
    ushort StartAddress,
    ushort RegisterCount,
    int StaticCoefficient1Offset,
    int KineticCoefficient1Offset,
    int StandardDeviation1Offset,
    int StaticCoefficient2Offset,
    int KineticCoefficient2Offset,
    int StandardDeviation2Offset);

public sealed record PlcTelemetryRegisterMap(
    byte SlaveAddress,
    ushort ForceAddress,
    ushort HorizontalSpeedAddress,
    ushort HorizontalDisplacementAddress,
    ushort LiftSpeedAddress,
    ushort LiftDisplacementAddress,
    ushort LiftPositionAddress,
    ushort HorizontalPositionAddress);