HC_APTBS/Infrastructure/Pcan/PcanAdapter.cs

using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using HC_APTBS.Infrastructure.Logging;
using HC_APTBS.Models;
using HC_APTBS.Services;
using Peak.Can.Basic;
using TPCANHandle = System.UInt16;

namespace HC_APTBS.Infrastructure.Pcan
{
    /// <summary>
    /// Wraps the PCAN-Basic native API behind <see cref="ICanService"/>.
    /// The raw P/Invoke declarations live in <see cref="PcanBasic"/> (vendor file,
    /// unchanged). This class handles lifecycle, threading, OEM legitimation,
    /// message dispatch, and parameter decoding.
    /// </summary>
    public sealed class PcanAdapter : ICanService, IDisposable
    {
        // ── Constants ───────────────────────────────────────────────────────────

        /// <summary>PEAK-System distributor code embedded into the OEM legitimation token.</summary>
        private const ulong OemDistributorCode = 20120378UL;

        /// <summary>PEAK-System OEM-ID for the PCAN-USB device.</summary>
        private const ulong OemId = 21200UL;

        private const string LogId = "PcanAdapter";

        // ── State ────────────────────────────────────────────────────────────────

        private readonly TPCANHandle _channel;
        private TPCANBaudrate _baudrate;
        private readonly IAppLogger _log;

        /// <summary>
        /// Live parameter map: CAN message ID → list of parameters decoded from that frame.
        /// All reads/writes on this dictionary happen on the CAN read thread, except for
        /// <see cref="AddParameters"/> / <see cref="RemoveParameters"/> which are guarded
        /// by <see cref="_mapLock"/>.
        /// </summary>
        private Dictionary<uint, List<CanBusParameter>> _parameterMap = new();
        private readonly object _mapLock = new();

        private Thread? _readThread;
        private AutoResetEvent? _receiveEvent;
        private volatile bool _stopRead = true;

        // ── Liveness tracking ────────────────────────────────────────────────────

        private const int LivenessTimeoutMs = 500;
        private HashSet<uint> _benchMessageIds = new();
        private HashSet<uint> _pumpMessageIds = new();
        private DateTime _lastBenchFrameUtc = DateTime.MinValue;
        private DateTime _lastPumpFrameUtc = DateTime.MinValue;
        private bool _benchAlive;
        private bool _pumpAlive;

        // ── ICanService ──────────────────────────────────────────────────────────

        /// <inheritdoc/>
        public event Action<string, bool>? StatusChanged;

        /// <inheritdoc/>
        public event Action<bool>? BenchLivenessChanged;

        /// <inheritdoc/>
        public event Action<bool>? PumpLivenessChanged;

        /// <inheritdoc/>
        public TPCANStatus CurrentStatus { get; private set; } = TPCANStatus.PCAN_ERROR_OK;

        /// <inheritdoc/>
        public bool IsConnected => !_stopRead;

        // ── Construction ─────────────────────────────────────────────────────────

        /// <summary>
        /// Creates a new adapter for the given PCAN channel and baudrate.
        /// Call <see cref="Connect"/> to open the hardware.
        /// </summary>
        /// <param name="channel">PCAN channel handle, e.g. <c>PCANBasic.PCAN_USBBUS1</c>.</param>
        /// <param name="baudrate">CAN baudrate, e.g. <c>TPCANBaudrate.PCAN_BAUD_500K</c>.</param>
        /// <param name="logger">Application logger.</param>
        public PcanAdapter(TPCANHandle channel, TPCANBaudrate baudrate, IAppLogger logger)
        {
            _channel = channel;
            _baudrate = baudrate;
            _log = logger;
        }

        // ── ICanService: lifecycle ────────────────────────────────────────────────

