/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package cz.academyofscience.upt.challenge2014.filters;

/**
 * Implementation of FFT filter
 *
 * @author xjuraj
 */
public class FFTFilter {

    public static double[][] dft(double[] input) {
        int N = input.length;
        double[][] output = new double[2][N];

        for (int k = 0; k < N; k++) {
            for (int n = 0; n < N; n++) {
                output[0][k] += input[n] * Math.cos(-2 * Math.PI * k * n / N); // Real
                output[1][k] += input[n] * Math.sin(-2 * Math.PI * k * n / N); // Imaginary
            }
        }

        return output;
    }

    public static double[][] fft(double[] input) {
        double[][] output = new double[2][input.length];
        //convert to complex values with complex number equal to Zero
        for (int i = 0; i < input.length; i++) {
            output[0][i] = input[i];
            output[1][i] = 0;
        }
        //compute complex  FFT
        return fftComplex(output);
    }

    public static double[][] fftComplex(double[][] input) {
        int N = input[0].length;
        double[][] output = new double[2][N];

        try {

            for (int i = 0; i < N; i++) {
                output[0][i] = input[0][i];
                output[1][i] = input[1][i];
            }

            int NM1 = N - 1;
            int ND2 = N / 2;

            int M = (int) (Math.log10(N) / Math.log10(2));

            int J = ND2;
            int K = -1;

            for (int I = 1; I <= N - 2; I++) {
                if (I < J) {
                    double TR = output[0][J];
                    double TI = output[1][J];

                    output[0][J] = output[0][I]; //1150
                    output[1][J] = output[1][I]; //1160

                    output[0][I] = TR;
                    output[1][I] = TI;
                }
                //goto label1190;
                //label1190:
                K = ND2;
                //label1200:
                while (K <= J) {
                    J = J - K;
                    K = K / 2;
                    //goto label1200; //1230
                }
                J = J + K;
            } //1250

            for (int L = 1; L <= M; L++) {
                int LE = (int) Math.pow(2, L);
                int LE2 = LE / 2;
                double UR = 1;
                double UI = 0;

                double pod = Math.PI / (double) LE2;

                double SR = Math.cos(pod);
                double SI = -Math.sin(pod); //1330

                double TR = 0;
                double TI = 0;

                for (J = 1; J <= LE2; J++) {
                    int JM1 = J - 1;

                    for (int I = JM1; I <= NM1; I = I + LE) {
                        int IP = I + LE2;

                        TR = output[0][IP] * (double) UR - output[1][IP] * (double) UI;
                        TI = output[0][IP] * (double) UI + output[1][IP] * (double) UR;
                        output[0][IP] = output[0][I] - TR;
                        output[1][IP] = output[1][I] - TI;
                        output[0][I] = output[0][I] + TR;
                        output[1][I] = output[1][I] + TI;
                    } //1440

                    double TMR = UR;

                    UR = TMR * SR - UI * SI;
                    UI = TMR * SI + UI * SR;

                } //1480
            } //1490
        } catch (Exception e) {
            System.err.println("Exception in fftComplex: " + e.getMessage());
        }

        return output;
    }

    public static double[] ifft(double[][] complexData, int originalLength, boolean doEnvelope) {
        int N = complexData[0].length;
        double[] output = new double[originalLength];

        for (int i = 0; i < N; i++) {
            complexData[1][i] = -complexData[1][i];
        }

        complexData = fftComplex(complexData);

        for (int i = 0; i < N; i++) {
            complexData[0][i] /= (double) N;
            complexData[1][i] /= -(double) N;

            if (i < originalLength) {
                if (doEnvelope) {
                    double real = (double) complexData[0][i];
                    double imag = (double) complexData[1][i];
                    output[i] = (double) Math.sqrt(real * real + imag * imag);
                } else {
                    output[i] = (double) complexData[0][i];
                }
            }
        }
        return output;
    }

    public static double[] dftMagnitude(double[] input) {
        double N = input.length;
        double[] output = new double[(int) N / 2];
        double temp1, temp2;

        for (int k = 0; k < N / 2; k++) {
            temp1 = 0;
            temp2 = 0;
            for (int n = 0; n < N; n++) {
                temp1 += input[n] * Math.cos(-2 * Math.PI * k * n / N);
                temp2 += input[n] * Math.sin(-2 * Math.PI * k * n / N);
            }
            output[k] = Math.sqrt(Math.pow(temp1, 2) + Math.pow(temp2, 2)); // Magnitude
        }
        return output;
    }

    public static double[][] dftMagPhase(double[] input) {
        int N = input.length;
        double[][] output = new double[2][N];
        double temp1, temp2;

        for (int k = 0; k < N; k++) {
            temp1 = 0;
            temp2 = 0;
            for (int n = 0; n < N; n++) {
                temp1 += input[n] * Math.cos(-2 * Math.PI * k * n / N);
                temp2 += input[n] * Math.sin(-2 * Math.PI * k * n / N);
            }
            output[0][k] = Math.sqrt(Math.pow(temp1, 2) + Math.pow(temp2, 2)); // Magnitude
            output[1][k] = Math.atan2(temp2, temp1); // Phase
        }

        return output;
    }

