
/************************************************************
**
**  Object name :
**
**  History :
**     2013.05.14 M.Takigahira Build有無指定に対応
**     2013.05.07 M.Takigahira 窓関数の三角関数と乗算回数の削減
**     2013.03.22 K.Takagi     初期化時の係数計算をループの外で計算
**     2012.09.21 K.Takagi     新規作成
**
**  Description
**  -----------
**  マルチチャネルのデータ配列のFFT
**
**  FFTはHARKのフロー上で複数使用される場合がある
**  そのため窓関数は本モジュール内では保持しない
**
**  Copyright (c) 2012
**  System In Frontier,Inc.
**  All rights reserved.
**
**  当社の許可なしに複製および転載を禁ず
**
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

///////////////////////////////////////////////////////////////
//  Include Files
#include "build_modules.h"
#include "HA_log.h"

#include "MultiFFT.h"

///////////////////////////////////////////////////////////////
//  Define Macros
#ifndef M_PI /* math.h */
#define M_PI (3.141592653589793238462643383279)
#endif

#if !defined BUILD_MODULE_MULTI_FFT
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
void SetFFTWindowGain(float gain){ }
static void CalcRectangleWindow(float *w, int length){ }
static void CalcHanningWindow(float *w, int length){ }
static void CalcHammingWindow(float *w, int length){ }
void CalcConjWindow(float *w, int length){ }
void MultiFFT(MULTI_FFT_INFO_t *info, COMPLEX_FLOAT *out_data[], float *in_data[], int channels){ }
static int CheckWindowType(const char *window_name){ return 0; }
int ReleaseMultiFFT(MULTI_FFT_INFO_t *info){ return 0; }
MULTI_FFT_INFO_t *InitMultiFFT(const char *window_name, int length){ return 0; }
#ifdef STANDALON_TEST
int main(int argc, char **argv){ return 0; }
#endif // STANDALON_TEST
#else // #if defined BUILD_MODULE_MULTI_FFT
///////////////////////////////////////////////////////////////
//  Parameter
static float fft_window_gain = 1.0;

///////////////////////////////////////////////////////////////
//  Local Buffer

///////////////////////////////////////////////////////////////
//  Clear

///////////////////////////////////////////////////////////////
//  Initialize

///////////////////////////////////////////////////////////////
//  SetParam
/************************************************************
**  History :
**    2012.10.12 K.Takagi 新規作成
*/
// Public : 窓関数のゲインを設定　入力信号へのゲインを窓関数の処理でまとめる
void SetFFTWindowGain(float gain)
{
    if(gain != 0.0){
        fft_window_gain = gain;
    }
}

///////////////////////////////////////////////////////////////
//  MAIN
/************************************************************
**  History :
**    2012.10.12 K.Takagi ゲイン計算を追加
**    2012.09.21 K.Takagi 新規作成
*/
// Public : 矩形窓の作成
static void CalcRectangleWindow(float *w, int length)
{
    int n;
    for(n=0; n<length; n++){
        w[n] = (float) fft_window_gain; // w[n] = 1.0 * fft_window_gain; 無駄な乗算を削除（念のためcast）
    }
}

/************************************************************
**  History :
**    2013.05.07 M.Takigahira 演算量の削減、
**    2012.10.12 K.Takagi ゲイン計算を追加
**    2012.09.21 K.Takagi 新規作成
*/
// Public : ハニング窓の作成
static void CalcHanningWindow(float *w, int length)
{
    int n;
    //float c = 2.0 * M_PI / (length - 1);
    //for(n=0; n<length; n++){
    //    w[n] = (0.5 - 0.5 * cos(c * n)) * fft_window_gain;
    //}

    //// w[n] = (0.5-0.5*cos(c*n)) * fft_window_gain
    ////      = (1-cos(c*n))/2 * fft_window_gain
    ////      = (sin^2(c*n/2)) * fft_window_gain
    ////      = [if c*=0.5 then] (sin^2(c*n)) * fft_window_gain
    const float c = (float) M_PI / (length - 1);
    float w_n;
    for(n=0; n<length; n++){
    	w_n = sin(c * n); // sin(x)
    	w_n *= w_n; // sin^2(x)
    	w[n] = w_n * fft_window_gain;
    }
}

