#include "sw_only_fft.h"
#include <alt_types.h>

void software_only_fft (
    alt_16   *InData,        // Input Data Buffer
    alt_16   *OutData,       // Output Data Buffer
    alt_16   *TwiddleTable   // Interleaved Sine/Cosine Table
)
{
    alt_16   i,ii;
    alt_16   bit_rev_index;
    alt_16   stage_index;
    alt_16   sub_stage_index;
    alt_16   butterfly_index;
    alt_16   twiddle_index;
    alt_16   twiddle_incr;
    alt_16   loop_element,loop_element_div2;
    alt_32   CosReal,SinReal;
    alt_32   TempReal,TempImaginary;
    alt_16   reversed_RealData[NUM_POINTS];
    alt_16   reversed_ImaginaryData[NUM_POINTS];


    // Re-order samples using bit reversal
    for (i = 0; i < NUM_POINTS; i++) {
        bit_rev_index = bitrev(i);   
        reversed_RealData[bit_rev_index] = InData[2*i];
        reversed_ImaginaryData[bit_rev_index] = InData[2*i+1];
    }

    // Loop through the stages of the N Point FFT
    for (stage_index = 1; stage_index <= FFT_SIZE; stage_index++) {
        loop_element = 1<<stage_index;
        loop_element_div2 = loop_element/2;

        // Initialize twiddle factor lookup indicies 
        twiddle_index = 0;
        twiddle_incr = 1 << (FFT_SIZE-stage_index+1);

        // Loop through each sub stage 
        for(sub_stage_index = 0; sub_stage_index < loop_element_div2; sub_stage_index++) {

          // Calculate sine and cosine values from interleaved twiddle table
          CosReal = TwiddleTable[twiddle_index];
          SinReal = TwiddleTable[twiddle_index+1];

          // Butterfly calculation
          for(butterfly_index = sub_stage_index; butterfly_index < NUM_POINTS; butterfly_index += loop_element) {
            TempReal = (( CosReal * reversed_RealData[butterfly_index + loop_element_div2] ) + 
                      ( SinReal * reversed_ImaginaryData[butterfly_index + loop_element_div2] )) >> PRESCALE;
            TempImaginary = (( CosReal * reversed_ImaginaryData[butterfly_index + loop_element_div2] ) - 
                      ( SinReal * reversed_RealData[butterfly_index + loop_element_div2] )) >> PRESCALE;

				    // Perform the butterfly calculation and scale result for alt_16 type
            reversed_RealData[butterfly_index + loop_element_div2] = reversed_RealData[butterfly_index] - TempReal;
            reversed_ImaginaryData[butterfly_index + loop_element_div2] = reversed_ImaginaryData[butterfly_index] - TempImaginary;
            reversed_RealData[butterfly_index] += TempReal;
            reversed_ImaginaryData[butterfly_index] += TempImaginary;

            // Write first half of interleaved data to output buffer
            if (stage_index == FFT_SIZE) {
              OutData[2*butterfly_index] = reversed_RealData[butterfly_index];
              OutData[2*butterfly_index+1] = reversed_ImaginaryData[butterfly_index];
            }
          }
          twiddle_index += twiddle_incr;
        }
      }
      // Write second half of interleaved data to output buffer
      for(i=NUM_POINTS/2;i<NUM_POINTS;i++) {
        OutData[2*i] = reversed_RealData[i];
        OutData[2*i+1] = reversed_ImaginaryData[i];    
      }
}