/*
SaeJ211FilterUtility.cs

$Log: FilterUtility.cs,v $
Revision 1.3  2008/11/11 21:56:41  DTS\paul.hrissikopoulos
Added Brian's suggested filtering changes.

Revision 1.2  2007/01/26 21:53:50  Paul Hrissikopoulos
Added bulk of October 2003 SAE J211-compliant filter utility implementation.

Revision 1.1  2007/01/13 00:42:00  Paul Hrissikopoulos
Added initial support for October 2003-compliant filtering.

Revision 1.3  2006/10/26 22:06:47  Paul Hrissikopoulos
Added channel data filtering.

Revision 1.2  2006/10/23 16:07:16  Paul Hrissikopoulos
Added data filtering phase and related support.

Revision 1.1  2006/10/18 23:44:11  Paul Hrissikopoulos
Added SaeJ211FilterUtility class.

Copyright © 2006
Diversified Technical Systems, Inc.
All Rights Reserved
*/

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text;
using DTS.Common.Utilities;
using DTS.Common.Utilities.DotNetProgrammingConstructs;
using System.Linq;

namespace DTS.Common.Utilities.SaeJ211
{
    /// <summary>
    /// Utility for filtering data according to the NHTSA implementation of the
    /// SAE J211 standard.
    /// </summary>
    public class FilterUtility : Exceptional
    {
        /// <summary>
        /// Get/set the calibration scaling factor to be applied to data, post-filtering.
        /// Useful for "last minute" scaling or inversion.  Defaults to "1.0".
        /// </summary>
        public double CalibrationFactor
        {
            get => _CalibrationFactor.Value;
            set => _CalibrationFactor.Value = value;
        }
        private readonly RangeRestrictedDoubleProperty _CalibrationFactor
            = new RangeRestrictedDoubleProperty(-1.0, 1.0, 1.0, true);

        /// <summary>
        /// Get/set the sampling rate (samples/sec) of the data to be filtered.  This
        /// property must be set by user before the data can be filtered.
        /// </summary>
        public double SampleRate
        {
            get => _SampleRate.Value;
            set => _SampleRate.Value = value;
        }
        private readonly RangeRestrictedDoubleProperty _SampleRate
            = new RangeRestrictedDoubleProperty(0.0, 1000000000.0, 0, false);

        /// <summary>
        /// Get/set SAE Channel Filter Class to be applied to data.  This property must
        /// be set by user before the data can be filtered.
        /// </summary>
        public ChannelFilter Cfc
        {
            get => _ChannelFilter.Value;
            set
            {
                _ChannelFilter.Value = value;
                _cfc = new CfcValueAttributeCoder().DecodeAttributeValue(_ChannelFilter.Value);
            }
        }
        private readonly Property<ChannelFilter> _ChannelFilter
            = new Property<ChannelFilter>(
                typeof(FilterUtility).Namespace + ".FilterUtility.ChannelFilter",
                ChannelFilter.Unfiltered,
                false
            );
        double _cfc;

        /// <summary>
        /// Get/set the <see cref="double"/> ad hoc frequency of this filter.
        /// </summary>
        public double AdHocFrequency
        {
            get => _AdHocFrequency.Value;
            set => _AdHocFrequency.Value = value;
        }
        private readonly Property<double> _AdHocFrequency
            = new Property<double>(
                typeof(FilterUtility).Namespace + ".FilterUtility",
                -1,
                false
            );

        /// <summary>
        /// Get/set the zero baseline flag.  Defaults to "false".
        /// </summary>
        public bool ZeroBaseline
        {
            get => _ZeroBaseline.Value;
            set => _ZeroBaseline.Value = value;
        }
        private readonly Property<bool> _ZeroBaseline
            = new Property<bool>(
                typeof(FilterUtility).Namespace + ".FilterUtility.ZeroBaseline",
                false,
                true
            );

        /// <summary>
        /// Get/set the size (in sec) of the padding added to the beginning and ending of
        /// the data before filtering to "squelch out" startup spikes.  A negative value
        /// will cause pad to default to 5% of data size.
        /// </summary>
        public double PadSize
        {
            get => _PadSize.Value;
            set => _PadSize.Value = value;
        }
        private readonly Property<double> _PadSize
            = new Property<double>(
                typeof(FilterUtility).Namespace + ".FilterUtility.PadSize",
                -1.0,
                true
            );

        /// <summary>
        /// Types of pre/post-padding data population available.
        /// </summary>
        public enum DataPaddingType
        {
            /// <summary>
            /// Create data padding by repeatedly copying value of first and last samples.
            /// </summary>
            FirstAndLast,