/************************************************************
**  History :
**    2012.10.12 K.Takagi ゲイン計算を追加
**    2012.09.21 K.Takagi 新規作成
*/
// Public : ハミング窓の作成
static void CalcHammingWindow(float *w, int length)
{
    int n;
    const float c = 2.0 * M_PI / (length - 1);
    for(n=0; n<length; n++){
        w[n] = (0.54 - 0.46 * cos(c * n)) * fft_window_gain;
    }
}

/************************************************************
**  History :
**    2013.05.07 M.Takigahira 三角関数演算回数と乗算回数の削減、不要なloop内ifを排除、メモリアクセス効率の向上
**    2012.10.12 K.Takagi ゲイン計算を追加
**    2012.09.21 K.Takagi 新規作成
*/
// Public : 複素窓の作成
void CalcConjWindow(float *w, int length)
{
    int i;
    float sum, w_i;
    //for(i=0; i<length; i++){
    //    int inv=0;
    //    float x = (4 * i) / (float)length;
    //    if(x < 1){                          }
    //    else if(x < 2){ x = 2 - x; inv = 1; }
    //    else if(x < 3){ x = x - 2; inv = 1; }
    //    else{           x = 4 - x;          }
    //    x *= 1.9979;
    //    w[i] = (0.5 - 0.5 * cos(x)) * (0.5 - 0.5 * cos(x));
    //    if(inv){ w[i] = 1 - w[i]; }
    //    w[i] = sqrt(w[i]) * fft_window_gain;
    //}

    //// if(inv==0) then following.
    //// w[i] = (0.5-0.5*cos(x))*(0.5-0.5*cos(x)) = (1-cos(x)/2)^2 = sin^4(x/2)
    ////      = [if x*=0.5 then] sin^4(x) << 三角関数と乗算回数の削減 >>
    //// w[i] = sqrt(w[i]) * fft_window_gain = sqrt(sin^4(x)) * fft_window_gain
    ////      = sin^2(x) * fft_window_gain << sqrtの排除 >>
    //// if(inv==1) then following.
    //// w[i] = (0.5-0.5*cos(x))*(0.5-0.5*cos(x)) = (1-cos(x)/2)^2 = sin^4(x/2)
    //// w[i] = 1-w[i] = 1-sin^4(x/2)
    ////      = [if x*=0.5 then] 1-sin^4(x) << 三角関数と乗算回数の削減 >>
    //// w[i] = sqrt(w[i]) * fft_window_gain = sqrt(1-sin^4(x)) * fft_window_gain

    const int thresh_a = (int)((float)length * 0.25f);
    const int thresh_b = (int)((float)length * 0.50f);
    const int thresh_c = (int)((float)length * 0.75f);
    const float inc = 4.0f / (float)length;

    i=0;
    sum=0.0f;
    for(; i<thresh_a; i++, sum+=inc){ // s<1, case of inv==0 ...
        w_i = sin(sum * (1.9979 * 0.5)); // sin(x)
        w_i *= w_i;  // sin^2(x)
        w[i] = w_i * fft_window_gain;
    }
    for(; i<thresh_b; i++, sum+=inc){ // s<2, case of inv==1 ...
    	w_i = sin((2.0f - sum) * (1.9979 * 0.5)); // sin(x)
        w_i *= w_i;  // sin^2(x)
        w_i *= w_i;  // sin^4(x)
        w[i] = sqrt(1 - w_i) * fft_window_gain;
    }
    for(; i<thresh_c; i++, sum+=inc){ // s<3, case of inv==1 ...
    	w_i = sin((sum - 2.0f) * (1.9979 * 0.5)); // sin(x)
    	w_i *= w_i;  // sin^2(x)
    	w_i *= w_i;  // sin^4(x)
    	w[i] = sqrt(1 - w_i) * fft_window_gain;
    }
    for(; i<length; i++, sum+=inc){ // s for the rest, case of inv==0 ...
    	w_i = sin((4.0f - sum) * (1.9979 * 0.5)); // sin(x)
        w_i *= w_i;  // sin^2(x)
        w[i] = w_i * fft_window_gain;
    }
}