        /// <inheritdoc/>
        public bool Connect()
        {
            try
            {
                // If the channel is already open, reuse it; otherwise initialize.
                CurrentStatus = PCANBasic.GetStatus(_channel);

                if (CurrentStatus == TPCANStatus.PCAN_ERROR_INITIALIZE ||
                    CurrentStatus == TPCANStatus.PCAN_ERROR_INITIALIZE2)
                {
                    CurrentStatus = PCANBasic.Initialize(_channel, _baudrate, (TPCANType)0, 0, 0);
                    EmitStatusChanged(CurrentStatus);
                }

                if (CurrentStatus == TPCANStatus.PCAN_ERROR_NETINUSE)
                {
                    _log.Error(LogId, "CAN channel is already in use by another application.");
                    EmitStatusChanged(CurrentStatus);
                    return false;
                }

                if (CurrentStatus != TPCANStatus.PCAN_ERROR_OK)
                {
                    LogPcanError("Connect: initialization failed");
                    EmitStatusChanged(CurrentStatus);
                    return false;
                }

                // OEM legitimation: token = (DistributorCode << 32) | OemId
                // This authenticates our application with the PCAN hardware.
                ulong token = (OemDistributorCode << 32) | OemId;
                CurrentStatus = PCANBasic.SetValue(
                    _channel, TPCANParameter.PCAN_CHANNEL_LEGITIMATION, ref token, 8);

                if (CurrentStatus != TPCANStatus.PCAN_ERROR_OK)
                {
                    _log.Error(LogId, "OEM legitimation failed — adapter has no OEM token.");
                    PCANBasic.Uninitialize(_channel);
                    EmitStatusChanged(CurrentStatus);
                    return false;
                }

                StartReadThread();
                return true;
            }
            catch (Exception ex)
            {
                _log.Error(LogId, $"Connect exception: {ex}");
                return false;
            }
        }

        /// <inheritdoc/>
        public void Disconnect()
        {
            _stopRead = true;
            PCANBasic.Uninitialize(_channel);
        }

        /// <inheritdoc/>
        public void SwitchBaudrate(TPCANBaudrate newBaudrate, uint baudrateMessageId)
        {
            // Send the baudrate-change command to the bench firmware before switching.
            SendMessageById(baudrateMessageId);

            _stopRead = true;
            Thread.Sleep(250);

            _baudrate = newBaudrate;
            PCANBasic.Uninitialize(_channel);
            PCANBasic.Reset(_channel);
            Connect();
        }

        // ── ICanService: parameter map ────────────────────────────────────────────

        /// <inheritdoc/>
        public void SetParameters(Dictionary<uint, List<CanBusParameter>> parameters)
        {
            lock (_mapLock)
            {
                _parameterMap = parameters;
            }
        }

        /// <inheritdoc/>
        public void AddParameters(Dictionary<uint, List<CanBusParameter>> parameters)
        {
            lock (_mapLock)
            {
                foreach (var kv in parameters)
                {
                    if (!_parameterMap.ContainsKey(kv.Key))
                        _parameterMap.Add(kv.Key, kv.Value);
                }
            }
        }

        /// <inheritdoc/>
        public void RemoveParameters(Dictionary<uint, List<CanBusParameter>> parameters)
        {
            lock (_mapLock)
            {
                foreach (var key in parameters.Keys)
                    _parameterMap.Remove(key);
            }
        }

        /// <inheritdoc/>
        public void RegisterBenchMessageIds(IReadOnlyCollection<uint> ids)
        {
            _benchMessageIds = new HashSet<uint>(ids);
        }

        /// <inheritdoc/>
        public void RegisterPumpMessageIds(IReadOnlyCollection<uint> ids)
        {
            _pumpMessageIds = new HashSet<uint>(ids);
        }

        // ── ICanService: transmit ─────────────────────────────────────────────────

        /// <inheritdoc/>
        public void SendMessageById(uint messageId)
        {
            Dictionary<uint, List<CanBusParameter>> snapshot;
            lock (_mapLock) { snapshot = _parameterMap; }

            if (!snapshot.TryGetValue(messageId, out var parameters) || parameters.Count == 0)
                return;

            var msg = new TPCANMsg
            {
                ID = messageId,
                LEN = 8,
                MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
                DATA = new byte[8]
            };

            // Write only transmit (non-receive) parameters into their assigned byte positions.
            foreach (var param in parameters)
            {
                if (param.IsReceive) continue;
                uint raw = (uint)param.GetTransmitValue();
                msg.DATA[param.ByteH] = (byte)((raw & 0xFF00) >> 8);
                msg.DATA[param.ByteL] = (byte)(raw & 0x00FF);
            }

            CurrentStatus = PCANBasic.Write(_channel, ref msg);
            if (CurrentStatus != TPCANStatus.PCAN_ERROR_OK)
                _log.Warning(LogId, $"SendMessageById({messageId:X}): {CurrentStatus}");
        }

