#include <math.h>
#include <stdio.h>
#include <stdlib.h>
//#include <time.h>
#include "fft_f.h"


/*
 * *************************************************************************************
 * function name: fft_f_prepare
 * parameters: N:          fft_size;
			   bit_width:  fft bit width
 * return: fft_tbl
 * description: Prepare FFT Table  
 * -------------------------------------------------------------------------------------
 */

fft_tbl fft_f_prepare(
                 unsigned short N,                  //fft size
                 unsigned char bit_width            //WP bit width
                 ) 
{
    unsigned short i,j,k,t;
    unsigned char log2n;
    unsigned char degree;
    unsigned short temp_j,temp_k;
    unsigned short temp;
    const unsigned char addr_constant[2] = {0,2};
    unsigned short COEF_W = bit_width-1;
    const double PI = 3.1415926;
    unsigned short nk;
/*
    unsigned short *p_reorder_idx;        //first stage reorder index
    complex16 *p_w1p;                     //WP gene
    complex16 *p_w2p;
    complex16 *p_w3p;
    */
    fft_tbl fft_tbl_inst;

    fft_tbl_inst.N = N;
    fft_tbl_inst.bit_width = bit_width;
    log2n = (unsigned char)(log(N)/log(2));
    degree = (unsigned char)(log2n >> 1);
    fft_tbl_inst.log2n = log2n;
    fft_tbl_inst.degree = degree;
   
    //malloc memory for p_reorder_idx
    fft_tbl_inst.p_reorder_idx = (unsigned short *)malloc(N * sizeof(unsigned short));
    if(fft_tbl_inst.p_reorder_idx == NULL) {
        printf("Cann't allocate memory ,terminating...");
        exit (1);
    }

    
    //malloc memory for WP gene
    k = 0;
    for (i = 0; i < degree; i++) {
        k = (unsigned short)(k+POW4I_1);
    }
    
    fft_tbl_inst.p_w1p = (complex16 *)malloc(k * sizeof(complex16));
    if(fft_tbl_inst.p_w1p == NULL) {
        printf("Cann't allocate memory ,terminating...");
        exit (1);
    }

    fft_tbl_inst.p_w2p = (complex16 *)malloc(k * sizeof(complex16));
    if(fft_tbl_inst.p_w2p == NULL) {
        printf("Cann't allocate memory ,terminating...");
        exit (1);
    }

    fft_tbl_inst.p_w3p = (complex16 *)malloc(k * sizeof(complex16));
    if(fft_tbl_inst.p_w3p == NULL) {
        printf("Cann't allocate memory ,terminating...");
        exit (1);
    }

    //Calculate reordered index
    for (i = 0; i < N; i++) {
        temp = 0;
        for (j = 0; j < degree; j++) {
            k = (unsigned short)(log2n + addr_constant[j % 2] - j -2);
            temp_k = (unsigned short)(((i & (1 << j)) >> j) << k);
            temp_j = (unsigned short)(((i & (1 << k)) >> k) << j);
            temp = (unsigned short)(temp + temp_k +temp_j);
        }
        *(fft_tbl_inst.p_reorder_idx+i) = temp;  
    }


    //calculate WP gene

     k = 0;
    for (i = 0; i < degree; i++) {
        t = (unsigned short)(1 << (i * 2));
        j = 0;
        do {                                      //for q=1:4^(degree-1)
            nk = (unsigned short)((j << log2n) >> ((i+1)*2));       // nk=(q-1)*N/(4^degree);
            fft_tbl_inst.p_w1p[k].r = (signed short)floor(cos(-2 * PI * nk / N) * (1 << COEF_W));
            fft_tbl_inst.p_w1p[k].i = (signed short)floor(sin(-2 * PI * nk / N) * (1 << COEF_W));
            k++;
        } while (++j < t);
    }
	
	k = 0;
    for (i = 0; i < degree; i++) {
        t = (unsigned short)(1 << (i * 2));
        j = 0;
        do {                                      //for q=1:4^(degree-1)
            nk = (unsigned short)(2 * (j << log2n) >> ((i+1)*2));       // nk=(q-1)*N/(4^degree);
            fft_tbl_inst.p_w2p[k].r = (signed short)floor(cos(-2 * PI * nk / N) * (1 << COEF_W));
            fft_tbl_inst.p_w2p[k].i = (signed short)floor(sin(-2 * PI * nk / N) * (1 << COEF_W));
            k++;
        } while (++j < t);
    }

    k = 0;
    for (i = 0; i < degree; i++) {
        t = (unsigned short)(1 << (i * 2));
        j = 0;
        do {                                      //for q=1:4^(degree-1)
            nk = (unsigned short)(3 * (j << log2n) >> ((i+1)*2));       // nk=(q-1)*N/(4^degree);
            fft_tbl_inst.p_w3p[k].r = (signed short)floor(cos(-2 * PI * nk / N) * (1 << COEF_W));
            fft_tbl_inst.p_w3p[k].i = (signed short)floor(sin(-2 * PI * nk / N) * (1 << COEF_W));
            k++;
        } while (++j < t);
    }

	fft_tbl_inst.p_w1p[0].r = fft_tbl_inst.p_w1p[0].r -1;
	fft_tbl_inst.p_w1p[1].r = fft_tbl_inst.p_w1p[1].r -1;
	fft_tbl_inst.p_w1p[5].r = fft_tbl_inst.p_w1p[5].r -1;
	fft_tbl_inst.p_w1p[21].r = fft_tbl_inst.p_w1p[21].r -1;
    
	fft_tbl_inst.p_w2p[0].r = fft_tbl_inst.p_w2p[0].r -1;
	fft_tbl_inst.p_w2p[1].r = fft_tbl_inst.p_w2p[1].r -1;
	fft_tbl_inst.p_w2p[5].r = fft_tbl_inst.p_w2p[5].r -1;
	fft_tbl_inst.p_w2p[21].r = fft_tbl_inst.p_w2p[21].r -1;
    
	fft_tbl_inst.p_w3p[0].r = fft_tbl_inst.p_w3p[0].r -1;
	fft_tbl_inst.p_w3p[1].r = fft_tbl_inst.p_w3p[1].r -1;
	fft_tbl_inst.p_w3p[5].r = fft_tbl_inst.p_w3p[5].r -1;
	fft_tbl_inst.p_w3p[21].r = fft_tbl_inst.p_w3p[21].r -1;
    

    return fft_tbl_inst;
}


