/*---------------------------------------------------------------------------*/
/*
	File: fft.cpp
*/
/*---------------------------------------------------------------------------*/

#define FFT_CPP_

#include "platform.h"
#include <math.h>

#include "fft.h"

/*-------------------------------------------------------------------------*/

#define PI 3.14159265


/*-------------------------------------------------------------------------*/

float CosinusTable[MAX_FFT_SIZE/4];

void FFTInit ()
{
	int i;
	float Scale;

	Scale = ((float)(PI / 2.0)) / ((float)(MAX_FFT_SIZE/4));
	for (i = 0; i < MAX_FFT_SIZE/4; i++)
		CosinusTable[i] = (float)(cos ((float)(Scale * (float)(i))));
}


void FFTWindow (TComplexFloat *FFTData, int FFTSize, float WindowFactor)
{
	int i, Phi, dPhi;
	float CenterLine, f;

	dPhi = MAX_FFT_SIZE / FFTSize;
	CenterLine = ((float)(1.0)) - WindowFactor;
	f = CenterLine - WindowFactor;
	FFTData[0].Re *= f;
	FFTData[0].Im *= f;
	FFTData[FFTSize/4].Re *= CenterLine;
	FFTData[FFTSize/4].Im *= CenterLine;
	FFTData[3 * FFTSize/4].Re *= CenterLine;
	FFTData[3 * FFTSize/4].Im *= CenterLine;
	Phi = 0;
	for (i = 1; i < FFTSize/4; i++)
	{
		Phi += dPhi;
		f = CenterLine - ((float)(CosinusTable[Phi] * WindowFactor));
		FFTData[i].Re *= f;
		FFTData[i].Im *= f;
		FFTData[FFTSize - i].Re *= f;
		FFTData[FFTSize - i].Im *= f;
		f = CenterLine + (CenterLine - f);
		FFTData[FFTSize/2 - i].Re *= f;
		FFTData[FFTSize/2 - i].Im *= f;
		FFTData[FFTSize/2 + i].Re *= f;
		FFTData[FFTSize/2 + i].Im *= f;
	}
}


void FFTCalc (TComplexFloat *FFTData, int FFTSize)
{
  int Phi, dPhi, i0, i1, i2;
  float SinPhi, CosPhi, Re1, Im1, Re2, Im2;
  TComplexFloat *p1, *p2, *e;

  e = FFTData + FFTSize;
  dPhi = MAX_FFT_SIZE / FFTSize;
  i0 = FFTSize;
  do
  {
    i0 >>= 1;
    p1 = FFTData;
    do
    {
      p2 = p1 + i0;
      Re1 = p1->Re;
      Im1 = p1->Im;
      Re2 = p2->Re;
      Im2 = p2->Im;
      p1->Re = Re1+Re2;
      p1->Im = Im1+Im2;
      p2->Re = Re1-Re2;
      p2->Im = Im1-Im2;
      p1 = p2 + i0;
    } while (p1 < e);
    if (dPhi <= MAX_FFT_SIZE/4)
    {
      p1 = FFTData + 1;
      Phi = dPhi;
      while (Phi < MAX_FFT_SIZE/4)
      {
        CosPhi = CosinusTable[Phi];
        SinPhi = CosinusTable[MAX_FFT_SIZE/4 - Phi];
        do
        {
          p2 = p1 + i0;
          Re1 = p1->Re;
          Im1 = p1->Im;
          Re2 = p2->Re;
          Im2 = p2->Im;
          p1->Re = Re1+Re2;
          p1->Im = Im1+Im2;
          p2->Re = ((float)(CosPhi * ((float)(Re1-Re2)))) + ((float)(SinPhi * ((float)(Im1-Im2))));
          p2->Im = ((float)(CosPhi * ((float)(Im1-Im2)))) - ((float)(SinPhi * ((float)(Re1-Re2))));
          p1 = p2 + i0;
        } while (p1 < e);
        p1 -= FFTSize - 1;
        Phi += dPhi;
      }
      do
      {
        p2 = p1 + i0;
        Re1 = p1->Re;
        Im1 = p1->Im;
        Re2 = p2->Re;
        Im2 = p2->Im;
        p1->Re = Re1+Re2;
        p1->Im = Im1+Im2;
        p2->Re = Im1-Im2;
        p2->Im = Re2-Re1;
        p1 = p2 + i0;
      } while (p1 < e);
      p1 -= FFTSize - 1;
      Phi = dPhi;
      while (Phi < MAX_FFT_SIZE/4)
      {
        CosPhi = CosinusTable[MAX_FFT_SIZE/4 - Phi];
        SinPhi = CosinusTable[Phi];
        do
        {
          p2 = p1 + i0;
          Re1 = p1->Re;
          Im1 = p1->Im;
          Re2 = p2->Re;
          Im2 = p2->Im;
          p1->Re = Re1+Re2;
          p1->Im = Im1+Im2;
          p2->Re = ((float)(CosPhi * ((float)(Re2-Re1)))) - ((float)(SinPhi * ((float)(Im2-Im1))));
          p2->Im = ((float)(CosPhi * ((float)(Im2-Im1)))) + ((float)(SinPhi * ((float)(Re2-Re1))));
          p1 = p2 + i0;
        } while (p1 < e);
        p1 -= FFTSize - 1;
        Phi += dPhi;
      }
    }
    dPhi <<= 1;
  } while (i0 != 1);

  i2 = 0;
  for (i1 = 0; i1 < FFTSize-1; i1++)
  {
    if (i2 > i1)
    {
      Re1 = FFTData[i1].Re;
      FFTData[i1].Re = FFTData[i2].Re;
      FFTData[i2].Re = Re1;
      Im1 = FFTData[i1].Im;
      FFTData[i1].Im = FFTData[i2].Im;
      FFTData[i2].Im = Im1;
    }
    i0 = FFTSize/2;
    while (i2 >= i0)
    {
      i2 -= i0;
      i0 >>= 1;
    }
    i2 += i0;
  }
}

