tbf/TBF/Rig/TestMethods/iPerlCommunication/iPerlHead/FlowDirectionDetection.cs

///
/// Copyright (c) 2018-2019 Sensus Slovensko a.s.
///
using System;
using log4net;
using Common;

namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
{
    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 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 = double.NaN;

        public FlowDirectionDetection()
        {
            volumeRawFifo = new double[FIFO_SIZE];
            timestampFifo = new double[FIFO_SIZE];
            ClearFifo();
        }

        public void ClearFifo()
        {
            fifoCount = 0;
            fifoIx = 0;
            lastFifoWriteTime = DateTime.MinValue;

            sumXX = 0;
            sumX = 0;
            sumXY = 0;
            sumY = 0;

            minSlope = 0;
            maxSlope = 0;
            firstTimestamp = double.NaN;
        }

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

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

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

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

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

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

            fifoIx = (fifoIx + 1) % FIFO_SIZE;
            lastFifoWriteTime = 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;
            }
        }
    }
}