/*

  ESP32 FFT
  =========

  This provides a vanilla radix-2 FFT implementation and a test example.

  Author
  ------

  This code was written by [Robin Scheibler](http://www.robinscheibler.org)
  during rainy days in October 2017.

  License
  -------

  Copyright (c) 2017 Robin Scheibler

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in all
  copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  SOFTWARE.

*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <complex.h>

#include "fft.h"

#define TWO_PI          6.28318530
#define USE_SPLIT_RADIX 1
#define LARGE_BASE_CASE 1

fft_config_t *fft_init(int size, fft_type_t type, fft_direction_t direction,
                       float *input, float *output) {
    /*
     * Prepare an FFT of correct size and types.
     *
     * If no input or output buffers are provided, they will be allocated.
     */
    int k, m;

    fft_config_t *config = (fft_config_t *)malloc(sizeof(fft_config_t));

    // Check if the size is a power of two
    if ((size & (size - 1)) != 0)  // tests if size is a power of two
        return NULL;

    // start configuration
    config->flags     = 0;
    config->type      = type;
    config->direction = direction;
    config->size      = size;

    // Allocate and precompute twiddle factors
    config->twiddle_factors = (float *)malloc(2 * config->size * sizeof(float));

    float two_pi_by_n = TWO_PI / config->size;

    for (k = 0, m = 0; k < config->size; k++, m += 2) {
        config->twiddle_factors[m]     = cosf(two_pi_by_n * k);  // real
        config->twiddle_factors[m + 1] = sinf(two_pi_by_n * k);  // imag
    }

    // Allocate input buffer
    if (input != NULL)
        config->input = input;
    else {
        if (config->type == FFT_REAL)
            config->input = (float *)malloc(config->size * sizeof(float));
        else if (config->type == FFT_COMPLEX)
            config->input = (float *)malloc(2 * config->size * sizeof(float));

        config->flags |= FFT_OWN_INPUT_MEM;
    }

    if (config->input == NULL) return NULL;

    // Allocate output buffer
    if (output != NULL)
        config->output = output;
    else {
        if (config->type == FFT_REAL)
            config->output = (float *)malloc(config->size * sizeof(float));
        else if (config->type == FFT_COMPLEX)
            config->output = (float *)malloc(2 * config->size * sizeof(float));

        config->flags |= FFT_OWN_OUTPUT_MEM;
    }

    if (config->output == NULL) return NULL;

    return config;
}

void fft_destroy(fft_config_t *config) {
    if (config->flags & FFT_OWN_INPUT_MEM) free(config->input);

    if (config->flags & FFT_OWN_OUTPUT_MEM) free(config->output);

    free(config->twiddle_factors);
    free(config);
}

void fft_execute(fft_config_t *config) {
    if (config->type == FFT_REAL && config->direction == FFT_FORWARD)
        rfft(config->input, config->output, config->twiddle_factors,
             config->size);
    else if (config->type == FFT_REAL && config->direction == FFT_BACKWARD)
        irfft(config->input, config->output, config->twiddle_factors,
              config->size);
    else if (config->type == FFT_COMPLEX && config->direction == FFT_FORWARD)
        fft(config->input, config->output, config->twiddle_factors,
            config->size);
    else if (config->type == FFT_COMPLEX && config->direction == FFT_BACKWARD)
        ifft(config->input, config->output, config->twiddle_factors,
             config->size);
}

void fft(float *input, float *output, float *twiddle_factors, int n) {
    /*
     * Forward fast Fourier transform
     * DIT, radix-2, out-of-place implementation
     *
     * Parameters
     * ----------
     *  input (float *)
     *    The input array containing the complex samples with
     *    real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1),
     * Im(x_n-1)] output (float *) The output array containing the complex
     * samples with real/imaginary parts interleaved [Re(x0), Im(x0), ...,
     * Re(x_n-1), Im(x_n-1)] n (int) The FFT size, should be a power of 2
     */

#if USE_SPLIT_RADIX
    split_radix_fft(input, output, n, 2, twiddle_factors, 2);
#else
    fft_primitive(input, output, n, 2, twiddle_factors, 2);
#endif
}