        /// <inheritdoc/>
        public void SendRawMessage(uint messageId, byte[] data)
        {
            if (data.Length != 8)
                throw new ArgumentException("CAN standard frame payload must be exactly 8 bytes.", nameof(data));

            var msg = new TPCANMsg
            {
                ID = messageId,
                LEN = 8,
                MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
                DATA = data
            };

            CurrentStatus = PCANBasic.Write(_channel, ref msg);
            if (CurrentStatus != TPCANStatus.PCAN_ERROR_OK)
                _log.Warning(LogId, $"SendRawMessage({messageId:X}): {CurrentStatus}");
        }

        // ── Read thread ───────────────────────────────────────────────────────────

        private void StartReadThread()
        {
            _stopRead = false;
            _receiveEvent = new AutoResetEvent(false);

            _readThread = new Thread(ReadThreadEntry) { IsBackground = true, Name = "CAN-Read" };
            _readThread.Start();
        }

        private void ReadThreadEntry()
        {
            // Bind the AutoResetEvent handle to the PCAN receive event so the driver
            // signals us whenever a new frame arrives in the hardware FIFO.
            uint eventHandle = Convert.ToUInt32(_receiveEvent!.SafeWaitHandle.DangerousGetHandle().ToInt32());
            var result = PCANBasic.SetValue(
                _channel, TPCANParameter.PCAN_RECEIVE_EVENT, ref eventHandle, sizeof(uint));

            if (result != TPCANStatus.PCAN_ERROR_OK)
            {
                _log.Error(LogId, $"Failed to bind receive event: {result}");
                return;
            }

            DrainMessageQueue();
        }

        /// <summary>
        /// Continuously drains the hardware receive FIFO until stopped.
        /// Runs on the dedicated CAN read background thread.
        /// </summary>
        private void DrainMessageQueue()
        {
            while (!_stopRead)
            {
                var status = ReadOnce();

                if (status == TPCANStatus.PCAN_ERROR_ILLOPERATION)
                {
                    _log.Error(LogId, "Read thread: illegal operation — stopping.");
                    _stopRead = true;
                    break;
                }

                if (status != TPCANStatus.PCAN_ERROR_QRCVEMPTY && status != TPCANStatus.PCAN_ERROR_OK)
                {
                    _log.Warning(LogId, $"DrainMessageQueue: {status}");
                    EmitStatusChanged(status);
                }

                // Check liveness timeouts.
                CheckLivenessTimeout();

                // Configurable polling interval to avoid pegging the CPU.
                // Typical value: 2–50 ms depending on operational phase.
                Thread.Sleep(2);
            }
        }

        /// <summary>
        /// Reads and decodes all frames currently available in the hardware FIFO.
        /// Returns the last <see cref="TPCANStatus"/> encountered.
        /// </summary>
        private TPCANStatus ReadOnce()
        {
            TPCANStatus status;
            do
            {
                status = PCANBasic.Read(_channel, out TPCANMsg frame);
                if (status != TPCANStatus.PCAN_ERROR_QRCVEMPTY)
                    DecodeFrame(frame);

                if (status != TPCANStatus.PCAN_ERROR_QRCVEMPTY && status != TPCANStatus.PCAN_ERROR_OK)
                {
                    _log.Warning(LogId, $"ReadOnce: {status}");
                    EmitStatusChanged(status);
                }
            }
            while (status != TPCANStatus.PCAN_ERROR_QRCVEMPTY &&
                   status != TPCANStatus.PCAN_ERROR_BUSHEAVY);

            return status;
        }

        // ── Frame decoding ────────────────────────────────────────────────────────

