/*****************************************************************
fftfunc.c
fft利用のための関数群（fftcore.cより抜粋）
copyright (c) Mar2002 Toshihumi Kosaka
Ver 1.00 10June2002
*****************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "fftfunc.h"

static void *getmemory(size_t size);

static double *sintable;
static double *costable;
static int *bitrev;

static void *getmemory(size_t size)
{
    char *error="memory not available\n";
    void *ptr;
    ptr=malloc(size);
    if (ptr==NULL) {
        fprintf(stderr,error);
        exit(1);
    }
    return ptr;
}

void real2cmp(double r[],dcomplex_t c[],int size)
{
    int i;
    for (i=0;i<size;i++) {
        c[i].real=r[i];
        c[i].image=0.0;
    }
}

void hanning(dcomplex_t data[],int size)
{
    int i;
    for (i = 0; i<size; i++) {
        /*data[i].real*=0.5-0.5*cos(omg*i);*/
        data[i].real*=0.5-0.5*costable[i];
    }
}

void expand(dcomplex_t x[],int size)
/*expand frequency domain data using symmetry*/
{
    int half,i;
    half=size>>1;
    for (i = 1; i<half; i++) {
        x[size-i].real=x[i].real;
        x[size-i].image=-x[i].image;
    }
}

void rectwindow(dcomplex_t data[],int size,int lowerlimit,int upperlimit)
{
    int i;
    for (i = 0; i<lowerlimit; i++) {
        data[i].real=0.0;
        data[i].image=0.0;
    }
    for (i = upperlimit+1; i<size; i++) {
        data[i].real=0.0;
        data[i].image=0.0;
    }
}

void makepower(dcomplex_t s[],double pow[],int size,int mode)
/* s: spectrum          (input) */
/* pow: power spectrum (output) */
/* size:size of data */
/* mode -1:clear&calc, 0:calc&add, 0<m:calc,add&normarize by m */
{
    int i;
    double div;
    
    if (mode==-1) {
        for (i=0; i<size; i++) pow[i]=0.0;
    }
    for (i = 0; i<size; i++) {
        pow[i]+=s[i].real*s[i].real+s[i].image*s[i].image;
    }
    if (0<mode) {
        div=1./(double)mode;
        for (i = 0; i<size; i++) {
            pow[i]*=div;
        }
    }
}

static int staticsize;

void fft(int cdir,dcomplex_t x[],int size)
/* ==============================================
:                                               :
:   Fast Fourier Transform                      :
:   cdir=1 ......  normal FFT                   :
:   cdir=-1 .....  inverse FFT                  :
:   cdir=-2 .....  inverse FFT(1/FFTSIZE)       :
:   cdir=2 ......  normal FFT(*FFTSIZE)         :
:                                               :
=============================================== */
{
    int npow=0,tmp=1,mask1;
    int i,j,l,le,le1,ip,omg,unit;
    double wr,wi,tr,ti,sr,si;
    double *xri,*xrj,*xii,*xij;
    double dvd;
    dcomplex_t ctmp;
    
    if (staticsize!=size) {
        printf("The size is different from the size defined in initfft function\n");
        exit(1);
    }
    dvd=1.0/(double)size;
    while (tmp<size) {
        tmp<<=1;
        npow++;
    }
    mask1=size-1;

    #ifdef DEBUG
        printf("fft\n");
    #endif
    #ifdef DEBUG
        for (i = 0; i<size; i++) printf("%6f %6f   ",x[i].real,x[i].image);
        printf("\n");
    #endif
    for (l=1;l<=npow;l++){
        le=2<<(npow-l);
        le1=le>>1;
        unit=1<<(l-1);
        omg=0;
        for (ip=0;ip<le1;ip++){
            wr=costable[(omg)&mask1];
            /*wr=indexcos(omg);*/
            wi=sintable[omg&mask1];
            /*wi=indexsin(omg);*/
            if (cdir>0) wi=-wi;
            for (i=ip;i<size;i+=le){
                j=i+le1;
                tr=*(xri=&x[i].real)+*(xrj=&x[j].real);
                ti=*(xii=&x[i].image)+*(xij=&x[j].image);
                sr=*xri-*xrj;
                si=*xii-*xij;
                *xri=tr;
                *xii=ti;
                *xrj=sr*wr-si*wi;
                *xij=sr*wi+si*wr;
            }
            omg+=unit;
        }
    }

    /*  bit reversal  */
    for (i=1;i<size-1;i++){
        j=bitrev[i];
        if (i<j){
            ctmp=x[j];
            x[j]=x[i];
            x[i]=ctmp;
        }
    }
    
    /* shift */
    if ((cdir==1)||(cdir==-2)){
        for (i = 0; i<size; i++){
            x[i].real*=dvd;
            x[i].image*=dvd;
        }
    }
}

/*FFT演算ルーチンの初期化*/
void initfft(int fftsize)
{
    int i,nv2,j,k;
    
    sintable=getmemory(fftsize*sizeof(double));
    costable=getmemory(fftsize*sizeof(double));
    bitrev=getmemory(fftsize*sizeof(int));

    for (i=0;i<fftsize;i++) {
        sintable[i]=sin((double)i*2.0*M_PI/(double)fftsize);
        costable[i]=cos((double)i*2.0*M_PI/(double)fftsize);
    }
    
    nv2=fftsize>>1;
    j=0;
    for (i=1;i<fftsize-1;i++){
        k=nv2;
        while (k<=j){
            j-=k;
            k>>=1;
        }
        j+=k;
        bitrev[i]=j;
    }
    bitrev[0]=0;
    bitrev[fftsize-1]=fftsize-1;
    staticsize=fftsize;
}

void postfft(void)
{
    free(sintable);
    free(costable);
    free(bitrev);
}

void calcPowerspectrum(double timedomain[],int size,long int samplingfrequency,powerspec_t powerspec[])
/*
与えられた時間領域データからパワースペクトルを計算する
入力
double        timedomain[]:時間領域データ
int                   size:時間領域のデータ数
long int samplingfrequency:サンプリング周波数
出力
powerspec_t powerspec[]:パワースペクトル格納領域，サイズはsize/2+1でよい
利用上の注意
sizeは2のべき乗の値であること。128,256,512,1024,2048,4096
すべての領域は呼び出し側で用意すること
*/
{
    int msize=size;
    int check=~1;
    dcomplex_t *work;
    double *power;
    int i;
    if (size==0) return;
    while (msize&check) {
        msize&=check;
        check<<=1;
    }
    initfft(msize);
    work=(dcomplex_t *)getmemory(msize*sizeof(dcomplex_t));
    power=(double *)getmemory((msize/2+1)*sizeof(double));
    real2cmp(timedomain,work,msize);
    fft(1,work,msize);
    makepower(work,power,msize/2+1,-1);
    for (i=0;i<msize/2+1;i++) {
        powerspec[i].frequency=(int)(samplingfrequency*(double)i/msize);
        powerspec[i].power=power[i];
    }
    for (;i<size/2+1;i++) {
        powerspec[i].frequency=0;
        powerspec[i].power=0.0;
    }
    postfft();
    free(work);
    free(power);
}