void ifft(float *input, float *output, float *twiddle_factors, int n) {
    /*
     * Inverse fast Fourier transform
     * DIT, radix-2, out-of-place implementation
     *
     * Parameters
     * ----------
     *  input (float *)
     *    The input array containing the complex samples with
     *    real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1),
     * Im(x_n-1)] output (float *) The output array containing the complex
     * samples with real/imaginary parts interleaved [Re(x0), Im(x0), ...,
     * Re(x_n-1), Im(x_n-1)] n (int) The FFT size, should be a power of 2
     */
    ifft_primitive(input, output, n, 2, twiddle_factors, 2);
}

void rfft(float *x, float *y, float *twiddle_factors, int n) {
    // This code uses the two-for-the-price-of-one strategy
#if USE_SPLIT_RADIX
    split_radix_fft(x, y, n / 2, 2, twiddle_factors, 4);
#else
    fft_primitive(x, y, n / 2, 2, twiddle_factors, 4);
#endif

    // Now apply post processing to recover positive
    // frequencies of the real FFT
    float t = y[0];
    y[0]    = t + y[1];  // DC coefficient
    y[1]    = t - y[1];  // Center coefficient

    // Apply post processing to quarter element
    // this boils down to taking complex conjugate
    y[n / 2 + 1] = -y[n / 2 + 1];

    // Now process all the other frequencies
    int k;
    for (k = 2; k < n / 2; k += 2) {
        float xer, xei, xor_t, xoi, c, s, tr, ti;

        c = twiddle_factors[k];
        s = twiddle_factors[k + 1];

        // even half coefficient
        xer = 0.5 * (y[k] + y[n - k]);
        xei = 0.5 * (y[k + 1] - y[n - k + 1]);

        // odd half coefficient
        xor_t = 0.5 * (y[k + 1] + y[n - k + 1]);
        xoi   = -0.5 * (y[k] - y[n - k]);

        tr = c * xor_t + s * xoi;
        ti = -s * xor_t + c * xoi;

        y[k]     = xer + tr;
        y[k + 1] = xei + ti;

        y[n - k]     = xer - tr;
        y[n - k + 1] = -(xei - ti);
    }
}

void irfft(float *x, float *y, float *twiddle_factors, int n) {
    /*
     * Destroys content of input vector
     */
    int k;

    // Here we need to apply a pre-processing first
    float t = x[0];
    x[0]    = 0.5 * (t + x[1]);
    x[1]    = 0.5 * (t - x[1]);

    x[n / 2 + 1] = -x[n / 2 + 1];

    for (k = 2; k < n / 2; k += 2) {
        float xer, xei, xor_t, xoi, c, s, tr, ti;

        c = twiddle_factors[k];
        s = twiddle_factors[k + 1];

        xer = 0.5 * (x[k] + x[n - k]);
        tr  = 0.5 * (x[k] - x[n - k]);

        xei = 0.5 * (x[k + 1] - x[n - k + 1]);
        ti  = 0.5 * (x[k + 1] + x[n - k + 1]);

        xor_t = c * tr - s * ti;
        xoi   = s * tr + c * ti;

        x[k]     = xer - xoi;
        x[k + 1] = xor_t + xei;

        x[n - k]     = xer + xoi;
        x[n - k + 1] = xor_t - xei;
    }

    ifft_primitive(x, y, n / 2, 2, twiddle_factors, 4);
}