        /// <summary>
        /// Decodes a received CAN frame and updates the associated <see cref="CanBusParameter"/>
        /// values, applying the calibration transfer function and exponential smoothing filter.
        /// </summary>
        private void DecodeFrame(TPCANMsg frame)
        {
            // Message ID 0 carries internal bus-status info — ignore.
            if (frame.ID == 0) return;

            Dictionary<uint, List<CanBusParameter>> snapshot;
            lock (_mapLock) { snapshot = _parameterMap; }

            if (!snapshot.TryGetValue(frame.ID, out var parameters)) return;

            // Track liveness for bench and pump frame groups.
            var now = DateTime.UtcNow;
            if (_benchMessageIds.Contains(frame.ID))
            {
                _lastBenchFrameUtc = now;
                if (!_benchAlive)
                {
                    _benchAlive = true;
                    BenchLivenessChanged?.Invoke(true);
                }
            }
            if (_pumpMessageIds.Contains(frame.ID))
            {
                _lastPumpFrameUtc = now;
                if (!_pumpAlive)
                {
                    _pumpAlive = true;
                    PumpLivenessChanged?.Invoke(true);
                }
            }

            byte[] data = frame.DATA;

            foreach (var param in parameters)
            {
                // Only decode receive parameters — skip send-only params to avoid
                // overwriting outgoing values with received frame bytes.
                if (!param.IsReceive) continue;

                double previousValue = param.Value;

                if (param.Name == BenchParameterNames.Temp || param.Name == PumpParameterNames.Temp)
                {
                    // Temperature uses a special packed BCD / signed format depending on sensor type.
                    param.Value = DecodeTempValue(data, param);
                }
                else if (param.Name == PumpParameterNames.Rpm)
                {
                    // RPM is packed in the upper 12 bits across two bytes (two encoding variants).
                    param.Value = DecodeRpmValue(data, param);
                    param.Value = param.GetTransformResult();
                }
                else
                {
                    // Generic 1-byte, 2-byte, or 3-byte big-endian integer.
                    int byteSpan = Math.Abs(param.ByteH - param.ByteL);
                    if (byteSpan == 0)
                    {
                        param.Value = data[param.ByteL];
                    }
                    else if (byteSpan == 1)
                    {
                        param.Value = (data[param.ByteH] << 8) | data[param.ByteL];
                    }
                    else
                    {
                        // 3-byte little-endian variant used for encoder/pulse counters.
                        param.Value = (data[param.ByteL + 2] << 16)
                                    | (data[param.ByteL + 1] << 8)
                                    | data[param.ByteL];
                    }

                    param.Value = param.GetTransformResult();
                    if (double.IsInfinity(param.Value)) param.Value = 0;
                }

                // Spike rejection for BenchRPM: the bench controller occasionally sends
                // a spurious value of 1 RPM — discard it and retain the previous value.
                if (param.Name == BenchParameterNames.BenchRpm && param.Value == 1)
                {
                    param.Value = previousValue;
                    return;
                }

                // Spike rejection for QDelivery: discard values that are more than
                // 100x the previous reading (caused by relay switching noise).
                if (param.Name == BenchParameterNames.QDelivery)
                {
                    if (previousValue > 0.1 && param.Value > previousValue * 100)
                    {
                        _log.Warning(LogId,
                            $"QDelivery spike suppressed: prev={previousValue:F3}, new={param.Value:F3}");
                        param.Value = previousValue;
                    }
                }

                // Apply single-pole IIR low-pass filter.
                // result = prev + alpha * (new - prev)
                param.Value = PassFilterUpdate(previousValue, param.Value, param.Alpha);
                param.NeedsUpdate = true;
            }
        }

