//////////////////////////////////////////////////////
// Быстрое преобразование Фурье. 
//////////////////////////////////////////////////////

/////////////////////////////////
// Для прямоугольного входного сигнала задать SQUARE
// (делать коррекцию амплитуд и фаз.
// Иначе #define закомментировать.
//#define SQUARE
/////////////////////////////////

/////////////////////////////////
// Число элементов задаётся степенью двойки,
// например, при вызове fft(src, dest, 10).
// Mассивы src и dest должны иметь соответствующий размер.
#include <math.h>
////////////////////
#ifdef SQUARE  
 #define _POWER_OF_2_       (5)
 #define _N_SAMPLES_        (32)
#else
 #define _POWER_OF_2_       (10)
 #define _N_SAMPLES_        (1024)
#endif
///////////////
typedef struct
 { 
  float re, im; 
 }TComplex;
TComplex _src[ _N_SAMPLES_ ], _dst[ _N_SAMPLES_ ];
TComplex T, U, W;
//---
#if !defined M_PI                      //Если PI не определено:
#define M_PI        (3.14159265359)    //Определяем его
#endif
//----------------------------------------------------
// Моделирование входного сигнала в форме синусоиды
// fF периодов синуса
// nN - количество точек
// aA - амплитуда синуса
void Sinsignal_For_Test( float fF,float aA, int nN )  
 {
  volatile int i;
  volatile float r, re, re1, im, im1;

  re = cos((2.00 * M_PI * fF ) / (float)nN);
  im = sin((2.00 * M_PI * fF ) / (float)nN);

  re1 = aA; 
  im1 = 0.00;
  
  for( i = 0; i < nN; i++ )
   {
    (_src[i].re) = re1;    
    r = re1;
    re1 = (r * re) - (im1 * im);    
    im1 = (im1 * re) + (r * im);    
   }/*for*/
 }/*Sinsignal_For_Test*/
//---------------------------------------------------- 
//   F F T
//------------------------------------------
void fft( TComplex *src, TComplex *dest, int pow2)
 {
  int Le, Le1, jt, it, ip, lt, kt, num;

  num = (1 << pow2);    //число элементов
  for (it = 0; it < num; it++) 
   {
    dest[it] = src[it];
   }/*for*/ 

  for (it = 0; it < num; it++)
   {
    kt = num / 2;
    jt = 0;
    Le = 1;
    Le1 = it;
    for (lt = 1; lt <= pow2; lt++)
     {
      if (kt <= Le1)
       {
	jt = jt + Le;
	Le1 = Le1 - kt;
       }/*if*/
      Le = Le * 2;
      kt = kt / 2;
     }/*for*/

    if (it < jt)
     {
      T = src[jt];
      dest[jt] = dest[it];
      dest[it] = T;
     }/*if*/
   }/*for*/

  for (lt = 1; lt <= pow2; lt++)
   {
    Le = 1;
    
    for (jt = 1; jt <= lt; jt++)
     {
      Le = Le * 2;
     }/*for*/ 
    Le1 = Le / 2;
    U.re = 1.0;
    U.im = 0.0;
    W.re = cos(M_PI / Le1);
    W.im = sin(M_PI / Le1);
    for (jt = 0; jt < Le1; jt++)
     {
      it = jt;
      while (it < num - Le1)
       {
        ip = it + Le1;
        T.re = dest[ip].re * U.re - dest[ip].im * U.im;
        T.im = dest[ip].re * U.im + dest[ip].im * U.re;
        dest[ip].re = dest[it].re - T.re;
        dest[ip].im = dest[it].im - T.im;
        dest[it].re = dest[it].re + T.re;
        dest[it].im = dest[it].im + T.im;
        it = it + Le;
       }/*while*/
       
      T.re = U.re * W.re - U.im * W.im;
      T.im = U.re * W.im + U.im * W.re;
      U.re = T.re;
      U.im = T.im;
     }/*for*/
   }/*for*/
 }/*fft*/