/************************************************************
**  History :
**    2012.09.21 K.Takagi 新規作成
*/
// Public : FFT
void MultiFFT(MULTI_FFT_INFO_t *info, COMPLEX_FLOAT *out_data[], float *in_data[], int channels)
{
    int ch;
    int index;
    int length;
    int length_2;
    int length_4;
    
    COMPLEX_FLOAT y; // This variable use as a temporary variable in the scrambling process.
    COMPLEX_FLOAT f1;
    COMPLEX_FLOAT f2;
    
    int i,j,k;
    int m, mh;
    int irev, prev_irev;

#ifdef MLTFFT_4BASIS_ENABLE
    int f_prev_irev1, f_prev_irev2, f_irev;
    int s_prev_irev, s_irev;
    COMPLEX_FLOAT y1;
    COMPLEX_FLOAT y2;
    COMPLEX_FLOAT tj1;
    COMPLEX_FLOAT tj2;
    COMPLEX_FLOAT tk1;
    COMPLEX_FLOAT tk2;
    int j1, j2, k1, k2;
#endif
    
    length   = info->length;
    length_2 = info->length / 2;
    length_4 = info->length / 4;
    
    for(ch=0; ch<channels; ch++){
        /* 窓関数をかける */
        for(index=0; index<length; index++){
            info->rfft.a[index] = in_data[ch][index] * info->window[index];
        }
        
        for(index=0; index<length_2; index++){
            info->fft.a[index].real = info->rfft.a[index*2];
            info->fft.a[index].imag = info->rfft.a[index*2+1];
//            info->fft.a[index].real = in_data[ch][index*2]   * info->window[index*2];
//            info->fft.a[index].imag = in_data[ch][index*2+1] * info->window[index*2+1];
        }
        
        /* データスワップ */
        i = 0;
        for(j=1; j<length_2-1; j++){
            for(k=length_2>>1; k>(i^=k); k>>=1);
            if(j < i){
                y.real = info->fft.a[i].real;
                y.imag = info->fft.a[i].imag;
                info->fft.a[i].real = info->fft.a[j].real;
                info->fft.a[i].imag = info->fft.a[j].imag;
                info->fft.a[j].real = y.real;
                info->fft.a[j].imag = y.imag;
            }
        }
        
        /* FFT */
#ifdef MLTFFT_4BASIS_ENABLE
	int logN = log2(length_2);
	if(logN % 2 != 0){
            /* 2basis */
            for(mh=1; (m=mh<<1)<=length_2; mh=m){
                irev = 0;
                for(i=0; i<length_2; i+=m){
                    prev_irev = irev;
                    for(k=length_2>>2; k>(irev^=k); k>>=1);
                    for(j=i; j<mh + i; j++){
                        k = j + mh;
                        y.real = info->fft.a[j].real - info->fft.a[k].real;
                        y.imag = info->fft.a[j].imag - info->fft.a[k].imag;
                    
                        info->fft.a[j].real += info->fft.a[k].real;
                        info->fft.a[j].imag += info->fft.a[k].imag;
                    
                        info->fft.a[k].real = info->fft.w[prev_irev].real * y.real - info->fft.w[prev_irev].imag * y.imag;
                        info->fft.a[k].imag = info->fft.w[prev_irev].real * y.imag + info->fft.w[prev_irev].imag * y.real;
                    }
                }
            }	
	}else{
	    /* 4basis */
            for(mh=1; (m=mh<<2)<=length_2; mh=m){
                f_irev = 0;
                s_irev = 0;
                for(i=0; i<length_2; i+=m){
                    f_prev_irev1 = f_irev;
                    for(k=length_2>>2; k>(f_irev^=k); k>>=1);
                    f_prev_irev2 = f_irev;
                    for(k=length_2>>2; k>(f_irev^=k); k>>=1);
                    s_prev_irev = s_irev;
                    for(k=length_2>>2; k>(s_irev^=k); k>>=1);

                    for(j=i; j<mh + i; j++){
                        j1 = j;
                        j2 = j + 2*mh;
                        k1 = j1 + mh;
                        k2 = j2 + mh;
                    
                        tj1 = info->fft.a[j1];
                        tk1 = info->fft.a[k1];
                        tj2 = info->fft.a[j2];
                        tk2 = info->fft.a[k2];

                        y1.real = tj1.real - tk1.real;
                        y1.imag = tj1.imag - tk1.imag;
                        y2.real = tj2.real - tk2.real;
                        y2.imag = tj2.imag - tk2.imag;
                    
                        tj1.real += tk1.real;
                        tj1.imag += tk1.imag;
                        tj2.real += tk2.real;
                        tj2.imag += tk2.imag;
                    
                        tk1.real = info->fft.w[f_prev_irev1].real * y1.real - info->fft.w[f_prev_irev1].imag * y1.imag;
                        tk1.imag = info->fft.w[f_prev_irev1].real * y1.imag + info->fft.w[f_prev_irev1].imag * y1.real;
                        tk2.real = info->fft.w[f_prev_irev2].real * y2.real - info->fft.w[f_prev_irev2].imag * y2.imag;
                        tk2.imag = info->fft.w[f_prev_irev2].real * y2.imag + info->fft.w[f_prev_irev2].imag * y2.real;

                        y1.real = tj1.real - tj2.real;
                        y1.imag = tj1.imag - tj2.imag;
                        y2.real = tk1.real - tk2.real;
                        y2.imag = tk1.imag - tk2.imag;

                        info->fft.a[j1].real = tj1.real + tj2.real;
                        info->fft.a[j1].imag = tj1.imag + tj2.imag;
                        info->fft.a[k1].real = tk1.real + tk2.real;
                        info->fft.a[k1].imag = tk1.imag + tk2.imag;

                        info->fft.a[j2].real = info->fft.w[s_prev_irev].real * y1.real - info->fft.w[s_prev_irev].imag * y1.imag;
                        info->fft.a[j2].imag = info->fft.w[s_prev_irev].real * y1.imag + info->fft.w[s_prev_irev].imag * y1.real;
                        info->fft.a[k2].real = info->fft.w[s_prev_irev].real * y2.real - info->fft.w[s_prev_irev].imag * y2.imag;
                        info->fft.a[k2].imag = info->fft.w[s_prev_irev].real * y2.imag + info->fft.w[s_prev_irev].imag * y2.real;
                    }
                }
            }
	}
#else
        for(mh=1; (m=mh<<1)<=length_2; mh=m){
            irev = 0;
            for(i=0; i<length_2; i+=m){
                prev_irev = irev;
                for(k=length_2>>2; k>(irev^=k); k>>=1);
                for(j=i; j<mh + i; j++){
                    k = j + mh;
                    y.real = info->fft.a[j].real - info->fft.a[k].real;
                    y.imag = info->fft.a[j].imag - info->fft.a[k].imag;
                    
                    info->fft.a[j].real += info->fft.a[k].real;
                    info->fft.a[j].imag += info->fft.a[k].imag;
                    
                    info->fft.a[k].real = info->fft.w[prev_irev].real * y.real - info->fft.w[prev_irev].imag * y.imag;
                    info->fft.a[k].imag = info->fft.w[prev_irev].real * y.imag + info->fft.w[prev_irev].imag * y.real;
                }
            }
        }
#endif
        
        for(k=1; k<length_4; k++){
            f1.real = 0.5 * (1.0 - info->rfft.w[k].imag);
            f1.imag = 0.5 * (      info->rfft.w[k].real);
            
            f2.real = info->fft.a[k].real - info->fft.a[length_2-k].real;
            f2.imag = info->fft.a[k].imag + info->fft.a[length_2-k].imag;
            
            y.real = f1.real * f2.real - f1.imag * f2.imag;
            y.imag = f1.real * f2.imag + f1.imag * f2.real;
            
            info->rfft.A[k].real = info->fft.a[k].real - y.real;
            info->rfft.A[k].imag = info->fft.a[k].imag - y.imag;
            
            info->rfft.A[length_2 - k].real = info->fft.a[length_2 - k].real + y.real;
            info->rfft.A[length_2 - k].imag = info->fft.a[length_2 - k].imag - y.imag;
        }
        info->rfft.A[0].real = info->fft.a[0].real + info->fft.a[0].imag;
        info->rfft.A[0].imag = 0;
        info->rfft.A[length_2].real = info->fft.a[0].real - info->fft.a[0].imag;
        info->rfft.A[length_2].imag = 0;
        info->rfft.A[length_4].real =  info->fft.a[length_4].real;
        info->rfft.A[length_4].imag = -info->fft.a[length_4].imag;

        /* 出力バッファにデータをコピー */
        for(index=0; index<length/2+1; index++){
            out_data[ch][index].real = info->rfft.A[index].real;
            out_data[ch][index].imag = info->rfft.A[index].imag;
        }
    }
}