        /// <summary>
        /// Decodes a temperature value from the CAN frame.
        /// The bench uses three different encoding schemes identified by <c>param.Type</c>:
        /// <list type="bullet">
        ///   <item>0 — 4-nibble BCD with sign extension: [−256…+256] + fractional nibble</item>
        ///   <item>1 — Compact signed: [−54…+∞] with 1/16 fractional resolution</item>
        ///   <item>2 — Kelvin raw integer: value = (raw − 273.15) via calibration transform</item>
        /// </list>
        /// </summary>
        private double DecodeTempValue(byte[] data, CanBusParameter param)
        {
            switch (param.Type)
            {
                case 0:
                {
                    // Full BCD signed: high byte nibbles encode hundreds/tens, low byte encodes units/fraction.
                    double val = -256
                        + 256 * ((data[param.ByteH] & 0xF0) >> 4)
                        + -16
                        + 16 * (data[param.ByteH] & 0x0F)
                        + ((data[param.ByteL] & 0xF0) >> 4);
                    val += (data[param.ByteL] & 0x0F) * (1.0 / 16.0);
                    return val;
                }
                case 1:
                {
                    // Compact signed encoding.
                    double val = -54
                        + 16 * ((data[param.ByteH] & 0xF0) >> 4)
                        + -1
                        + (data[param.ByteH] & 0x0F);
                    val += ((data[param.ByteL] & 0xF0) >> 4) * (1.0 / 16.0);
                    return val;
                }
                case 2:
                {
                    // Raw Kelvin integer, converted via calibration formula (subtracts 273.15).
                    param.Value = (data[param.ByteH] << 8) | data[param.ByteL];
                    return param.GetTransformResult() - 273.15;
                }
                default:
                    return 0;
            }
        }

        /// <summary>
        /// Decodes an RPM value from the CAN frame.
        /// Two encoding variants exist depending on <c>param.Type</c>:
        /// <list type="bullet">
        ///   <item>0/1 — Upper 12 bits: ByteH shifted left 4, ByteL shifted right 4</item>
        ///   <item>2   — Lower 12 bits: lower nibble of ByteH as upper bits, ByteL as lower byte</item>
        /// </list>
        /// </summary>
        private static double DecodeRpmValue(byte[] data, CanBusParameter param)
        {
            int raw = param.Type switch
            {
                0 or 1 => (data[param.ByteH] << 4) | (data[param.ByteL] >> 4),
                2       => ((data[param.ByteH] & 0x0F) << 8) | data[param.ByteL],
                _       => 0
            };
            return raw;
        }

        /// <summary>
        /// Checks if bench or pump frame reception has timed out and fires
        /// liveness events on transition from alive to dead.
        /// </summary>
        private void CheckLivenessTimeout()
        {
            var now = DateTime.UtcNow;

            if (_benchAlive && (now - _lastBenchFrameUtc).TotalMilliseconds > LivenessTimeoutMs)
            {
                _benchAlive = false;
                BenchLivenessChanged?.Invoke(false);
            }

            if (_pumpAlive && (now - _lastPumpFrameUtc).TotalMilliseconds > LivenessTimeoutMs)
            {
                _pumpAlive = false;
                PumpLivenessChanged?.Invoke(false);
            }
        }

        // ── IIR low-pass filter ───────────────────────────────────────────────────

        /// <summary>
        /// Single-pole exponential moving average: result = prev + alpha * (value − prev).
        /// Rounds to 4 decimal places to avoid floating-point drift accumulation.
        /// </summary>
        private static double PassFilterUpdate(double prev, double value, double alpha)
            => Math.Round(prev + alpha * (value - prev), 4);

        // ── Helpers ───────────────────────────────────────────────────────────────

        private void EmitStatusChanged(TPCANStatus status)
        {
            string text = status.ToString();
            // Strip the "PCAN_ERROR_" prefix for brevity in the UI status bar.
            string shortText = text.StartsWith("PCAN_ERROR_", StringComparison.Ordinal)
                ? text[11..]
                : text;
            StatusChanged?.Invoke(shortText, status == TPCANStatus.PCAN_ERROR_OK);
        }

        private void LogPcanError(string context)
        {
            var sb = new StringBuilder(256);
            PCANBasic.GetErrorText(CurrentStatus, 0, sb);
            _log.Error(LogId, $"{context}: {CurrentStatus} — {sb}");
        }

        // ── IDisposable ───────────────────────────────────────────────────────────

        /// <summary>Stops the read thread and releases the PCAN channel.</summary>
        public void Dispose()
        {
            Disconnect();
            _receiveEvent?.Dispose();
        }
    }
}