tbf/TBF/Rig/RegisterReaders/GenesisRegReader/implementations/FlowDirectionDetection.cs

///
/// Copyright (c) 2018-2019 Sensus Slovensko a.s.
///

using System;
using System.Linq;
using Common;
using log4net;
using TBF.Rig.TestMethods.iPerlCommunication.iPerlHead;

namespace TBF.Rig.RegisterReaders.GenesisRegReader.implementations
{
    public class FlowDirectionDetection
    {
        private static readonly ILog log = LogManager.GetLogger(typeof(IperlHead));

        const int FIFO_SIZE = 64;            // 8 sec @ 8Hz
        const double MAX_OPTO_DROPOUT = 4.5; // sec

        private readonly double[] volumeRawFifo;
        private readonly double[] timestampFifo;   // centered timestamps

        private int iChanelsCount = 3;
        private int[] fifoCount;
        private int[] fifoIx;
        private DateTime[] lastFifoWriteTime;

        // regression sums (double is ideal here)
        private double[] sumXX;
        private double[] sumX;
        private double[] sumXY;
        private double[] sumY;

        private double[] minSlope;
        private double[] maxSlope;

        // timestamp centering for numerical stability
        private double[] firstTimestamp;

        public FlowDirectionDetection()
        {
            volumeRawFifo = new double[FIFO_SIZE];
            timestampFifo = new double[FIFO_SIZE];
            
            fifoCount = new int[iChanelsCount];
            fifoIx = new int[iChanelsCount];
            lastFifoWriteTime = new DateTime[iChanelsCount];

            sumXX = new double[iChanelsCount];
            sumX = new double[iChanelsCount];
            sumXY = new double[iChanelsCount];
            sumY = new double[iChanelsCount];

            minSlope = new double[iChanelsCount];
            maxSlope = new double[iChanelsCount];
            
            firstTimestamp = new []{double.NaN,double.NaN,double.NaN};
            
            ClearFifo();
        }

        public void ClearFifo()
        {
            
            Array.Clear(fifoCount, 0, fifoCount.Length);
            Array.Clear(fifoIx, 0, fifoIx.Length);    
            
            lastFifoWriteTime = Enumerable
                .Repeat(DateTime.MinValue, lastFifoWriteTime.Length)
                .ToArray();
            
            Array.Clear(sumXX, 0, sumXX.Length);
            Array.Clear(sumX, 0, sumX.Length);
            Array.Clear(sumXY, 0, sumXY.Length);
            Array.Clear(sumY, 0, sumY.Length);

            Array.Clear(minSlope, 0, minSlope.Length);
            Array.Clear(maxSlope, 0, maxSlope.Length);
            
            // clear FIFO - TODO test if this is necessary
            Array.Clear(volumeRawFifo, 0, volumeRawFifo.Length);
            Array.Clear(timestampFifo, 0, timestampFifo.Length);
            
            firstTimestamp = Enumerable
                .Repeat(double.NaN, firstTimestamp.Length)
                .ToArray();
        }

        /// <summary>
        /// Add sample to rolling FIFO and update regression sums
        /// </summary>
        public void WriteToFifo(double[] volumeRaw, double[] timestamp, int iChanel)
        {
            // establish time origin (CRITICAL for double precision)
            if (double.IsNaN(firstTimestamp[iChanel]))
                firstTimestamp[iChanel] = timestamp[iChanel];

            double x = timestamp[iChanel] - firstTimestamp[iChanel]; // centered time
            double y = volumeRaw[iChanel];

            // remove oldest sample if buffer full
            if (fifoCount[iChanel] == FIFO_SIZE)
            {
                double oldX = timestampFifo[fifoIx[iChanel]];
                double oldY = volumeRawFifo[fifoIx[iChanel]];

                sumXX[iChanel] -= oldX * oldX;
                sumX[iChanel]  -= oldX;
                sumXY[iChanel] -= oldX * oldY;
                sumY[iChanel]  -= oldY;
            }
            else
            {
                fifoCount[iChanel]++;
            }

            // add new sample
            sumXX[iChanel] += x * x;
            sumX[iChanel]  += x;
            sumXY[iChanel] += x * y;
            sumY[iChanel]  += y;

            // store sample
            timestampFifo[fifoIx[iChanel]] = x;
            volumeRawFifo[fifoIx[iChanel]] = y;

            fifoIx[iChanel] = (fifoIx[iChanel] + 1) % FIFO_SIZE;
            lastFifoWriteTime[iChanel] = DateTime.Now;
        }

        // public bool AreFifoDataValid()
        // {
        //     return (DateTime.Now.Subtract(lastFifoWriteTime).TotalSeconds <= MAX_OPTO_DROPOUT)
        //            && (fifoCount == FIFO_SIZE);
        // }

        // public OptoHeadState CheckFlowDirection(Counting counting, string iPerlHeadName)
        // {
        //     if (!AreFifoDataValid())
        //         return OptoHeadState.OptoNok;
        //
        //     try
        //     {
        //         double N = fifoCount;
        //
        //         double numer = N * sumXY - sumX * sumY;
        //         double denom = N * sumXX - sumX * sumX;
        //
        //         if (Math.Abs(denom) < 1e-12)
        //             return OptoHeadState.DirNok;
        //
        //         double slope = numer / denom;
        //
        //         if (slope > maxSlope) maxSlope = slope;
        //         if (slope < minSlope) minSlope = slope;
        //
        //         if (counting == Counting.Arbitrary ||
        //             (counting == Counting.Positive && maxSlope > Math.Abs(2 * minSlope)) ||
        //             (counting == Counting.Negative && minSlope < -Math.Abs(2 * maxSlope)))
        //         {
        //             return OptoHeadState.OptoAndDirOK;
        //         }
        //
        //         return OptoHeadState.DirNok;
        //     }
        //     catch (Exception ex)
        //     {
        //         log.ErrorFormat("{0} : CheckFlowDirection() failed: {1}", iPerlHeadName, ex);
        //         return OptoHeadState.DirNok;
        //     }
        // }
    }
}