// ******************************************************************
// Copyright (c) 2002- Satoshi, All Rights Reserved.
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
//
// Author : Satoshi ( af230533@im07.alpha-net.ne.jp )
// ******************************************************************

//***********************************************************************
// 高速フーリエ変換（ＦＦＴ）
// 更にスピード・アップするにはＣＯＳを固定で持つこと！
//
// Copyright (C) Satoshi 1994-2002 All rights reserved.
// ***********************************************************************

#include <math.h>

#define       FFT_TEST_COUNT      500                 // Bench FFT

// ----------------------------------------------------- FFT
#define       OBJ_DATA_COUNT      128
#define       OBJ_DATA_SISU       7                   // 128 = 2 ** 7
#define       OBJ_DATA_SLIDE      1

#define       FFT_TRN             1
#define       IFFT_TRN            -1

typedef  struct  _fft {
        int         N;              // デジタル・データ件数
        int         r;              // N = 2^r
        double*     result_A;       // サンプリングデータをここにセットする
                                    // cos 成分…求めるスペクトル成分の数分の領域が必要
        double*     result_B;       // sin 成分…求めるスペクトル成分の数分の領域が必要
} FFT;
#define         FFT_SIZE            sizeof( FFT )


void digital_fft( FFT* fft );


double        SpectA[OBJ_DATA_COUNT];
double        SpectB[OBJ_DATA_COUNT];

