#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "fourier.h"
#include "ddcmath.h"

#include "fft.h"
#include "cmsis_os.h"

#define CHECKPOINTER(p) CheckPointer(p, #p)

static void CheckPointer(void *p, char *name)
{
    if (p == NULL)
    {
        // fprintf(stderr, "Error in fft_float():  %s == NULL\n", name);
        printf("Error in fft_float():  %s == NULL\n", name);
        exit(1);
    }
}

void vFFT()
{
    unsigned MAXSIZE;
    unsigned MAXWAVES;
    unsigned i, j;
    int invfft = 0;

    MAXSIZE = 128;
    MAXWAVES = 4;

    int IMC_REPEAT = 10;
    // IMC_REPEAT = 100;

    // srand(1);
    
    float RealIn[MAXSIZE];
    float ImagIn[MAXSIZE];
    float RealOut[MAXSIZE];
    float ImagOut[MAXSIZE];
    float coeff[MAXWAVES];
    float amp[MAXWAVES];

    for (int imc_repeat=0; imc_repeat < IMC_REPEAT; imc_repeat++) {

    /* Makes MAXWAVES waves of random amplitude and period */
    for (i = 0; i < MAXWAVES; i++)
    {
        // coeff[i] = rand() % 1000;
        // amp[i] = rand() % 1000;
        coeff[i] = 123;
        amp[i] = 456;
    }
    for (i = 0; i < MAXSIZE; i++)
    {
        /*   RealIn[i]=rand();*/
        RealIn[i] = 0;
        ImagIn[i] = 0;
        float sum = 0;
        for (j = 0; j < MAXWAVES; j++)
        {
            /* randomly select sin or cos */
            if (i % 2)
            {
                sum += coeff[j] * cos(amp[j] * i);
                // RealIn[i] += coeff[j] * cos(amp[j] * i);
            }
            else
            {
                // RealIn[i] += coeff[j] * sin(amp[j] * i);
                sum += coeff[j] * sin(amp[j] * i);
            }
        }
        RealIn[i] = sum;
    }

    /* regular*/
    {
        unsigned NumBits; /* Number of bits needed to store indices */
        unsigned i, j, k, n;
        unsigned BlockSize, BlockEnd;

        double angle_numerator = 2.0 * DDC_PI;
        double tr, ti; /* temp real, temp imaginary */

        unsigned NumSamples = MAXSIZE;
        int InverseTransform = invfft;

        if (!IsPowerOfTwo(NumSamples))
        {
            // fprintf(
            //     stderr,
            //     "Error in fft():  NumSamples=%u is not power of two\n",
            //     NumSamples);
            printf(
                "Error in fft():  NumSamples=%u is not power of two\n",
                NumSamples);

            exit(1);
        }

        if (InverseTransform)
            angle_numerator = -angle_numerator;

        NumBits = NumberOfBitsNeeded(NumSamples);

        /*
        **   Do simultaneous data copy and bit-reversal ordering into outputs...
        */

        for (i = 0; i < NumSamples; i++)
        {
            j = ReverseBits(i, NumBits);
            RealOut[j] = RealIn[i];
            // ImagOut[j] = (ImagIn == NULL) ? 0.0 : ImagIn[i];
            ImagOut[j] = ImagIn[i];
        }

        /*
        **   Do the FFT itself...
        */

        BlockEnd = 1;
        for (BlockSize = 2; BlockSize <= NumSamples; BlockSize <<= 1)
        {
            double delta_angle = angle_numerator / (double)BlockSize;
            double sm2 = sin(-2 * delta_angle);
            double sm1 = sin(-delta_angle);
            double cm2 = cos(-2 * delta_angle);
            double cm1 = cos(-delta_angle);
            double w = 2 * cm1;
            double ar[3], ai[3];
            double temp;

            for (i = 0; i < NumSamples; i += BlockSize)
            {
                ar[2] = cm2;
                ar[1] = cm1;

                ai[2] = sm2;
                ai[1] = sm1;

                for (j = i, n = 0; n < BlockEnd; j++, n++)
                {
                    ar[0] = w * ar[1] - ar[2];
                    ar[2] = ar[1];
                    ar[1] = ar[0];

                    ai[0] = w * ai[1] - ai[2];
                    ai[2] = ai[1];
                    ai[1] = ai[0];

                    k = j + BlockEnd;
                    tr = ar[0] * RealOut[k] - ai[0] * ImagOut[k];
                    ti = ar[0] * ImagOut[k] + ai[0] * RealOut[k];

                    RealOut[k] = RealOut[j] - tr;
                    ImagOut[k] = ImagOut[j] - ti;

                    RealOut[j] += tr;
                    ImagOut[j] += ti;
                }
            }

            BlockEnd = BlockSize;
        }

        /*
        **   Need to normalize if inverse transform...
        */

        if (InverseTransform)
        {
            double denom = (double)NumSamples;

            for (i = 0; i < NumSamples; i++)
            {
                RealOut[i] /= denom;
                ImagOut[i] /= denom;
            }
        }
    }

    } //imc_repeat

    /*
    portDISABLE_INTERRUPTS();
    printf("RealOut:\r\n");
    portENABLE_INTERRUPTS();
    for (i = 0; i < MAXSIZE; i++)
        printf("%f \t", RealOut[i]);
    printf("\r\n");

    printf("ImagOut:\r\n");
    for (i = 0; i < MAXSIZE; i++)
        printf("%f \t", ImagOut[i]);
    printf("\r\n");
    */

    float real_sum = 0;
    float imag_sum = 0;

    for (i = 0; i < MAXSIZE; i++) {
        real_sum += RealOut[i];
        imag_sum += ImagOut[i];
    }

    portDISABLE_INTERRUPTS();
    printf("%d, %d\r\n", (int)(real_sum*1000000), (int)(imag_sum*1000000));
    portENABLE_INTERRUPTS();

    // printf("RealOut: ");
    // for (i = 0; i < 3; i++)
    //     printf("%f\t", RealOut[i]);
    // printf("\r\n");
    // printf("ImagOut: ");
    // for (i = 0; i < 3; i++)
    //     printf("%f\t", ImagOut[i]);
    // printf("\r\n");
}