void fft_primitive(float *x, float *y, int n, int stride,
                   float *twiddle_factors, int tw_stride) {
    /*
     * This code will compute the FFT of the input vector x
     *
     * The input data is assumed to be real/imag interleaved
     *
     * The size n should be a power of two
     *
     * y is an output buffer of size 2n to accomodate for complex numbers
     *
     * Forward fast Fourier transform
     * DIT, radix-2, out-of-place implementation
     *
     * For a complex FFT, call first stage as:
     * fft(x, y, n, 2, 2);
     *
     * Parameters
     * ----------
     *  x (float *)
     *    The input array containing the complex samples with
     *    real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1),
     * Im(x_n-1)] y (float *) The output array containing the complex samples
     * with real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1),
     * Im(x_n-1)] n (int) The FFT size, should be a power of 2 stride (int) The
     * number of elements to skip between two successive samples tw_stride (int)
     *    The number of elements to skip between two successive twiddle factors
     */
    int k;
    float t;

#if LARGE_BASE_CASE
    // End condition, stop at n=8 to avoid one trivial recursion
    if (n == 8) {
        fft8(x, stride, y, 2);
        return;
    }
#else
    // End condition, stop at n=2 to avoid one trivial recursion
    if (n == 2) {
        y[0] = x[0] + x[stride];
        y[1] = x[1] + x[stride + 1];
        y[2] = x[0] - x[stride];
        y[3] = x[1] - x[stride + 1];
        return;
    }
#endif

    // Recursion -- Decimation In Time algorithm
    fft_primitive(x, y, n / 2, 2 * stride, twiddle_factors,
                  2 * tw_stride);  // even half
    fft_primitive(x + stride, y + n, n / 2, 2 * stride, twiddle_factors,
                  2 * tw_stride);  // odd half

    // Stitch back together

    // We can a few multiplications in the first step
    t    = y[0];
    y[0] = t + y[n];
    y[n] = t - y[n];

    t        = y[1];
    y[1]     = t + y[n + 1];
    y[n + 1] = t - y[n + 1];

    for (k = 1; k < n / 2; k++) {
        float x1r, x1i, x2r, x2i, c, s;
        c = twiddle_factors[k * tw_stride];
        s = twiddle_factors[k * tw_stride + 1];

        x1r = y[2 * k];
        x1i = y[2 * k + 1];
        x2r = c * y[n + 2 * k] + s * y[n + 2 * k + 1];
        x2i = -s * y[n + 2 * k] + c * y[n + 2 * k + 1];

        y[2 * k]     = x1r + x2r;
        y[2 * k + 1] = x1i + x2i;

        y[n + 2 * k]     = x1r - x2r;
        y[n + 2 * k + 1] = x1i - x2i;
    }
}