            /// <summary>
            /// Create data padding by mirroring data at beginning and end of data vector.
            /// </summary>
            Mirror,

            /// <summary>
            /// Pad the data with zeros
            /// </summary>
            Zero
        }

        /// <summary>
        /// Get/set the type of data padding appended/prepended to the data prior to
        /// being subjected to the filtering algorithm.
        /// </summary>
        public DataPaddingType DataPadding
        {
            get => _DataPadding.Value;
            set => _DataPadding.Value = value;
        }
        private readonly Property<DataPaddingType> _DataPadding
            = new Property<DataPaddingType>(
                typeof(FilterUtility).Namespace + ".FilterUtility.DataPaddingType",
                DataPaddingType.Mirror,
                true
            );

        /// <summary>
        /// Get the string that describes the type of filtering performed by this object.
        /// </summary>
        public string Type => "Butterworth, 4-pole";

        public uint NumberOfPreDataPointsToInclude
        {
            get => _NumberOfPreDataPointsToInclude.Value;
            set => _NumberOfPreDataPointsToInclude.Value = value;
        }

        private readonly Property<uint> _NumberOfPreDataPointsToInclude
            = new Property<uint>(typeof(FilterUtility).Namespace + ".FilterUtility.NumberOfPreDataPointsToInclude",
                0, true);

        /// <summary>
        /// Initialize an instance of the SaeJ211FilterUtility class.
        /// </summary>
        public FilterUtility()
        {
        }

        /// <summary>
        /// Convert the specified frequency to an equivalent CFC value.  Note that the 
        /// "Big 4" CFC values get slightly special treatment.
        /// </summary>
        /// 
        /// <param name="frequency">
        /// The <see cref="double"/> frequency to be converted.
        /// </param>
        /// 
        /// <returns>
        /// The <see cref="double"/> CFC equivalent of the specified frequency.
        /// </returns>
        /// 
        private double FrequencyToCfc(double frequency)
        {
            try
            {
                double cfc = -1;
                foreach (ChannelFilter filter in Enum.GetValues(typeof(ChannelFilter)))
                {
                    if (frequency == (int)filter)
                    {
                        cfc = new CfcValueAttributeCoder().DecodeAttributeValue(filter);
                        break;
                    }
                }
                return cfc >= 0 ? cfc : frequency * 0.6;
            }

            catch (System.Exception ex)
            {
                throw new FilterUtility.Exception("encountered problem converting frequency to CFC value", ex);
            }
        }