//-----------------------------------------
void inverse_fft( TComplex *src, TComplex *dest, int pow2)
 {
  int num, it;
  
  num = (1 << pow2);    //число элементов
  for (it = 0; it < num; it++)
   {
    dest[it].re = src[it].re;
    dest[it].im = -src[it].im;
   }/*for*/
  fft(dest, dest, pow2);
  for (it = 0; it < num; it++)
   {
    dest[it].re = dest[it].re / num;
    dest[it].im = -dest[it].im / num;
   }/*for*/
 }/*inverse_fft*/
//----------------------------------------------------
__C_task main( void )
 {
  unsigned int j;
  float A, Re, Im, Fi;

#ifdef SQUARE  
  float Koef, dFi;
#endif

  ///////////////////////////////////////////////////////////////
  // Задание тестовых входных сигналов
  ///////////////////////////////////////////////////////////////

#ifdef SQUARE  
  // Импульсный вх. сигнал из 32 отсчетов, первые 8 из 
  // которых == 1, остальные нули.
  // Для _N_SAMPLES_ = 32,    _POWER_OF_2 = 5  
   for(j=0; j<_N_SAMPLES_; j++)
    {
     if( j<8)    _src[j].re = 1;
     else        _src[j].re = 0;
     _src[j].im = 0;     
    }/*for*/
  /////////////////////////////////
#else
  // Для sin-входного сигнала.  
   Sinsignal_For_Test( 100, 100, _N_SAMPLES_ );  // Генерация sin
  ///////////////////////////////////////////////////////////////
#endif       

    
    
    
  // Вызов fft
  fft((TComplex *)&_src, (TComplex *)&_dst, _POWER_OF_2_ );
  //--------
  
  // Обработка результата
  for( j = 0; j < _N_SAMPLES_/2; j++ )
   {
    /////////////////////////////////////////////////////////
    // 1. Взять действительную и мнимую часть
    /////////////////////////////////////////////////////////
    Re = (_dst[j].re);
    Im = (_dst[j].im);

#ifdef SQUARE  
    //////////////////////////////////////////////////////////
    // 2. Обработать выходные данные для повышения точности БПФ 
    //    путем домножения Re и Im на коэффициент.
    //////////////////////////////////////////////////////////
    Fi = 0.000;          // Фаза гармоники. Сначала = 0.
    dFi = 0.000;         // Переменная для коррекции фазы
    
    if( j )              // Не делать для 0 гармоники (иначе - деление на 0)
     {
      dFi = ( M_PI * (float)j );
      dFi = ( dFi / (float)_N_SAMPLES_ );
      Koef = ( sin( dFi ) / dFi );
      Re *= Koef;
      Im *= Koef;
     }/*if*/ 
#endif

    ////////////////////////////////////////////////////
    // 3. Расчет амплитуд гармоник
    ////////////////////////////////////////////////////
    A = sqrt( (Re * Re) + (Im * Im) );
    if( j )
     {
      A = (A * 2.000);
     }/*if*/
    A = ( A / (float)_N_SAMPLES_); 
    /////////////////////////////////////////////////////
    
    /////////////////////////////////////////////////////
    // 4. Расчет фаз
    /////////////////////////////////////////////////////
    
    if( (A < (-0.001)) || (A > 0.001 ) ) // Если А близко к 0, то и 
//фазы пусть не будет
     {
      if( (Re > (-0.000000001)) && (Re < 0.000000001 ) )
       {
        if( Im >   0.000000001 )     Fi = 1.5708;
        if( Im < (-0.000000001) )    Fi = (-1.5708);      
       }/*if*/
      else
       {        
        Fi = atan2( Im , Re );
        
#ifdef SQUARE  
        Fi += dFi;
#endif
        
        Fi = ((180.000 * Fi) / M_PI);  // Перевод из радиан в градусы
        
       }/*else*/ 
     }/*if*/
    ////////////////////////////////////////////////////

    /////////////////////////////////////////////////////////////////
    ////////////// Вывод на экран
    /////////////////////////////////////////////////////////////////
    printf(" A[%d] = %.4f, Fi = %.2f \n", j, A, Fi );
    /////////////////////////////////////////////////////////////////
   }/*for*/ 
 }/*main*/
//////////////////////////////////////