#if 0

void FFTEnergy (TComplexFloat *FFTData, int FFTSize)
{
	int i;
	float f;

	f = (float)(1.0 / (((float) FFTSize) * ((float) FFTSize)));
	FFTData[0].Re = (float)((FFTData[0].Re * FFTData[0].Re) * f * 0.25);
	for (i = 1; i < FFTSize / 2; i++)
		FFTData[i].Re = (float)((FFTData[i].Re * FFTData[i].Re + FFTData[i].Im * FFTData[i].Im) * f);
}


void FFTPolar (TComplexFloat *FFTData, int FFTSize)
{
	int i;
	float Magnitude;

	for (i = 0; i < FFTSize; i++)
	{
		Magnitude = (float)(sqrt (FFTData[i].Re * FFTData[i].Re + FFTData[i].Im * FFTData[i].Im));
		if (Magnitude >= 0.001)
		{
			if (FFTData[i].Re >= 0.0)
			{
				if (FFTData[i].Im >= 0.0)
				{
					if (FFTData[i].Re >= FFTData[i].Im)
						FFTData[i].Im = (float)(atan (FFTData[i].Im / FFTData[i].Re));
					else
						FFTData[i].Im = (float)((PI / 2.0) - atan (FFTData[i].Re / FFTData[i].Im));
				}
				else
				{
					if (FFTData[i].Re >= -FFTData[i].Im)
						FFTData[i].Im = (float)(atan (FFTData[i].Im / FFTData[i].Re));
					else
						FFTData[i].Im = (float)(-PI / 2.0 - atan (FFTData[i].Re / FFTData[i].Im));
				}
			}
			else
			{
				if (FFTData[i].Im >= 0.0)
				{
					if (-FFTData[i].Re >= FFTData[i].Im)
						FFTData[i].Im = (float)(PI + atan (FFTData[i].Im / FFTData[i].Re));
					else
						FFTData[i].Im = (float)(PI / 2.0 - atan (FFTData[i].Re / FFTData[i].Im));
				}
				else
				{
					if (-FFTData[i].Re >= -FFTData[i].Im)
						FFTData[i].Im = (float)(-PI + atan (FFTData[i].Im / FFTData[i].Re));
					else
						FFTData[i].Im = (float)(-PI / 2.0 - atan (FFTData[i].Re / FFTData[i].Im));
				}
			}
		}
		else
			FFTData[i].Im = 0;
		FFTData[i].Re = Magnitude;
	}
}


void FFTVector (TComplexFloat *FFTData, int FFTSize)
{
	int i;
	float Magnitude;

	for (i = 0; i < FFTSize; i++)
	{
		Magnitude = FFTData[i].Re;
		FFTData[i].Re = (float)(Magnitude * cos (FFTData[i].Im));
		FFTData[i].Im = (float)(Magnitude * sin (FFTData[i].Im));
	}	
}

#endif
/*-------------------------------------------------------------------------*/