    /**
     * computation of log with base X. As suggested here:
     * http://stackoverflow.com/questions/3305059/how-do-you-calculate-log-base-2-in-java-for-integers
     *
     * @param value value to compute log from
     * @param base logarithm base
     * @return log_x(value)
     */
    public static double logX(double value, double base) {
        return Math.log(value) / Math.log(base);// + 1e-10;
    }

    public static int convertToLowerPow2(double val) {
        double lf = logX(val, 2);
        return (int) Math.pow(2, (int) lf);
    }

    public static int convertToHigherPow2(double val) {
        double lf = logX(val, 2);
        double sp = (int) lf;

        if (sp < lf) {
            sp += 1;
        }

        return (int) Math.pow(2, sp);
    }

    public static double[] fft(double[][] data, double sampleFrequency, int windowLength, double startFreq, double endFreq, boolean bandStop, boolean doEnvelope) {

        double[] outputPre = null;
        double freqBinWidth = sampleFrequency / (double)(data[0].length);

        double[][] fDFTR = new double[2][data[0].length];
        double[][] copyfDFTR = new double[2][data[0].length];

        for (int i = 0; i < data[0].length; i++) {
            fDFTR[0][i] = data[0][i];
            fDFTR[1][i] = data[1][i];
        }

        for (int i = 0; i < data[0].length / 2; i++)//take care here
        {
            double freq = (double) i * freqBinWidth;

            boolean toDel = freq >= startFreq && freq <= endFreq;
            if (!bandStop) {
                toDel = !toDel;
            }

            int i2 = data[0].length - i - 1;

            if (toDel) {
                fDFTR[0][i] = 0;
                fDFTR[1][i] = 0;

                fDFTR[0][i2] = 0;
                fDFTR[1][i2] = 0;
            }

        }

        //---přesun pro obálku---
        if (doEnvelope && !bandStop) {
            double center = (startFreq + endFreq) / 2; //--střed pole

            int centerIndex = (int) (center / freqBinWidth);
            int leftIndex = (int) (startFreq / freqBinWidth);
            int rightIndex = (int) (endFreq / freqBinWidth);

            for (int i = 0; i < data[0].length; i++) {
                copyfDFTR[0][i] = fDFTR[0][i];
                copyfDFTR[1][i] = fDFTR[1][i];

                if (i < leftIndex || i > rightIndex) {
                    copyfDFTR[0][i] = 0;
                    copyfDFTR[1][i] = 0;
                }

            }

            for (int i = 0; i < fDFTR[0].length; i++) {
                int newIndex = i - centerIndex;
                if (newIndex < 0) {
                    newIndex = data[0].length + newIndex;
                }
                fDFTR[0][newIndex] = copyfDFTR[0][i];
                fDFTR[1][newIndex] = copyfDFTR[1][i];
            }
        }//--obálka

        double[] output = ifft(fDFTR, windowLength, doEnvelope);

        outputPre = new double[output.length];

        for (int i = 0; i < output.length; i++) {
            outputPre[i] = (double) output[i];
        }

        return outputPre;
    }

    public static double[] fft(double[] data, double sampleFrequency, double startFreq, double endFreq, boolean bandStop, boolean doEnvelope) {
        //---do FFT---
        int neededLength = convertToHigherPow2(data.length);
        double[] sourceD = new double[neededLength];
        System.arraycopy(data, 0, sourceD, 0, data.length);
        double[][] FFTall = fft(sourceD);
        double fbw = sampleFrequency / (double) (FFTall[0].length);
        //---filtrování frekvencí----
        double[][] fDFTR = new double[2][FFTall[0].length];

        for (int i = 0; i < FFTall[0].length; i++) {
            fDFTR[0][i] = FFTall[0][i];
            fDFTR[1][i] = FFTall[1][i];
        }

        for (int i = 0; i < FFTall[0].length / 2; i++) {

            double freq = (double) i * fbw;

            boolean toDel = freq >= startFreq && freq <= endFreq;
            if (!bandStop) {
                toDel = !toDel;
            }

            int i2 = FFTall[0].length - i - 1;

            if (toDel) {
                fDFTR[0][i] = 0;
                fDFTR[1][i] = 0;

                fDFTR[0][i2] = 0;
                fDFTR[1][i2] = 0;
            }

        }

        //---přesun pro obálku---
        if (doEnvelope && !bandStop) {
            double[][] copyfDFTR = new double[2][FFTall[0].length];

            int i1 = (int) (startFreq / fbw);
            int i2 = (int) (endFreq / fbw);

            for (int i = 0; i < fDFTR[0].length; i++) {
                copyfDFTR[0][i] = fDFTR[0][i];
                copyfDFTR[1][i] = fDFTR[1][i];

                if (i < i1 || i > i2) {
                    copyfDFTR[0][i] = 0;
                    copyfDFTR[1][i] = 0;
                }
            }

            double center = (startFreq + endFreq) / 2; //--střed pole

            int centerIndex = (int) (center / fbw);

            for (int i = 0; i < fDFTR[0].length; i++) {
                int newIndex = i - centerIndex;
                if (newIndex < 0) {
                    newIndex = fDFTR[0].length + newIndex;
                }

                fDFTR[0][newIndex] = copyfDFTR[0][i];
                fDFTR[1][newIndex] = copyfDFTR[1][i];
            }
        }//--obálka

        //----ifft
        return ifft(fDFTR, data.length, doEnvelope);
    }


}