double        TestData[]  = {
                               0.998795456205172405,
                               0.995184726672196929,
                               0.146735474455360860,
                               0.098217140329559660,
                               0.980784545503230431,
                               0.970031253194543974,
                               0.956940335252408824,
                              -0.857728610000272118,
                              -0.831465612302545236,
                              -0.803205431480644943,
                              -0.774010453362736882,
                              -0.747954125354958995,
                              -0.707116781186547351,
                              -0.671125754847018219,
                              -0.634394284163645266,
                              -0.594619304492433024,
                              -0.555545233019601845,
                               0.941544045483020806,
                               0.923879532511286738,
                               0.903989293123443338,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.831469612544545236,
                               0.803207531420452543,
                               0.773010453362736882,
                               0.740451125354958995,
                               0.707106781186547351,
                              -0.974034153194543974,
                              -0.956940335732208824,
                              -0.944144065183020806,
                              -0.923211532511286738,
                              -0.905989293123443338,
                              -0.881112264348355050,
                              -0.857728610000272118,
                               0.671558954847018219,
                               0.049167674327417023,
                              -0.001212000000001049,
                              -0.998791456205172405,
                              -0.995214726672196929,
                              -0.989176509964781014,
                              -0.980782180403230431,
                              -0.974034153194543974,
                              -0.956940335732208824,
                              -0.944144065183020806,
                              -0.923211532511286738,
                              -0.905989293123443338,
                               0.803207531420452543,
                               0.773010453362736882,
                               0.740451125354958995,
                               0.707106781186547351,
                               0.671558954847018219,
                               0.989576509964781014,
                               0.980784545503230431,
                               0.970031253194543974,
                               0.654634123783645266,
                               0.634646284163645266,
                               0.595624504492433024,
                               0.555570245019601845,
                               0.514442744193221328,
                               0.471356736825997198,
                               0.424551093430281585,
                               0.314683432365089171,
                              -0.881112264348355050,
                              -0.857728610000272118,
                              -0.831465612302545236,
                              -0.803205431480644943,
                              -0.774010453362736882,
                              -0.747954125354958995,
                              -0.707116781186547351,
                              -0.671125754847018219,
                              -0.634394284163645266,
                              -0.594619304492433024,
                              -0.555545233019601845,
                              -0.514102744193221328,
                              -0.477396736825997198,
                              -0.477555093430281585,
                              -0.387688432365089171,
                              -0.335879853392219440,
                              -0.295878677254461665,
                               0.903989293123443338,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.831469612544545236,
                               0.803207531420452543,
                               0.773010453362736882,
                               0.740451125354958995,
                              -0.242980179903263122,
                              -0.195057822016127443,
                              -0.146775474455360860,
                              -0.098897540329559660,
                              -0.042866864327417023,
                               0.998795456205172405,
                               0.995184726672196929,
                               0.989576509964781014,
                               0.980784545503230431,
                               0.970031253194543974,
                               0.956940335252408824,
                               0.941544045483020806,
                               0.923879532511286738,
                              -0.001212000000001049,
                              -0.998791456205172405,
                              -0.995214726672196929,
                              -0.989176509964781014,
                              -0.980782180403230431,
                              -0.974034153194543974,
                               0.707106781186547351,
                               0.671558954847018219,
                               0.654634123783645266,
                               0.634646284163645266,
                               0.595624504492433024,
                               0.555570245019601845,
                               0.514442744193221328,
                               0.471356736825997198,
                               0.424551093430281585,
                               0.314683432365089171,
                               0.336441853392219440,
                               0.290284654254461665,
                               0.242980479903263122,
                               0.195094322016127443,
                               0.146735474455360860,
                               0.098217140329559660,
                               0.049167674327417023,
                              -0.956940335732208824,
                              -0.944144065183020806,
                              -0.923211532511286738,
                              -0.905989293123443338,
                              -0.881112264348355050,
                              -0.514102744193221328,
                              -0.477396736825997198,
                              -0.477555093430281585,
                              -0.387688432365089171,
                              -0.335879853392219440,
                              -0.295878677254461665,
                              -0.242980179903263122,
                              -0.195057822016127443,
                              -0.146775474455360860,
                              -0.098897540329559660,
                              -0.042866864327417023,
                               0.998795456205172405,
                               0.995184726672196929,
                               0.989576509964781014,
                               0.654634123783645266,
                               0.634646284163645266,
                               0.595624504492433024,
                               0.555570245019601845,
                               0.514442744193221328,
                              -0.001212000000001049,
                              -0.998791456205172405,
                              -0.995214726672196929,
                              -0.989176509964781014,
                              -0.980782180403230431,
                              -0.831465612302545236,
                              -0.803205431480644943,
                              -0.774010453362736882,
                              -0.747954125354958995,
                              -0.707116781186547351,
                              -0.671125754847018219,
                               0.471356736825997198,
                               0.424551093430281585,
                               0.314683432365089171,
                               0.336441853392219440,
                               0.740451125354958995,
                               0.707106781186547351,
                               0.903989293123443338,
                               0.471356736825997198,
                               0.998795456205172405,
                               0.995184726672196929,
                               0.956940335252408824,
                               0.941544045483020806,
                               0.923879532511286738,
                               0.903989293123443338,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.831469612544545236,
                               0.336441853392219440,
                               0.290284654254461665,
                               0.242980479903263122,
                               0.195094322016127443,
                               0.146735474455360860,
                               0.098217140329559660,
                               0.049167674327417023,
                              -0.001212000000001049,
                              -0.998791456205172405,
                              -0.995214726672196929,
                              -0.989176509964781014,
                              -0.980782180403230431,
                              -0.974034153194543974,
                              -0.956940335732208824,
                              -0.944144065183020806,
                              -0.923211532511286738,
                              -0.905989293123443338,
                               0.803207531420452543,
                               0.773010453362736882,
                               0.740451125354958995,
                               0.707106781186547351,
                               0.671558954847018219,
                               0.989576509964781014,
                               0.980784545503230431,
                               0.970031253194543974,
                               0.654634123783645266,
                               0.634646284163645266,
                               0.595624504492433024,
                               0.555570245019601845,
                               0.514442744193221328,
                               0.471356736825997198,
                               0.424551093430281585,
                               0.314683432365089171,
                              -0.881112264348355050,
                              -0.857728610000272118,
                              -0.831465612302545236,
                              -0.803205431480644943,
                              -0.774010453362736882,
                              -0.747954125354958995,
                              -0.707116781186547351,
                              -0.671125754847018219,
                              -0.634394284163645266,
                              -0.594619304492433024,
                              -0.555545233019601845,
                              -0.514102744193221328,
                              -0.477396736825997198,
                              -0.477555093430281585,
                              -0.387688432365089171,
                              -0.335879853392219440,
                              -0.295878677254461665,
                               0.903989293123443338,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.831469612544545236,
                               0.803207531420452543,
                               0.773010453362736882,
                               0.740451125354958995,
                              -0.242980179903263122,
                              -0.195057822016127443,
                              -0.146775474455360860,
                              -0.098897540329559660,
                              -0.042866864327417023,
                               0.998795456205172405,
                               0.995184726672196929,
                               0.989576509964781014,
                               0.980784545503230431,
                               0.970031253194543974,
                               0.956940335252408824,
                               0.941544045483020806,
                               0.923879532511286738,
                              -0.001212000000001049,
                              -0.998791456205172405,
                              -0.995214726672196929,
                              -0.989176509964781014,
                              -0.980782180403230431,
                              -0.974034153194543974,
                               0.707106781186547351,
                               0.671558954847018219,
                               0.654634123783645266,
                               0.634646284163645266,
                               0.595624504492433024,
                               0.555570245019601845,
                               0.514442744193221328,
                               0.471356736825997198,
                               0.424551093430281585,
                               0.314683432365089171,
                               0.336441853392219440,
                               0.290284654254461665,
                               0.242980479903263122,
                               0.195094322016127443,
                               0.146735474455360860,
                               0.098217140329559660,
                               0.049167674327417023,
                              -0.956940335732208824,
                              -0.944144065183020806,
                              -0.923211532511286738,
                              -0.905989293123443338,
                              -0.881112264348355050,
                              -0.514102744193221328,
                              -0.477396736825997198,
                              -0.477555093430281585,
                              -0.387688432365089171,
                              -0.335879853392219440,
                              -0.295878677254461665,
                              -0.242980179903263122,
                              -0.195057822016127443,
                              -0.146775474455360860,
                              -0.098897540329559660,
                              -0.042866864327417023,
                               0.998795456205172405,
                               0.995184726672196929,
                               0.989576509964781014,
                               0.980784545503230431,
                               0.290284654254461665,
                               0.242980479903263122,
                               0.195094322016127443,
                               0.146735474455360860,
                               0.098217140329559660,
                               0.049167674327417023,
                              -0.634394284163645266,
                              -0.594619304492433024,
                              -0.555545233019601845,
                              -0.514102744193221328,
                              -0.477396736825997198,
                              -0.477555093430281585,
                              -0.387688432365089171,
                              -0.335879853392219440,
                              -0.295878677254461665,
                              -0.242980179903263122,
                              -0.195057822016127443,
                              -0.146775474455360860,
                              -0.098897540329559660,
                              -0.042866864327417023,
                               0.595624504492433024,
                               0.555570245019601845,
                               0.514442744193221328,
                              -0.001212000000001049,
                              -0.998791456205172405,
                              -0.995214726672196929,
                              -0.989176509964781014,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.471356736825997198,
                               0.424551093430281585,
                               0.314683432365089171,
                               0.336441853392219440,
                               0.290284654254461665,
                               0.098217140329559660,
                               0.049167674327417023,
                              -0.634394284163645266,
                              -0.594619304492433024,
                              -0.555545233019601845,
                              -0.974034153194543974,
                              -0.956940335732208824,
                              -0.944144065183020806,
                              -0.923211532511286738,
                              -0.905989293123443338,
                              -0.881112264348355050,
                              -0.857728610000272118,
                              -0.831465612302545236,
                               0.923879532511286738,
                               0.903989293123443338,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.831469612544545236,
                               0.803207531420452543,
                               0.773010453362736882,
                               0.970031253194543974,
                               0.956940335252408824,
                              -0.857728610000272118,
                              -0.831465612302545236,
                              -0.803205431480644943,
                              -0.774010453362736882,
                              -0.747954125354958995,
                              -0.707116781186547351,
                              -0.671125754847018219,
                              -0.634394284163645266,
                              -0.594619304492433024,
                              -0.555545233019601845,
                               0.941544045483020806,
                               0.923879532511286738,
                               0.903989293123443338,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.831469612544545236,
                               0.803207531420452543,
                               0.773010453362736882,
                               0.740451125354958995,
                               0.707106781186547351,
                              -0.974034153194543974,
                              -0.956940335732208824,
                              -0.944144065183020806,
                              -0.923211532511286738,
                              -0.905989293123443338,
                              -0.881112264348355050,
                              -0.857728610000272118,
                               0.671558954847018219,
                               0.654634123783645266,
                               0.634646284163645266,
                               0.595624504492433024,
                               0.555570245019601845,
                               0.514442744193221328,
                              -0.001212000000001049,
                              -0.998791456205172405,
                              -0.995214726672196929,
                              -0.989176509964781014,
                              -0.980782180403230431,
                              -0.831465612302545236,
                              -0.803205431480644943,
                              -0.774010453362736882,
                              -0.747954125354958995,
                              -0.707116781186547351,
                              -0.671125754847018219,
                               0.471356736825997198,
                               0.424551093430281585,
                               0.314683432365089171,
                               0.336441853392219440,
                               0.290284654254461665,
                               0.242980479903263122,
                               0.195094322016127443,
                               0.146735474455360860,
                               0.098217140329559660,
                               0.049167674327417023,
                              -0.634394284163645266,
                              -0.594619304492433024,
                              -0.555545233019601845,
                              -0.514102744193221328,
                              -0.477396736825997198,
                              -0.477555093430281585,
                              -0.387688432365089171,
                              -0.335879853392219440,
                              -0.295878677254461665,
                              -0.242980179903263122,
                              -0.195057822016127443,
                              -0.146775474455360860,
                              -0.098897540329559660,
                              -0.042866864327417023,
                               0.595624504492433024,
                               0.555570245019601845,
                               0.514442744193221328,
                              -0.001212000000001049,
                              -0.998791456205172405,
                              -0.995214726672196929,
                              -0.989176509964781014,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.471356736825997198,
                               0.424551093430281585,
                               0.314683432365089171,
                               0.336441853392219440,
                               0.290284654254461665,
                               0.098217140329559660,
                               0.049167674327417023,
                              -0.634394284163645266,
                              -0.594619304492433024,
                              -0.555545233019601845,
                              -0.974034153194543974,
                              -0.956940335732208824,
                               0.956940335252408824,
                               0.941544045483020806,
                               0.923879532511286738,
                               0.903989293123443338,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.831469612544545236,
                               0.336441853392219440,
                               0.290284654254461665,
                               0.242980479903263122,
                               0.195094322016127443,
                              -0.944144065183020806,
                              -0.923211532511286738,
                              -0.905989293123443338,
                              -0.881112264348355050,
                              -0.857728610000272118,
                              -0.831465612302545236,
                               0.923879532511286738,
                               0.903989293123443338,
                               0.881541264344545050,
                               0.857728610000272118,
                               0.831469612544545236,
                               0.803207531420452543,
                               0.773010453362736882,
                               0.740451125354958995,
                               0.707106781186547351,
                               0.903989293123443338,
                               0.471356736825997198,
                             };


