//
// 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];
}
}