DP44/DataPRO/ExocortexDSP/.svn/pristine/9c/9c438182334e8b0d649acfe028878e7ed7839722.svn-base

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Exocortex.DSP
{
    public enum PassFilterType
    {
        Bessel,
        Butterworth,
        Chebyshev,
        CriticalDamping
    }
    public static class PassFilter
    {
        public static double[] HighPass(double[] values, double sampleRate, double centerFrequency, PassFilterType type, uint order)
        {
            return RunFilter(values, sampleRate, centerFrequency, type, order, false);
        }
        public static double[] LowPass(double[] values, double sampleRate, double centerFrequency, PassFilterType type, uint order)
        {
            return RunFilter(values, sampleRate, centerFrequency, type, order, true);
        }
        private static double[] RunFilter(double[] values, double sampleRate, double centerFrequency, PassFilterType type, uint order, bool lowPass)
        {
            Complex[] data = new Complex[values.Length];
            for (int i = 0; i < values.Length; i++)
            {
                data[i] = (Complex)values[i];
            }
            Complex[] result;
            switch (type)
            {
                case PassFilterType.Bessel:
                    result = BesselFilter(data, sampleRate, centerFrequency, order, lowPass);
                    break;
                case PassFilterType.Chebyshev:
                    result = ChebyshevFilter(data, sampleRate, centerFrequency, order, 1D, 0.005, lowPass);
                    break;
                case PassFilterType.Butterworth:
                default:
                    result = ButterworthFilter(data, sampleRate, centerFrequency, order, 1D, lowPass);
                    break;
            }
            return result.Select(c => c.Re).ToArray();
        }
        private static Complex[] BesselFilter(Complex[] values, double sampleRate, double centerFrequency, uint order, bool lowPass)
        {
            var signal = (Complex[])values.Clone();
            var N = signal.Length;

            // Get the reverse Bessel polynomial-to-gain function denominator
            Polynomial B = BesselDenominatorPolynomial((int)order);
            var numerator = Math.Sqrt(B.GetCoefficient(0));
            // Apply forward FFT
            Fourier.FFT(signal, N, FourierDirection.Forward);
            // Apply gain function
            if (centerFrequency > 0)
            {
                var numBins = N / 2;  // Half the length of the FFT by symmetry
                var binWidth = sampleRate / N; // Hz

                // Filter
                System.Threading.Tasks.Parallel.For(1, N / 2, i =>
                {
                    var binFreq = binWidth * i;
                    var gain = BesselGain(numerator, B, binFreq, centerFrequency, lowPass);
                    signal[i] *= gain;
                    signal[N - i] *= gain;
                });

            }
            // Reverse filtered signal
            Fourier.FFT(signal, N, FourierDirection.Backward);
            signal = signal.Select(n => n / N).ToArray(); // scale result by length

            return signal;
        }
        private static double BesselGain(double numerator, Polynomial B, double freq, double centerFreq, bool lowPass)
        {
            return numerator / Math.Sqrt(B.Evaluate(lowPass ? freq / centerFreq : centerFreq / freq)); // low-to-high pass is inverting f/fc
        }

        // uses logic from https://www.centerspace.net/butterworth-filter-csharp
        private static Complex[] ButterworthFilter(Complex[] values, double sampleRate, double centerFrequency, uint order, double DCGain, bool lowPass)
        {
            var signal = (Complex[])values.Clone();
            var N = signal.Length;

            // Apply forward FFT
            Fourier.FFT(signal, N, FourierDirection.Forward);
            // Apply gain function
            if (centerFrequency > 0)
            {
                var numBins = N / 2;  // Half the length of the FFT by symmetry
                var binWidth = sampleRate / N; // Hz

                // Filter
                System.Threading.Tasks.Parallel.For(1, N / 2, i =>
                {
                    var binFreq = binWidth * i;
                    var gain = ButterworthGain(DCGain, binFreq, centerFrequency, order, lowPass);
                    signal[i] *= gain;
                    signal[N - i] *= gain;
                });

            }
            // Reverse filtered signal
            Fourier.FFT(signal, N, FourierDirection.Backward);
            // scale result by length
            signal = signal.Select(n => n / N).ToArray();
            return signal;
        }
        private static double ButterworthGain(double DCGain, double freq, double centerFreq, uint order, bool lowPass)
        {
            return DCGain / Math.Sqrt(1 + Math.Pow(lowPass ? freq / centerFreq : centerFreq / freq, 2.0 * order)); // low-to-high pass is just inverting f/fc
        }

        // uses logic from https://www.centerspace.net/chebyshev-filter-csharp
        private static Complex[] ChebyshevFilter(Complex[] values, double sampleRate, double centerFrequency, uint order, double DCGain, double ripple, bool lowPass)
        {
            var signal = (Complex[])values.Clone();
            var N = signal.Length;

            // Get the Chebyshev polynomial
            Polynomial T = ChebyshevPolynomial((int)order);
            // Apply forward FFT
            Fourier.FFT(signal, N, FourierDirection.Forward);
            // Remove DC offset
            signal[0] = (Complex)0;

            // Apply gain function
            if (centerFrequency > 0)
            {
                var numBins = N / 2;  // Half the length of the FFT by symmetry
                var binWidth = sampleRate / N; // Hz

                // Filter
                System.Threading.Tasks.Parallel.For(1, N / 2, i =>
                {
                    var binFreq = binWidth * i;
                    var gain = ChebyshevGain(DCGain, ripple, T, binFreq, centerFrequency, order, lowPass);
                    signal[i] *= gain;
                    signal[N - i] *= gain;
                });

            }
            // Reverse filtered signal
            Fourier.FFT(signal, N, FourierDirection.Backward);
            // scale result by length
            signal = signal.Select(n => n / N).ToArray();

            return signal;
        }
        private static double ChebyshevGain(double DCGain, double ripple, Polynomial T, double freq, double centerFreq, uint order, bool lowPass)
        {
            return DCGain / Math.Sqrt(1 + ripple * Math.Pow(T.Evaluate(lowPass ? freq / centerFreq : centerFreq / freq), 2.0 * order));
        }

        private static Polynomial ChebyshevPolynomial(int order)
        {
            switch (order)
            {
                case 0:
                    return new SimplePolynomial(new double[] { 1 });
                case 1:
                    return new SimplePolynomial(new double[] { 0, 1 });
                case 2:
                    return new SimplePolynomial(new double[] { -1, 0, 2 });
                case 3:
                    return new SimplePolynomial(new double[] { 0, -3, 0, 4 });
                case 4:
                    return new SimplePolynomial(new double[] { 1, 0, -8, 0, 8 });
                case 5:
                    return new SimplePolynomial(new double[] { 0, 5, 0, -20, 0, 16 });
                case 6:
                    return new SimplePolynomial(new double[] { -1, 0, 1, 8, 0, -48, 0, 32 });
                case 7:
                    return new SimplePolynomial(new double[] { 0, 7, 0, 56, 0, -112, 0, 64 });
                case 8:
                    return new SimplePolynomial(new double[] { 1, 0, -32, 0, 160, 0, -256, 0, 128 });
                default:
                    throw new NotImplementedException();
            }
        }
        private static Polynomial BesselDenominatorPolynomial(int order)
        {
            // Polynomials in the square-rooted denominator of bessel gain functions
            // Obtained via magnitude calc of H(iw) of each Bessel H(s): https://en.wikipedia.org/wiki/Bessel_filter#The_transfer_function
            // at https://www.symbolab.com/solver/complex-numbers-magnitude-calculator/
            switch (order)
            {
                case 2:
                    return new SimplePolynomial(new double[] { 9, 0, 3, 0, 1 });
                case 3:
                    return new SimplePolynomial(new double[] { 225, 0, 45, 0, 6, 0, 1 });
                case 4:
                    return new SimplePolynomial(new double[] { 11025, 0, 1575, 0, 135, 0, 10, 0, 1 });
                case 5:
                    return new SimplePolynomial(new double[] { 893025, 0, 99225, 0, 6300, 0, 315, 0, 15, 0, 1 });
                case 6:
                    return new SimplePolynomial(new double[] { 108056025, 0, 9823275, 0, 496125, 0, 18900, 0, 630, 0, 21, 0, 1 });
                case 7:
                    return new SimplePolynomial(new double[] { 18261468225, 0, 1404728325, 0, 58939650, 0, 1819125, 0, 47250, 0, 1134, 0, 28, 0, 1 });
                case 8:
                    return new SimplePolynomial(new double[] { 4108830350625, 0, 273922023375, 0, 9833098275, 0, 255405150, 0, 5457375, 0, 103950, 0, 1890, 0, 36, 0, 1 });
                default:
                    throw new NotImplementedException();
            }
        }
    }
}