//
//  fft.c
//  NaiveFFT
//
//  Created by Jacky Jack on 26/08/2021.
//

#include "fft.h"




//calc the fft
void KEEPALIVE fft_if(double *x_i, double *x_q, int n, int inv) {
    
    int i=0,j=0,k=0,m=0, irem=0, sign;
    double tq,ti;
    
    k=n/2;
    for (j=1,i=0;i<n,j<n; i++) {
        if (i<j) {
            
            //swap values
            ti = x_i[j-1];
            tq = x_q[j-1];
            
            x_i[j-1] = x_i[i];
            x_q[j-1] = x_q[i];
            
            x_i[i] = ti;
            x_q[i] = tq;
            printf("i=%d j=%d\n",i,j);
            //find middle point
            k = n/2;
            while (k<j) {
                j = j-k;
                k=k/2;
            }
        } else {
            printf("i=%d j=%d\n",i,j);
            k = n/2;
            while (k<j) {
                j = j - k;
                k = k/2;
            }
        }
        j = j+k;
    }
    //calculate number of stages:
    //m=log2(n)
    m=0;
    irem = n;
    while(irem>1) {
        irem = irem/2;
        m = m+1;
    }
    
    //FFT or IFFT
    if (inv==1) {
        sign = 1;
    } else {
        sign = -1;
    }
    
    // transform
    for (i=1; i<m; i++) {
        int le=pow(2,i);
        int le1=le/2;
        double ui=1.0;
        double uq=0.0;
        double wi = cos(M_PI/le1);
        double wq = sign*sin(M_PI/le1);
        
        printf("le1=%d\n",le1);
        for (j=0; j<le1;j++) {
            
            k=j;
            while (k<n) {
                int ip = k+le1;
                
                ti = x_i[ip]*ui - x_q[ip]*uq;
                tq = x_q[ip]*ui + x_i[ip]*uq;
                
                x_i[ip] = x_i[k-1] - ti;
                x_q[ip] = x_q[k-1] - tq;
                
                x_i[k-1] = x_i[k-1] + ti;
                x_q[k-1] = x_q[k-1] + tq;
                
                k = k+le;
                printf("k=%d\n",k-1);
            }
            double temp = ui*wi - uq*wq;
            uq = uq*wi + ui*wq;
            ui = temp;
        }
    }
    
    //if inverse
    if (inv==1) {
        for (i=0;i<n;i++) {
            x_i[i] = x_i[i]/n;
            x_q[i] = x_q[i]/n;
        }
    }
    
}

#define complex_mul_re(a_re, a_im, b_re, b_im)  (a_re * b_re - a_im * b_im)
#define complex_mul_im(a_re, a_im, b_re, b_im)  (a_re * b_im + a_im * b_re)
//https://github.com/rshuston/FFT-C/blob/master/libfft/fft.c
void KEEPALIVE ffti_shuffle_1(double *x_i, double *x_q, uint64_t n) {
    int Nd2 = n>>1;
    int Nm1 = n-1;
    int i,j;
    
    
    for (i = 0, j = 0; i < n; i++) {
           if (j > i) {
               double tmp_r = x_i[i];
               double tmp_i = x_q[i];
               //data[i] = data[j];
               x_i[i] = x_q[j];
               x_q[i] = x_q[j];
               //data[j] = tmp;
               x_i[j] = tmp_r;
               x_q[j] = tmp_i;
           }

           /*
            * Find least significant zero bit
            */

           unsigned lszb = ~i & (i + 1);

           /*
            * Use division to bit-reverse the single bit so that we now have
            * the most significant zero bit
            *
            * N = 2^r = 2^(m+1)
            * Nd2 = N/2 = 2^m
            * if lszb = 2^k, where k is within the range of 0...m, then
            *     mszb = Nd2 / lszb
            *          = 2^m / 2^k
            *          = 2^(m-k)
            *          = bit-reversed value of lszb
            */

           unsigned mszb = Nd2 / lszb;

           /*
            * Toggle bits with bit-reverse mask
            */

           unsigned bits = Nm1 & ~(mszb - 1);
           j ^= bits;
       }
}
void KEEPALIVE fft_1(double *x_i, double *x_q, uint64_t n, uint64_t inv) {
    uint64_t n_log2;
    uint64_t r;
    uint64_t m, md2;
    uint64_t i,j,k;
    uint64_t i_e, i_o;
    double theta_2pi;
    double theta;
    
    double Wm_r, Wm_i, Wmk_r, Wmk_i;
    double u_r, u_i, t_r, t_i;
    
    //find log of n
    i=n;
    n_log2 = 0;
    while (i>1) {
        i=i/2;
        n_log2+=1;
    }
    
    if (inv==1) {
        theta_2pi = -2*M_PI;
    } else {
        theta_2pi = 2*M_PI;
    }
    
    
    for (i=1; i<= n_log2; i++) {
        m  = 1 << i;
        md2 = m >> 1;
        theta = theta_2pi / m;
        Wm_r = cos(theta);
        Wm_i = sin(theta);
        
        
        for (j=0; j<n; j+=m) {
            Wmk_r = 1.0f;
            Wmk_i = 0.0f;
            
            for (k=0; k<md2; k++) {
                i_e = j+k;
                i_o = i_e + md2;
                
                u_r = x_i[i_e];
                u_i = x_q[i_e];
                
                //t_r = Wmk_r * x_i[i_o] - Wmk_i * x_q[i_o];
                //t_i = Wmk_r * x_q[i_o] - Wmk_i * x_i[i_o];
                t_r = complex_mul_re(Wmk_r, Wmk_i, x_i[i_o], x_q[i_o]);
                t_i = complex_mul_im(Wmk_r, Wmk_i, x_i[i_o], x_q[i_o]);
                
                
                x_i[i_e] = u_r + t_r;
                x_q[i_e] = u_i + t_i;
                
                x_i[i_o] = u_r - t_r;
                x_q[i_o] = u_i - t_i;

                t_r = complex_mul_re(Wmk_r, Wmk_i, Wm_r, Wm_i);
                t_i = complex_mul_im(Wmk_r, Wmk_i, Wm_r, Wm_i);
                
                Wmk_r = t_r;
                Wmk_i = t_i;
            }
        }
    }
}

//dft works fine
void KEEPALIVE dft(double *x_i, double *x_q, int n, int inv) {
    double Wn,Wk;
    //static array
    //double Xi[DATA_SIZE],Xq[DATA_SIZE];
    //dynamic array
    double *Xi, *Xq;
    double c,s;
    int i,j;
    
    Xi = malloc(n*sizeof(double));
    Xq = malloc(n*sizeof(double));
    
    Wn = 2*M_PI/n;
    
    if (inv==1) {
        Wn=-Wn;
    }
    
    for (i=0;i<n;i++) {
        Xi[i] = 0.0f;
        Xq[i] = 0.0f;
        
        Wk = i*Wn;
        for (j=0;j<n;j++) {
            c = cos(j*Wk);
            s = sin(j*Wk);
            
            //i - real, q - imaginary
            Xi[i] = Xi[i] + x_i[j]*c + x_q[j]*s;
            Xq[i] = Xq[i] - x_i[j]*s + x_q[j]*c;
        }
        
        if (inv==1) {
            Xi[i] = Xi[i]/n;
            Xq[i] = Xq[i]/n;
        }
    }
    
    for (i=0;i<n;i++) {
        x_i[i] = Xi[i];
        x_q[i] = Xq[i];
    }
    
}