#include <msp430x16x.h>
#include <math.h>
#include "../includes.h"

#define DATSIZE 128  //定义采样点数, DATSIZE = 2^n 
#define DUOPI 6.283185307179586476925286766559

FP32 DataRe[DATSIZE] = {0}; //数据实部,存放采样点,运算结果的实部
FP32 DataIm[DATSIZE] = {0};  //数据虚部,运算结果的虚部

void FFT(FP32 * const Re,FP32 * const Im, const INT16S DataSize);
void main()
{
  WDTCTL = WDTPW + WDTHOLD;
  
  FFT(DataRe, DataIm, DATSIZE);
  
  while(1);
}


INT16S InvAdd(INT16S InvNum, INT16S BITWIDTH)
{
  INT16U m;
  
  m = 0x0001<<(BITWIDTH - 1);
  while((m == (m&InvNum))&&(0 != m))
  {
    m >>= 1;      
  }
  if(m != 0)
  {      
    do
    {
      InvNum = m^InvNum;
      m <<= 1;
    }
    while(m != (0x0001<<BITWIDTH));
  }
  
  return InvNum;
}

INT16S Pow2(INT16S Num)
{
  INT16S i = 1;
  
  if(0 == Num)
  {
    return 1;
  }
  else
  {
    while(Num-- > 0)
    {
      i = i * 2;
    }
    
    return i;
  }
}

void FFT(FP32 * const Re, FP32 * const Im, const INT16S DataSize)
{
  INT16S StepNum;  //级数目
  INT16S CellNum;  //每级中蝶形单元数目
  INT16S GrpNum;   //不同旋转因子的个数
  INT16S StepCnt;  //计数
  INT16S CellCnt;  
  INT16S GrpCnt;
  INT8U Intval;
  INT8U BaseAddr;
  INT16S i;
  INT16S InvNum;
  INT16S BITWIDTH;
  INT16S NDiv;
  FP32 SinTmp, CosTmp;
  FP32 ReTmp1, ImTmp1, ReTmp2, ImTmp2;
  
  StepNum  = (INT16S)(log((FP32)DataSize)/log(2.0)); //计算级数
  BITWIDTH = StepNum;
  
  for(i = 0; i < DataSize; i++)
  {
    *(Im + i)= *(Re + i);
  }
  for(i = 0, InvNum = 0; i < DataSize; i++)
  {    
    *(Re + InvNum) = *(Im + i);
    InvNum = InvAdd(InvNum, BITWIDTH);
  }
  for(i = 0; i < DataSize; i++)
  {
    *(Im + i) = 0;
  }
  
  for(StepCnt = 0; StepCnt < StepNum; StepCnt++)
  {    
    CellNum = Pow2(StepNum - StepCnt - 1); //计算蝶形单元数目
    GrpNum  = Pow2(StepCnt);
    Intval  = Pow2(StepCnt);
    NDiv    = Pow2(StepCnt + 1);
    
    for(GrpCnt = 0; GrpCnt < GrpNum; GrpCnt++)
    {      
      SinTmp = sin(DUOPI / NDiv * GrpCnt);
      CosTmp = cos(DUOPI / NDiv * GrpCnt);
      
      for(CellCnt = 0; CellCnt < CellNum; CellCnt++)
      {
        BaseAddr = CellCnt * GrpNum * 2 + GrpCnt;
        ReTmp1   = *(Re + BaseAddr);
        ImTmp1   = *(Im + BaseAddr);
        ReTmp2   = *(Re + BaseAddr + Intval);
        ImTmp2   = *(Im + BaseAddr + Intval);
        
        *(Re + BaseAddr)          = ReTmp1 + ReTmp2 * CosTmp + ImTmp2 * SinTmp;
        *(Im + BaseAddr)          = ImTmp1 - ReTmp2 * SinTmp + ImTmp2 * CosTmp;
        *(Re + BaseAddr + Intval) = ReTmp1 - ReTmp2 * CosTmp - ImTmp2 * SinTmp;
        *(Im + BaseAddr + Intval) = ImTmp1 + ReTmp2 * SinTmp - ImTmp2 * CosTmp;
      }
    }
  }
  
  for(i = 0; i < DataSize; i++)
  {
    *(Re + i) = sqrt(*(Re + i) * *(Re + i) + *(Im + i) * *(Im + i));
  }
}