void split_radix_fft(float *x, float *y, int n, int stride,
                     float *twiddle_factors, int tw_stride) {
    /*
     * This code will compute the FFT of the input vector x
     *
     * The input data is assumed to be real/imag interleaved
     *
     * The size n should be a power of two
     *
     * y is an output buffer of size 2n to accomodate for complex numbers
     *
     * Forward fast Fourier transform
     * Split-Radix
     * DIT, radix-2, out-of-place implementation
     *
     * For a complex FFT, call first stage as:
     * fft(x, y, n, 2, 2);
     *
     * Parameters
     * ----------
     *  x (float *)
     *    The input array containing the complex samples with
     *    real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1),
     * Im(x_n-1)] y (float *) The output array containing the complex samples
     * with real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1),
     * Im(x_n-1)] n (int) The FFT size, should be a power of 2 stride (int) The
     * number of elements to skip between two successive samples twiddle_factors
     * (float *) The array of twiddle factors tw_stride (int) The number of
     * elements to skip between two successive twiddle factors
     */
    int k;

#if LARGE_BASE_CASE
    // End condition, stop at n=2 to avoid one trivial recursion
    if (n == 8) {
        fft8(x, stride, y, 2);
        return;
    } else if (n == 4) {
        fft4(x, stride, y, 2);
        return;
    }
#else
    // End condition, stop at n=2 to avoid one trivial recursion
    if (n == 2) {
        y[0] = x[0] + x[stride];
        y[1] = x[1] + x[stride + 1];
        y[2] = x[0] - x[stride];
        y[3] = x[1] - x[stride + 1];
        return;
    } else if (n == 1) {
        y[0] = x[0];
        y[1] = x[1];
        return;
    }
#endif

    // Recursion -- Decimation In Time algorithm
    split_radix_fft(x, y, n / 2, 2 * stride, twiddle_factors, 2 * tw_stride);
    split_radix_fft(x + stride, y + n, n / 4, 4 * stride, twiddle_factors,
                    4 * tw_stride);
    split_radix_fft(x + 3 * stride, y + n + n / 2, n / 4, 4 * stride,
                    twiddle_factors, 4 * tw_stride);

    // Stitch together the output
    float u1r, u1i, u2r, u2i, x1r, x1i, x2r, x2i;
    float t;

    // We can save a few multiplications in the first step
    u1r = y[0];
    u1i = y[1];
    u2r = y[n / 2];
    u2i = y[n / 2 + 1];

    x1r = y[n];
    x1i = y[n + 1];
    x2r = y[n / 2 + n];
    x2i = y[n / 2 + n + 1];

    t    = x1r + x2r;
    y[0] = u1r + t;
    y[n] = u1r - t;

    t        = x1i + x2i;
    y[1]     = u1i + t;
    y[n + 1] = u1i - t;

    t            = x2i - x1i;
    y[n / 2]     = u2r - t;
    y[n + n / 2] = u2r + t;

    t                = x1r - x2r;
    y[n / 2 + 1]     = u2i - t;
    y[n + n / 2 + 1] = u2i + t;

    for (k = 1; k < n / 4; k++) {
        float u1r, u1i, u2r, u2i, x1r, x1i, x2r, x2i, c1, s1, c2, s2;
        c1 = twiddle_factors[k * tw_stride];
        s1 = twiddle_factors[k * tw_stride + 1];
        c2 = twiddle_factors[3 * k * tw_stride];
        s2 = twiddle_factors[3 * k * tw_stride + 1];

        u1r = y[2 * k];
        u1i = y[2 * k + 1];
        u2r = y[2 * k + n / 2];
        u2i = y[2 * k + n / 2 + 1];

        x1r = c1 * y[n + 2 * k] + s1 * y[n + 2 * k + 1];
        x1i = -s1 * y[n + 2 * k] + c1 * y[n + 2 * k + 1];
        x2r = c2 * y[n / 2 + n + 2 * k] + s2 * y[n / 2 + n + 2 * k + 1];
        x2i = -s2 * y[n / 2 + n + 2 * k] + c2 * y[n / 2 + n + 2 * k + 1];

        t            = x1r + x2r;
        y[2 * k]     = u1r + t;
        y[2 * k + n] = u1r - t;

        t                = x1i + x2i;
        y[2 * k + 1]     = u1i + t;
        y[2 * k + n + 1] = u1i - t;

        t                    = x2i - x1i;
        y[2 * k + n / 2]     = u2r - t;
        y[2 * k + n + n / 2] = u2r + t;

        t                        = x1r - x2r;
        y[2 * k + n / 2 + 1]     = u2i - t;
        y[2 * k + n + n / 2 + 1] = u2i + t;
    }
}

void ifft_primitive(float *input, float *output, int n, int stride,
                    float *twiddle_factors, int tw_stride) {
#if USE_SPLIT_RADIX
    split_radix_fft(input, output, n, stride, twiddle_factors, tw_stride);
#else
    fft_primitive(input, output, n, stride, twiddle_factors, tw_stride);
#endif

    int ks;

    int ns = n * stride;

    // reverse all coefficients from 1 to n / 2 - 1
    for (ks = stride; ks < ns / 2; ks += stride) {
        float t;

        t               = output[ks];
        output[ks]      = output[ns - ks];
        output[ns - ks] = t;

        t                   = output[ks + 1];
        output[ks + 1]      = output[ns - ks + 1];
        output[ns - ks + 1] = t;
    }

    // Apply normalization
    float norm = 1. / n;
    for (ks = 0; ks < ns; ks += stride) {
        output[ks] *= norm;
        output[ks + 1] *= norm;
    }
}

