//------------------------------------------------------------------------------
//  データが実数の場合の FFT クラス，ver.1.1
//      FFT     複素共役になる部分は計算しない
//      IFFT    複素共役の部分を除いた値から計算する
//
//      このクラスでは以下の二つの Cortex-M4 の Intrinsic 命令を使用
//          __CLZ()     リーディング 0 の数を取得する命令に対応
//          __RBIT(k)   ビットの並びを逆にする命令に対応
//
//  2023/02/17 Copyright (c) 2023 MIKAMI, Naoki
//------------------------------------------------------------------------------

#include "fftReal.hpp"

namespace Mikami
{
    // コンストラクタ
    //      __CLZ(): リーディング 0 の数を取得する命令に対応
    FftReal::FftReal(int n)
            : N_FFT_(n), N_FFT_2_(n/2), N_INV_(1.0f/n),
              MASK_(n-1), N_SHIFT_(__CLZ(n)+1),
              wTable_(n/2), u_(n)
    {
        // n が 2 のべき乗か否かのチェック
        MBED_ASSERT((n << N_SHIFT_) == 0);

        // 回転子の表を作成
        Complex arg = Complex(0, -6.283185f/N_FFT_);
        for (int k=0; k<N_FFT_2_; k++)
            wTable_[k] = exp(arg*(float)k);
    }

    // FFT の実行
    //      有効な部分 y[0] ～ y[N_FFT_/2]
    //      それ以外の部分は複素共役になるので，計算をしていない
    void FftReal::Execute(const float x[], Complex y[])
    {
        // 最初のステージの処理
        for (int k=0; k<N_FFT_2_; k++)
        {
            // バタフライ演算
            int k2 = k+N_FFT_2_;
            u_[k2] = (x[k] - x[k2])*wTable_[k];
            u_[k] = x[k] + x[k2];
        }
  
        // 最初と最後のステージを除いた処理
        InnerStages();
  
        // ビット逆順の並べ替えを含む最終ステージ
        y[0] = u_[0] + u_[1];
        y[N_FFT_2_] = u_[0] - u_[1];
        for (int k=2; k<N_FFT_; k+=2)
            y[__RBIT(k) >> N_SHIFT_] = u_[k] + u_[k+1];
    }

    // IFFT の実行
    //      このクラスの Execute() で計算された結果の IFFT を計算する
    //      実行結果    x[0] ～ x[N_FFT_-1]
    void FftReal::ExecuteIfft(const Complex y[], float x[])
    {
        // 最初のステージの処理
        for (int k=0; k<N_FFT_2_; k++)
        {
            // バタフライ演算
            Complex yConj = conj(y[N_FFT_2_-k]);    // 後半は複素共役
            u_[k+N_FFT_2_] = (y[k] - yConj)*wTable_[k];
            u_[k] = y[k] + yConj;
        }

        // 最初と最後のステージを除いた処理
        InnerStages();

        // 最終ステージとビット逆順の並べ替え処理
        x[0] = N_INV_*(u_[0].real() + u_[1].real());
        x[N_FFT_2_] = N_INV_*(u_[0].real() - u_[1].real());
        for (int k=2; k<N_FFT_; k+=2)
        {
            float un  = u_[k].real();
            float un1 = u_[k+1].real();
            int index = -(__RBIT(k) >> N_SHIFT_) & MASK_;
            x[index]   = N_INV_*(un + un1);
            x[index-N_FFT_2_] = N_INV_*(un - un1);
        }
    }

    // 最初と最後のステージを除いた処理
    void FftReal::InnerStages()
    {
        int nHalf = N_FFT_2_ >> 1;
        for (int stg=2; stg<N_FFT_2_; stg*=2)
        {
            int nHalf2 = nHalf*2;
            for (int kp=0; kp<N_FFT_; kp+=nHalf2)
            {
                int kx = 0;
                for (int k=kp; k<kp+nHalf; k++)
                {
                    // バタフライ演算
                    Complex uTmp = u_[k+nHalf];
                    u_[k+nHalf] = (u_[k] - uTmp)*wTable_[kx];
                    u_[k] = u_[k] + uTmp;
                    kx += stg;
                }
            }
            nHalf = nHalf >> 1;
        }        
    }
}
