/* Header File Including ------------------------------------------------------------------------ */
#include "Fft.h"

/* Macro Declaration ---------------------------------------------------------------------------- */
#define FFT_MCB_TYPE_OMEGA_AND_POSITION 0x01
#define SWAP(type, a, b)                { type t = a; a = b; b = t; }

/* Structure Declaration ------------------------------------------------------------------------ */
typedef struct FFT_MCB {
    uint8_t        type;
    uint32_t       parameter[4];
    struct FFT_MCB *next;
    struct FFT_MCB *prev;
    uint8_t        buffer[10240];
} FFT_MCB;

/* Global Variable Definition ------------------------------------------------------------------- */
static FFT_MCB gMemery;

/* Function Definition -------------------------------------------------------------------------- */
/**----------------------------------------------------------------------
* Function    : FftInit
* Description : 初始化傅里叶运算库
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
static int FftInit(int length) {
    Complex_t *omega;
    uint16_t *position;
    int bitLength;
    int i;
	int shift;
	int j;
    if (length * (sizeof(Complex_t) + sizeof(uint16_t)) > sizeof(gMemery.buffer)) {
        return 0;
    }

    if (FFT_MCB_TYPE_OMEGA_AND_POSITION == gMemery.type && length == (int)gMemery.parameter[0]) {
        return length;
    }

    gMemery.type = FFT_MCB_TYPE_OMEGA_AND_POSITION;
    gMemery.parameter[0] = length;

    omega = (Complex_t *) gMemery.buffer;
    for (i = 0; i < length; ++i) {
        omega[i].real = (float)cos(-2 * M_PI * i / length);
        omega[i].image = (float)sin(-2 * M_PI * i / length);
    }

    position = (uint16_t *)(omega + length);
    for (bitLength = 0, i = length; (i >> bitLength); ++bitLength);
    for (i = 0; i < length; ++i) {
        j = 0;
        for (shift = 0; shift < bitLength - 1; ++shift) {
            if((i >> shift) & 1) {
                j |= (1 << (bitLength - shift - 2));
            }
        }
        position[i] = (uint16_t)j;
    }

    return length;
}

int FFT_Dft(Complex_t *dft, float *signal, int length) {

    if (0 == FftInit(length)) return 0;
    
    for (int k = 0; k < length; ++k) {
        dft[k].real  = 0;
        dft[k].image = 0;
        for(int i = 0; i < length; ++i) {
            int ik = i * k % length;
            dft[k].real  += signal[i] * ((Complex_t *)gMemery.buffer)[ik].real;
            dft[k].image += signal[i] * ((Complex_t *)gMemery.buffer)[ik].image;
        }
    }
    return length;
}


int FFT_Fft(Complex_t *fft, float *signal, int length) {
    int i,l,j,m;
    uint16_t *position;
    Complex_t *omega;

	//正常情况下，length正常、没溢出就不会进入return0
    if (length <= 0 || 0 != (length & (length - 1)) || 0 == FftInit(length)) {
        return 0;
    }

    for (i = 0; i < length; ++i) {
        fft[i].real = signal[i];
        fft[i].image = 0;
    }

    position = (uint16_t *)(gMemery.buffer + length * sizeof(Complex_t));
    for (i = 0; i < length; ++i) {
        if (i < position[i]) {
            SWAP(Complex_t, fft[i], fft[position[i]]);
        }
    }

    omega = (Complex_t *)gMemery.buffer;
    for (l = 2, m = 1; l <= length; l <<= 1, m <<= 1) {
        for (i = 0; i < length; i += l) {
            for (j = 0; j < m; ++j) {
                Complex_t delta = ComplexMul(omega[length / l * j], fft[i + m + j]);
                fft[i + m + j] = ComplexSub(fft[i + j], delta);
                fft[i + j] = ComplexAdd(fft[i + j], delta);
            }
        } 
    }

    return length;
}