/************************************************************
**  History :
**    2012.09.21 K.Takagi 新規作成
*/
// Private : 窓の種類の判定
#define WINDOW_TYPE_ERROR     0
#define WINDOW_TYPE_RECTANGLE 1
#define WINDOW_TYPE_HAMMING   2
#define WINDOW_TYPE_HANNING   3
#define WINDOW_TYPE_CONJ      4

static int CheckWindowType(const char *window_name)
{
    if(strcmp(window_name, "RECTANGLE") == 0){ return WINDOW_TYPE_RECTANGLE; }
    if(strcmp(window_name, "HAMMING")   == 0){ return WINDOW_TYPE_HAMMING;   }
    if(strcmp(window_name, "HANNING")   == 0){ return WINDOW_TYPE_HANNING;   }
    if(strcmp(window_name, "CONJ")      == 0){ return WINDOW_TYPE_CONJ;      }
    return WINDOW_TYPE_ERROR;
}

/************************************************************
**  History :
**    2012.10.10 K.Takagi 開放処理を変更
**    2012.09.21 K.Takagi 新規作成
*/
// Public : FFT関係のバッファ等を開放
int ReleaseMultiFFT(MULTI_FFT_INFO_t *info)
{
    if(info == NULL){ return 0; }
    
    /* バッファの開放 */
    free(info->window);
    
    free(info->rfft.w);
    free(info->rfft.a);
    free(info->rfft.A);
    
    free(info->fft.w);
    free(info->fft.a);
    
    free(info);
    
    return 0;
}