        public delegate void InvalidDataDelegate();
        public delegate void SetProgressDelegate(double value = -1D);
        /// <summary>
        /// Apply the SAE J211 filter algorithm (4-pole, phaseless, lo-pass) to the specified
        /// data using to the current FilterTool properties.
        /// </summary>
        /// 
        /// <param name="data">
        /// An array of <see cref="double"/>s containing the raw data to be filtered.
        /// </param>
        /// 
        /// <param name="bUseLegacyTDCFilterAdjustment"></param>
        /// <param name="invalidDataFunc"></param>
        /// <returns>
        /// An array of <see cref="double"/>s containing the filtered data.
        /// </returns>
        /// 
        public virtual double[] ApplyFilter(double[] data, InvalidDataDelegate invalidDataFunc = null, bool bUseLegacyTDCFilterAdjustment = false,
            SetProgressDelegate setProgress = null)
        {
            try
            {
                if (null != invalidDataFunc)
                {
                    var invalids = from d in data where double.IsNaN(d) select d;
                    if (invalids.Any())
                    {
                        invalidDataFunc();
                    }
                }
                ////////////////////////////////////////////////////////////////////////////////////	
                // IMPLEMENTATION NOTE:
                //
                // Pads the data with 5% of the original buffer size fore and aft to "squelch" out
                // filter start-up spikes.  Optionally zeros baseline by subtracting the average
                // of the first 5% of the original data points (skips data which is padded by this
                // method).  Optionally scales the data after filtering.
                ////////////////////////////////////////////////////////////////////////////////////	

                if (null == data)
                    throw new ArgumentNullException("attempted to apply filter to null data buffer reference");

                else
                {
                    if (Cfc == ChannelFilter.Unfiltered) return data;
                    if (Cfc == ChannelFilter.UnfilteredZero) { return data; }
                    double t, pi, wd, wa, d0, d1, d2, e1, e2;
                    double f1, f2, f3, f4, f5;

                    ////////////////////////////////////////////////////////////////////////////////////	
                    // Formula setup
                    ////////////////////////////////////////////////////////////////////////////////////	
                    t = 1.0 / SampleRate;
                    pi = 4.0 * System.Math.Atan(1.0);
                    wd = 2.0 * pi * (Cfc == ChannelFilter.AdHoc ? FrequencyToCfc(AdHocFrequency) : _cfc) * 2.0775;
                    wa = (System.Math.Sin(wd * (t / 2.0))) / (System.Math.Cos(wd * (t / 2.0)));  //  The original formula
                    d0 = (wa * wa) / (1 + System.Math.Sqrt(2.0) * wa + (wa * wa));
                    d1 = 2.0 * d0;
                    d2 = d0;
                    e1 = -2.0 * ((wa * wa) - 1) / (1 + System.Math.Sqrt(2.0) * wa + (wa * wa));
                    e2 = (-1 + System.Math.Sqrt(2.0) * wa - (wa * wa)) / (1 + System.Math.Sqrt(2.0) * wa + (wa * wa));

                    var DataEnd = data.Length - 1;
                    int Pad;

                    if (PadSize >= 0.0)
                    {
                        Pad = (int)(PadSize * SampleRate);
                    }
                    else
                    {
                        // Calculate padding as either 5% of all data, or the time required
                        // for theoretical filter stabilization.
                        Pad = (int)System.Math.Max(0.05 * DataEnd, 4 / AdHocFrequency * SampleRate);
                    }

                    var Z = new double[2 * Pad + data.Length];
                    var A = new double[2 * Pad + data.Length];
                    var paddedData = new double[2 * Pad + data.Length];

                    var FirstIndexOfBeginPad = 0;
                    var LastIndexOfBeginPad = Pad - 1;
                    if ((DataPadding == DataPaddingType.Mirror) && (Pad - FirstIndexOfBeginPad >= data.Length))
                    {   // Avoid fault in Mirror mode
                        DataPadding = DataPaddingType.FirstAndLast;
                    }
                    switch (DataPadding)
                    {   //
                        // Populate the start-side pad according to this instance's current
                        // DataPadding setting.
                        //
                        case DataPaddingType.FirstAndLast:
                            for (var i = FirstIndexOfBeginPad; i <= LastIndexOfBeginPad; i++)
                                paddedData[i] = data[0];   // 1st point of data
                            break;

                        case DataPaddingType.Mirror:
                            for (var i = FirstIndexOfBeginPad; i <= LastIndexOfBeginPad; i++)
                                paddedData[i] = data[Pad - i]; // Mirror of data
                            break;

                        case DataPaddingType.Zero:
                            for (var i = FirstIndexOfBeginPad; i <= LastIndexOfBeginPad; i++)
                                paddedData[i] = 0;
                            break;
                        default:

                            throw new FilterUtility.Exception("implementor attempted to use an undefined data padding type");
                    }

                    // Copy raw data into newData after initial pad.
                    var FirstIndexOfData = LastIndexOfBeginPad + 1;
                    var LastIndexOfData = FirstIndexOfData + DataEnd;
                    for (var i = FirstIndexOfData; i <= LastIndexOfData; i++)
                        paddedData[i] = data[i - Pad];

                    var FirstIndexOfEndPad = LastIndexOfData + 1;
                    var LastIndexOfEndPad = LastIndexOfData + Pad;
                    switch (DataPadding)
                    {   //
                        // Populate the end-side pad according to this instance's current
                        // DataPadding setting.
                        //
                        case DataPaddingType.FirstAndLast:
                            for (var i = FirstIndexOfEndPad; i <= LastIndexOfEndPad; i++)
                                paddedData[i] = data[DataEnd]; // Last point of data
                            break;

                        case DataPaddingType.Mirror:
                            for (var i = FirstIndexOfEndPad; i <= LastIndexOfEndPad; i++)
                                paddedData[i] = paddedData[2 * LastIndexOfData - i]; // Mirror of data
                            break;

                        case DataPaddingType.Zero:
                            for (var i = FirstIndexOfEndPad; i <= LastIndexOfEndPad; i++)
                                paddedData[i] = 0;
                            break;

                        default:

                            throw new FilterUtility.Exception("implementor attempted to use an undefined data padding type");
                    }

                    var PaddedDataEnd = LastIndexOfEndPad; // DataEnd + 2 * Pad;


                    // You need to set initial values before the calculation
                    for (var i = 0; i <= 9; i++)
                        Z[PaddedDataEnd] += 0.1 * paddedData[i];
                    Z[PaddedDataEnd - 1] = Z[PaddedDataEnd];

                    ////////////////////////////////////////////////////////////////////////////////////	
                    //  Data passed through filter backward
                    ////////////////////////////////////////////////////////////////////////////////////	

                    long travel = (PaddedDataEnd - 2);
                    var progress = 0D;
                    for (var j = travel; j >= 0; j--)
                    {
                        f1 = (d0 * paddedData[j]);  // Breaking
                        f2 = (d1 * paddedData[j + 1]);  // down
                        f3 = (d2 * paddedData[j + 2]);  // a big 
                        f4 = (e1 * Z[j + 1]);           // formula
                        f5 = (e2 * Z[j + 2]);           // into smaller "groups".
                        Z[j] = f1 + f2 + f3 + f4 + f5;    // Putting the "groups" back together
                        var percent = System.Math.Floor(33D * ((travel - j) / (double)travel));
                        if (percent > progress) { setProgress?.Invoke(percent); progress = percent; }
                    }
                    setProgress?.Invoke(33);
                    ////////////////////////////////////////////////////////////////////////////////////	
                    //  Data passed through filter forward 
                    ////////////////////////////////////////////////////////////////////////////////////	

                    // You need to set initial values before the calculation
                    for (var i = 0; i <= 9; i++)
                        A[0] += 0.1 * Z[i];
                    A[1] = A[0];

                    for (var k = 3; k <= PaddedDataEnd; k++)
                    {
                        f1 = (d0 * Z[k]);           // Breaking 
                        f2 = (d1 * Z[k - 1]);           // down 
                        f3 = (d2 * Z[k - 2]);           // a big
                        f4 = (e1 * A[k - 1]);           // formula
                        f5 = (e2 * A[k - 2]);           // into smaller "groups".
                        A[k] = f1 + f2 + f3 + f4 + f5;    // Putting the "groups" back together
                        var percent = System.Math.Floor(33 + 33D * ((PaddedDataEnd - k) / (double)PaddedDataEnd));
                        if (percent > progress) { setProgress?.Invoke(percent); progress = percent; }
                    }
                    setProgress?.Invoke(66);
                    ////////////////////////////////////////////////////////////////////////////////////	
                    // Prepare baseline calculation, if required
                    ////////////////////////////////////////////////////////////////////////////////////	
                    var baselineSum = 0.0;

                    // Compute baseline if it's going to be applied.
                    if (ZeroBaseline)
                        for (var n = Pad; n <= (2 * Pad); n++)
                            baselineSum += A[n];

                    // Baseline adjustment.
                    var adjust = baselineSum / Pad;

                    ////////////////////////////////////////////////////////////////////////////////////	
                    // Apply baseline and/or calibration factor, then finalize filtered data
                    ////////////////////////////////////////////////////////////////////////////////////	
                    var filteredIndex = 0;
                    var filteredData = new double[data.Length];
                    //8747 - TDC data shifts one point when filtered
                    //this allows us to preserve that shift on demand
                    var numOfPreDataPointsToInclude = NumberOfPreDataPointsToInclude;
                    if (bUseLegacyTDCFilterAdjustment)
                    {
                        numOfPreDataPointsToInclude++;
                    }
                    travel = (PaddedDataEnd - (Pad + numOfPreDataPointsToInclude)) - (Pad - numOfPreDataPointsToInclude);
                    for (var k = (int)(Pad - numOfPreDataPointsToInclude); k <= (PaddedDataEnd - (Pad + numOfPreDataPointsToInclude)); k++)
                    {
                        // Zero baseline, if requested.
                        if (ZeroBaseline)
                            A[k] = A[k] - adjust;

                        // Multiplying by the calibration factor unless factor is 1.0
                        // AFTER the filtering has been finished.
                        if (1.0 != CalibrationFactor)
                            A[k] = A[k] * CalibrationFactor;

                        // Copy finished product into return buffer.
                        filteredData[filteredIndex++] = A[k];
                        var percent = System.Math.Floor(66D + 33D * (filteredIndex / (double)travel));
                        if (percent > progress) { setProgress?.Invoke(percent); progress = percent; }
                    }
                    setProgress?.Invoke(100);
                    // The deed is done.  Share the pain.
                    return filteredData;
                }
            }
            catch (System.Exception ex)
            {
                throw new FilterUtility.Exception("encountered problem SAE J211-filtering data", ex);
            }
        }
    }
}