inline void fft8(float *input, int stride_in, float *output, int stride_out) {
    /*
     * Unrolled implementation of FFT8 for a little more performance
     */
    float a0r, a1r, a2r, a3r, a4r, a5r, a6r, a7r;
    float a0i, a1i, a2i, a3i, a4i, a5i, a6i, a7i;
    float b0r, b1r, b2r, b3r, b4r, b5r, b6r, b7r;
    float b0i, b1i, b2i, b3i, b4i, b5i, b6i, b7i;
    float t;
    float sin_pi_4 = 0.7071067812;

    a0r = input[0];
    a0i = input[1];
    a1r = input[stride_in];
    a1i = input[stride_in + 1];
    a2r = input[2 * stride_in];
    a2i = input[2 * stride_in + 1];
    a3r = input[3 * stride_in];
    a3i = input[3 * stride_in + 1];
    a4r = input[4 * stride_in];
    a4i = input[4 * stride_in + 1];
    a5r = input[5 * stride_in];
    a5i = input[5 * stride_in + 1];
    a6r = input[6 * stride_in];
    a6i = input[6 * stride_in + 1];
    a7r = input[7 * stride_in];
    a7i = input[7 * stride_in + 1];

    // Stage 1

    b0r = a0r + a4r;
    b0i = a0i + a4i;

    b1r = a1r + a5r;
    b1i = a1i + a5i;

    b2r = a2r + a6r;
    b2i = a2i + a6i;

    b3r = a3r + a7r;
    b3i = a3i + a7i;

    b4r = a0r - a4r;
    b4i = a0i - a4i;

    b5r = a1r - a5r;
    b5i = a1i - a5i;
    // W_8^1 = 1/sqrt(2) - j / sqrt(2)
    t   = b5r + b5i;
    b5i = (b5i - b5r) * sin_pi_4;
    b5r = t * sin_pi_4;

    // W_8^2 = -j
    b6r = a2i - a6i;
    b6i = a6r - a2r;

    b7r = a3r - a7r;
    b7i = a3i - a7i;
    // W_8^3 = -1 / sqrt(2) + j / sqrt(2)
    t   = sin_pi_4 * (b7i - b7r);
    b7i = -(b7r + b7i) * sin_pi_4;
    b7r = t;

    // Stage 2

    a0r = b0r + b2r;
    a0i = b0i + b2i;

    a1r = b1r + b3r;
    a1i = b1i + b3i;

    a2r = b0r - b2r;
    a2i = b0i - b2i;

    // * j
    a3r = b1i - b3i;
    a3i = b3r - b1r;

    a4r = b4r + b6r;
    a4i = b4i + b6i;

    a5r = b5r + b7r;
    a5i = b5i + b7i;

    a6r = b4r - b6r;
    a6i = b4i - b6i;

    // * j
    a7r = b5i - b7i;
    a7i = b7r - b5r;

    // Stage 3

    // X[0]
    output[0] = a0r + a1r;
    output[1] = a0i + a1i;

    // X[4]
    output[4 * stride_out]     = a0r - a1r;
    output[4 * stride_out + 1] = a0i - a1i;

    // X[2]
    output[2 * stride_out]     = a2r + a3r;
    output[2 * stride_out + 1] = a2i + a3i;

    // X[6]
    output[6 * stride_out]     = a2r - a3r;
    output[6 * stride_out + 1] = a2i - a3i;

    // X[1]
    output[stride_out]     = a4r + a5r;
    output[stride_out + 1] = a4i + a5i;

    // X[5]
    output[5 * stride_out]     = a4r - a5r;
    output[5 * stride_out + 1] = a4i - a5i;

    // X[3]
    output[3 * stride_out]     = a6r + a7r;
    output[3 * stride_out + 1] = a6i + a7i;

    // X[7]
    output[7 * stride_out]     = a6r - a7r;
    output[7 * stride_out + 1] = a6i - a7i;
}

inline void fft4(float *input, int stride_in, float *output, int stride_out) {
    /*
     * Unrolled implementation of FFT4 for a little more performance
     */
    float t1, t2;

    t1                     = input[0] + input[2 * stride_in];
    t2                     = input[stride_in] + input[3 * stride_in];
    output[0]              = t1 + t2;
    output[2 * stride_out] = t1 - t2;

    t1        = input[1] + input[2 * stride_in + 1];
    t2        = input[stride_in + 1] + input[3 * stride_in + 1];
    output[1] = t1 + t2;
    output[2 * stride_out + 1] = t1 - t2;

    t1                     = input[0] - input[2 * stride_in];
    t2                     = input[stride_in + 1] - input[3 * stride_in + 1];
    output[stride_out]     = t1 + t2;
    output[3 * stride_out] = t1 - t2;

    t1                         = input[1] - input[2 * stride_in + 1];
    t2                         = input[3 * stride_in] - input[stride_in];
    output[stride_out + 1]     = t1 + t2;
    output[3 * stride_out + 1] = t1 - t2;
}