/************************************************************
**  History :
**    2013.03.22 K.Takagi 係数計算をループの外で計算
**    2012.10.10 K.Takagi 戻り値と引数を変更
**                        構造体を内部で確保するように変更
**    2012.09.21 K.Takagi 新規作成
*/
// Public : FFT関係の初期化
MULTI_FFT_INFO_t *InitMultiFFT(const char *window_name, int length)
{
    int index;
    float coef;
    
    MULTI_FFT_INFO_t *info;
    
    info = (MULTI_FFT_INFO_t *)malloc(sizeof(MULTI_FFT_INFO_t));
    if(info == NULL){ return NULL; }
    bzero(info, sizeof(MULTI_FFT_INFO_t));
    
    
    info->length = length;
    info->window_type = CheckWindowType(window_name);
    if(info->window_type == WINDOW_TYPE_ERROR){ return NULL; }
    
    /* バッファ確保 */
    info->window = (float *)malloc(sizeof(float) * length);
    
    info->rfft.w = (COMPLEX_FLOAT *)malloc(sizeof(COMPLEX_FLOAT) * length);
    info->rfft.a = (float *)malloc(sizeof(float) * length);
    info->rfft.A = (COMPLEX_FLOAT *)malloc(sizeof(COMPLEX_FLOAT) * (length/2+1));

    info->fft.w = (COMPLEX_FLOAT *)malloc(sizeof(COMPLEX_FLOAT) * length/2);
    info->fft.a = (COMPLEX_FLOAT *)malloc(sizeof(COMPLEX_FLOAT) * length/2);
    
    if( (info->window == NULL) || 
        (info->rfft.w == NULL) || 
        (info->rfft.a == NULL) || 
        (info->rfft.A == NULL) ||
        (info->fft.w == NULL)  || 
        (info->fft.a == NULL)  ){
        goto InitErrEnd;
    }
    
    /* 窓関数を計算 */
    switch(info->window_type){
        case WINDOW_TYPE_RECTANGLE: CalcRectangleWindow(info->window, info->length); break;
        case WINDOW_TYPE_HAMMING:   CalcHammingWindow(info->window, info->length);   break;
        case WINDOW_TYPE_HANNING:   CalcHanningWindow(info->window, info->length);   break;
        case WINDOW_TYPE_CONJ:      CalcConjWindow(info->window, info->length);      break;
        default:
        {
            /* ここにくることはない */
            goto InitErrEnd;
        }
        break;
    }
    
    /* 複素テーブルを計算 */
    //for(index=0; index<length; index++)
    for(index=0, coef=2.0*M_PI/length; index<length; index++){
        //info->rfft.w[index].real =  cos(2.0 * M_PI * index / length);
        //info->rfft.w[index].imag = -sin(2.0 * M_PI * index / length);
        info->rfft.w[index].real =  cos(coef * index);
        info->rfft.w[index].imag = -sin(coef * index);
    }
    
    //for(index=0; index<length/2; index++)
    for(index=0, coef=4.0*M_PI/length; index<(length/2); index++){
        //info->fft.w[index].real =  cos(2.0 * M_PI * index / (length/2));
        //info->fft.w[index].imag = -sin(2.0 * M_PI * index / (length/2));
        info->fft.w[index].real =  cos(coef * index);
        info->fft.w[index].imag = -sin(coef * index);
    }
    
    return info;

InitErrEnd:
    /* バッファの開放 */
    free(info->window);
    
    free(info->rfft.w);
    free(info->rfft.a);
    free(info->rfft.A);
    
    free(info->fft.w);
    free(info->fft.a);
    
    free(info);

    return NULL;
}