/*
 * *************************************************************************************
 * function name: fft_f_free
 * parameters: fft_tbl_inst:   FFT table;
 * return: void
 * description:  Free FFT Table  
 * -------------------------------------------------------------------------------------
 */
void fft_f_free(
                fft_tbl fft_tbl_inst
                )
{
    free(fft_tbl_inst.p_reorder_idx);
    free(fft_tbl_inst.p_w1p);
    free(fft_tbl_inst.p_w2p);
    free(fft_tbl_inst.p_w3p);
}

/*
 * *************************************************************************************
 * function name: mult_comp
 * parameters: x1:     complex number1;
               x2:     complex number2;
               COEF_W: right shift bits of complex mult
 * return: complex mult result
 * description:  complex mult  
 * -------------------------------------------------------------------------------------
 */

complex16 mult_comp(
                    complex16 x1,
                    complex16 x2,
                    unsigned char COEF_W
                    )
{
    complex16 y;

    y.r = (signed short)(((x1.r * x2.r) >> COEF_W) - ((x1.i * x2.i) >> COEF_W));
    y.i = (signed short)(((x1.r * x2.i) >> COEF_W) + ((x1.i * x2.r) >> COEF_W));

    return y;
}



/*
 * *************************************************************************************
 * function name: dft4p
 * parameters: x1,x2,x3,x4:         input for compex data
               w1p,w2p,w3p:         w gene of FFT
               p_y1,p_y2,p_y3,p_y4: address to store result data
 * return: void
 * description:  base4 fft butterfly  
 * -------------------------------------------------------------------------------------
 */

void dft4p(
           complex16 x1,complex16 x2,complex16 x3,complex16 x4,
           complex16 w1p,complex16 w2p,complex16 w3p,
           unsigned char COEF_W,
           complex16 *p_y1,complex16 *p_y2,complex16 *p_y3,complex16 *p_y4
           )
{
    complex16 cw2p,bw1p,dw3p;

    cw2p = mult_comp(x3,w2p,COEF_W);
    bw1p = mult_comp(x2,w1p,COEF_W);
    dw3p = mult_comp(x4,w3p,COEF_W);


    p_y1->r = (signed short)(x1.r + bw1p.r + cw2p.r + dw3p.r);
    p_y1->i = (signed short)(x1.i + bw1p.i + cw2p.i + dw3p.i);

    p_y2->r = (signed short)(x1.r + bw1p.i - cw2p.r - dw3p.i);
    p_y2->i = (signed short)(x1.i - bw1p.r - cw2p.i + dw3p.r);

    p_y3->r = (signed short)(x1.r - bw1p.r + cw2p.r - dw3p.r);
    p_y3->i = (signed short)(x1.i - bw1p.i + cw2p.i - dw3p.i);

    p_y4->r = (signed short)(x1.r - bw1p.i - cw2p.r + dw3p.i);
    p_y4->i = (signed short)(x1.i + bw1p.r - cw2p.i - dw3p.r);

/*
y1 = x1 + bw1p + cw2p + dw3p;
y2 = x1 - j*bw1p - cw2p + j*dw3p;
y3 = x1 - bw1p + cw2p - dw3p;
y4 = x1 + j*bw1p - cw2p - j*dw3p;
*/
}




/*
 * *************************************************************************************
 * function name: fft_f_execute
 * parameters: p_dat_in : point to input buffer
               p_dat_out: point to output buffer
               mode     : 0 fft; 1 IFFT
               fft_tbl  : FFT table;generate from function fft_f_prepare 
 * return exponent.The rusult is (*p_dat_out)*(2^exponent);
 * description: 256 point fft/ifft 
 * -------------------------------------------------------------------------------------
 */

signed short fft_f_execute(
            const complex16 *p_dat_in,       //point to input buffer;
            complex16 *p_dat_out,            //point to output buffer;
            const unsigned char mode,        //0 fft; 1 IFFT