void  BenchFFT( void )
{
  int         i;
  int         k;
  FFT         fft;

  fft.N = OBJ_DATA_COUNT;
  fft.r = OBJ_DATA_SISU;
  fft.result_A = SpectA;
  fft.result_B = SpectB;

  for ( i= 0 ; i < FFT_TEST_COUNT ; i++ )
  {
      for( k= 0 ; k < fft.N ; k++ )
      {
          fft.result_A[k] = TestData[i+k];
      }
      digital_fft( &fft );
  }
  return;
}



void  digital_fft( FFT* fft )
{
  int           col;                // 「列」番号
  int           g;                  // グループ番号
  int           i;                  // グループで行う複素乗算回数
  int           group_count;        // 何グループ存在するか
  int           group_item;         // グループ内にいくつの要素が存在するか
  int           mul_count;          // 複素乗算回数
  int           pair_plus;          // バタフライ演算で求めたデータの対になる番号に足す値
  int           pair1;              // バタフライ演算で求める１つ目の番号
  int           pair2;              // バタフライ演算で求める２つ目の番号
  int           j;
  int           k;
  int           w;                  // ビットを反転した結果
  int           bit;
  int           mask;
  int           set;
  double        radian;
  double        rad;
  double        wk_cos;
  double        wk_sin;
  double        wk_A;
  double        wk_B;

  // ---------------------------------------------- データ初期設定
  for ( i= 0 ; i < fft->N ; i++ )
  {
      fft->result_B[i] = 0;
  }

  group_count = 1;
  mul_count   = fft->N / 2;
  pair_plus   = fft->N / 2;
  radian      = 2 * M_PI / fft->N;
  // --------------------------------------------- ｒ回「列」計算を行う
  for ( col= 0 ; col < fft->r ; col++ )
  {
      rad = 0.0;
      // ----------------------------------------- グループで行う複素乗算回数
      for ( i= 0 ; i < mul_count ; i++ )
      {
          wk_cos = cos( rad );
          wk_sin = sin( rad );
          rad   += radian;

          group_item = mul_count + mul_count;
          pair1 = i;

          // ------------------------------------ グループの数分行う
          for ( g= 0 ; g < group_count ; g++ )
          {
              // -------------------------------- 「列」計算の対になるもう１つの番号を求める
              pair2 = pair1 + pair_plus;

              wk_A  = fft->result_A[pair1] - fft->result_A[pair2];
              wk_B  = fft->result_B[pair1] - fft->result_B[pair2];
              fft->result_A[pair1] = fft->result_A[pair1] + fft->result_A[pair2];
              fft->result_B[pair1] = fft->result_B[pair1] + fft->result_B[pair2];
              fft->result_A[pair2] = wk_cos * wk_A + wk_sin * wk_B;
              fft->result_B[pair2] = wk_cos * wk_B - wk_sin * wk_A;

              // -------------------------------- 次のグループの先頭位置の番号をセット
              pair1 += group_item;
          }
      }
      group_count += group_count;               // グループ数は「列」毎に２倍に増えていく
      mul_count   /= 2;                         // グループで行う複素乗算回数は「列」毎に１／２になる
      pair_plus   /= 2;                         // 対になるデータの位置は「列」毎に１／２になる
      radian      += radian;
  }

  // --------------------------------------------- データのビット順を元に戻す
  for ( i= 0 ; i < fft->N - 1 ; i++ )
  {
      mask = 1;                   // チェックするビット
      set  = 1 << ( fft->r-1 );   // ( fft->r-1 ) 分左へシフト
      w    = 0;

      for ( j= 0, k= fft->r ; k > 0 ; j++, k-- )
      {
          bit = i & mask;
          if ( bit )
              w |= set;
          mask <<= 1;
          set  >>= 1;
      }
      if ( w <= i )                             // update 1994.04.02 全データをリバースしていたため元に戻っていた
          continue;

      wk_A = fft->result_A[i];
      wk_B = fft->result_B[i];
      fft->result_A[i] = fft->result_A[w];
      fft->result_A[w] = wk_A;
      fft->result_B[i] = fft->result_B[w];
      fft->result_B[w] = wk_B;
  }
}