#ifdef STANDALON_TEST
/************************************************************
**  History :
**    2012.09.21 K.Takagi 新規作成
*/
// Public : 単体テストのメイン　各種windowデータの計算を確認
int main(int argc, char **argv)
{
    float window[512];
    int length;
    int type;
    int index;
    FILE *fp;
    
    length = 512;
    
    if(argc != 2){
        printf("Usage : %s <type>\n", argv[0]);
        printf("  type = 0 : RECTANGLE WINDOW\n");
        printf("  type = 1 : CONJ WINDOW\n");
        printf("  type = 2 : HAMMING WINDOW\n");
        printf("  type = 3 : HANNING WINDOW\n");
        return -1;
    }
    
    type = atoi(argv[1]);
    
    switch(type){
        case 0: CalcRectangleWindow(window, length); break;
        case 1: CalcConjWindow(window, length);      break;
        case 2: CalcHammingWindow(window, length);   break;
        case 3: CalcHanningWindow(window, length);   break;
        default:
        {
            printf("unknown type\n");
            return -1;
        }
        break;
    }
    
    fp = fopen("window.dat", "wt");
    if(fp != NULL){
        for(index=0; index<length; index++){
            fprintf(fp, "%f\n", window[index]);
        }
        fclose(fp);
    }
    
    {
        MULTI_FFT_INFO_t *info;
        int index;
        
        float *data[2];
        float data1[512];
        float data2[512];
        
        COMPLEX_FLOAT *f_data[2];
        COMPLEX_FLOAT f_data1[512];
        COMPLEX_FLOAT f_data2[512];
        
        data[0] = data1;
        data[1] = data2;
        
        f_data[0] = f_data1;
        f_data[1] = f_data2;
        
        //info = InitMultiFFT("RECTANGLE", 512);
        //info = InitMultiFFT("HAMMING", 512);
        info = InitMultiFFT("HANNING", 512);
        if(info == NULL){
            printf("InitMultiFFT Error.\n");
            goto MultiFFTEnd;
        }
        
        for(index=0; index<512; index++){
            data1[index] = 20000 * sin(2.0 * M_PI * index / 16);
            data2[index] = 20000 * sin(2.0 * M_PI * index / 8);
        }
        
        MultiFFT(info, f_data, data, 2);
/*
        printf("== data1 re ==\n");
        for(index=0; index<512/2+1; index++){
            printf("%f\n", f_data1[index].real);
        }
        printf("== data1 im ==\n");
        for(index=0; index<512; index++){
            printf("%f\n", f_data1[index].imag);
        }
        printf("== data2 re ==\n");
        for(index=0; index<512; index++){
            printf("%f\n", f_data2[index].real);
        }
        printf("== data2 im ==\n");
        for(index=0; index<512; index++){
            printf("%f\n", f_data2[index].imag);
        }
*/
MultiFFTEnd:
        ReleaseMultiFFT(info);
    }
    return 0;
}
#endif // STANDALON_TEST
#endif // BUILD_MODULE_MULTI_FFT

/* EOF */
