/* test_pq_fft.c */
#include "app.h"

#define CFFT_INPUT_LEN 128u



void TEST_PQ_CFFT_Q31(void)
{
    PRINTF("%s\r\n", __func__);

    arm_cfft_instance_q31 instance; // = { .fftLen = CFFT_INPUT_LEN };
    float32_t f32In[CFFT_INPUT_LEN * 2], f32Out[CFFT_INPUT_LEN * 2];
    q31_t     q31In[CFFT_INPUT_LEN * 2], /* q31Out[CFFT_INPUT_LEN * 2], */ q31InOut[CFFT_INPUT_LEN * 2];

    uint32_t i;

    ///* Two full period sin wave. */
    //for(i=0; i < CFFT_INPUT_LEN / 2; i++)
    for (i = 0; i < CFFT_INPUT_LEN; i++)
    {
        //inputSave[i * 2]     = (arm_sin_q31(i * (0x80000000 / (CFFT_INPUT_LEN / 2))) / 2) >> 5;
        //inputSave[i * 2 + 1] = (arm_cos_q31(i * (0x80000000 / (CFFT_INPUT_LEN / 2))) / 2) >> 5;

        //f32In[i*2]    = arm_sin_f32( (PI/128.0f) * i) /2.0; /* 实部 */
        //f32In[i*2+1]  = arm_cos_f32( (PI/128.0f) * i) /2.0; /* 虚部 */

        q31In[i*2]       = arm_cos_q31(i * (0x80000000 / (CFFT_INPUT_LEN / 2))) / 2;  /* 1/(N/2), 两个周期 */
        q31In[i * 2 + 1] = arm_sin_q31(i * (0x80000000 / (CFFT_INPUT_LEN / 2))) / 2;
    }
#if 1
    arm_q31_to_float(q31In, f32In, CFFT_INPUT_LEN*2u); /* 鸟数转人数, 仅用于查看数值计算过程的正确性 */

    PRINTF("INPUT:\r\n");
    for (i = 0u; i < CFFT_INPUT_LEN * 2; i++)
    {
        PRINTF("[%4d]: 0x%8X \t", i, q31In[i]);
        if ( (i % 2u) == 1u)
        {
            PRINTF("          ");
        }
        PRINTF("%f\r\n", f32In[i]);
    }
    PRINTF("\r\n");
#endif
    for(i = 0; i < CFFT_INPUT_LEN * 2; i++)
    {
        /* PowerQuad的FFT只能处理27比特数 */
        q31InOut[i] = q31In[i] >> 5u;
    }


    /* The CMSIS function changes the input data, so the input data is initialized every loop. */
    instance.fftLen = CFFT_INPUT_LEN;/* 指定FFT采样点个数(复数的个数) */
    arm_cfft_q31(&instance, q31InOut, 0, 1); /* 开算 */

    /* 2B. */
    //for(i = 0; i < CFFT_INPUT_LEN * 2; i++)
    //{
        //q31InOut[i] = q31InOut[i] << 7u;
    //}
#if 1
    arm_q31_to_float(q31InOut, f32Out, CFFT_INPUT_LEN * 2); /* 鸟数转人数. */

    arm_cmplx_mag_f32(f32Out, f32In, CFFT_INPUT_LEN); /* 临时借用一下f32In的存储空间 */

    PRINTF("OUTPUT:\r\n");
    //for (i = 0u; i < CFFT_INPUT_LEN * 2; i++)
    for (i = 0u; i < CFFT_INPUT_LEN * 2; i++)
    {
        //f32Out[i] *= CFFT_INPUT_LEN;
        f32In[i] *= CFFT_INPUT_LEN;
        //PRINTF("[%4d]: %f\r\n", i, f32Out[i]);
        PRINTF("[%4d]: %f\r\n", i, f32In[i]);
    }
    PRINTF("\r\n");
#endif

}

/* CFFT辅助测试:
 * 1. 用浮点数计算得到信号波形
 * 2. 将浮点数转成定点数Q31
 * 3. 用转换后的Q31数进行CFFT
 * 4. 对CFFT结果转换成浮点数,同上一个函数进行对比
 */
void TEST_PQ_CFFT_Q31_F32(void)
{
    PRINTF("%s\r\n", __func__);

    arm_cfft_instance_q31 instance;
    float32_t f32In[CFFT_INPUT_LEN * 2], f32Out[CFFT_INPUT_LEN * 2];
    q31_t     q31In[CFFT_INPUT_LEN * 2], /* q31Out[CFFT_INPUT_LEN * 2], */ q31InOut[CFFT_INPUT_LEN * 2];
    uint32_t i;

    /* Generate the wave. */
    for (i = 0; i < CFFT_INPUT_LEN; i++)
    {
        f32In[i*2]    = arm_cos_f32(    (2.0f * PI / (CFFT_INPUT_LEN/2)) * i    ) / 1; /* 实部 */
        f32In[i*2+1]  = arm_sin_f32(    (2.0f * PI / (CFFT_INPUT_LEN/2)) * i    ) / 1; /* 虚部 */

        //q31In[i*2]       = (arm_cos_q31(i * (0x80000000 / (CFFT_INPUT_LEN / 2))) / 2);  /* 1/(N/2), 两个周期 */
        //q31In[i * 2 + 1] = (arm_sin_q31(i * (0x80000000 / (CFFT_INPUT_LEN / 2))) / 2);
    }

    arm_float_to_q31(f32In, q31In, CFFT_INPUT_LEN*2u); /* 人数转鸟数 */

    PRINTF("INPUT:\r\n");
    for (i = 0u; i < CFFT_INPUT_LEN * 2; i++)
    {
        PRINTF("[%4d]: 0x%8X \t", i, q31In[i]);
        if ( (i % 2u) == 1u )
        {
            PRINTF("          ");
        }
        PRINTF("%f\r\n", f32In[i]);
    }
    PRINTF("\r\n");

    for(i = 0; i < CFFT_INPUT_LEN * 2; i++)
    {
        /* PowerQuad的FFT只能处理27比特数 */
        //q31InOut[i] = q31In[i] >> 5u;
        q31InOut[i] = q31In[i] >> 5u;
    }


    /* The CMSIS function changes the input data, so the input data is initialized every loop. */
    instance.fftLen = CFFT_INPUT_LEN;/* 指定FFT采样点个数(复数的个数) */
    arm_cfft_q31(&instance, q31InOut, 0, 1); /* 开算 */

    /* 2B. */
    for(i = 0; i < CFFT_INPUT_LEN * 2; i++)
    {
        //q31InOut[i] = q31InOut[i] << 7u;
    }

    arm_q31_to_float(q31InOut, f32Out, CFFT_INPUT_LEN * 2); /* 鸟数转人数. */

    arm_cmplx_mag_f32(f32Out, f32In, CFFT_INPUT_LEN); /* 临时借用一下f32In的存储空间 */

    PRINTF("OUTPUT:\r\n");
    //for (i = 0u; i < CFFT_INPUT_LEN * 2; i++)
    for (i = 0u; i < CFFT_INPUT_LEN; i++)
    {
        //f32Out[i] = f32Out[i] * CFFT_INPUT_LEN;
        //PRINTF("[%4d]: 0x%8X \t %f\r\n", i, q31InOut[i], f32Out[i]);

        //f32In[i] = f32In[i] * CFFT_INPUT_LEN;
        f32In[i] *= 32; /* 因为FFT引擎只能处理27位,前文程序对输入数据进行了缩放(右移5位),此处需要还原 */
        PRINTF("[%4d]: %f\r\n", i, f32In[i]);
    }
}






/******************************************************************************/



#define FLOAT_2_Q31(x) ((int32_t)((x) * 2147483648.0f))

/* jason's test case. */
void _pq_cfft_q31Test(void)
{
    q31_t inout[CFFT_INPUT_LEN * 2];
    q31_t inputSave[CFFT_INPUT_LEN * 2];
    q31_t ref[CFFT_INPUT_LEN * 2] = { 0 };
    arm_cfft_instance_q31 instance = { .fftLen = CFFT_INPUT_LEN };

    //uint32_t allTime;
    //uint32_t memCopyTime;
    //uint32_t oldTime;

    /* Two full period sin wave. */
    for(uint32_t i=0; i<CFFT_INPUT_LEN / 2; i++) {
        inputSave[i * 2]     = (arm_sin_q31(i * (0x80000000 / (CFFT_INPUT_LEN / 2))) / 2) >> 5;
        inputSave[i * 2 + 1] = (arm_cos_q31(i * (0x80000000 / (CFFT_INPUT_LEN / 2))) / 2) >> 5;
    }
    memcpy(&inputSave[CFFT_INPUT_LEN], inputSave, sizeof(inputSave) / 2);

    /* Reference result. */
    ref[253] = FLOAT_2_Q31(0.5f / 32.0f); /* Imag(62) */

    /* Eliminate the time to copy memory. */
    //oldTime = TEST_GetTime();
    //for (uint32_t i = 0; i < FFT_TEST_LOOP; i++)
    {
        //memcpy(inout, inputSave, sizeof(inputSave));
    }
    //memCopyTime = TEST_GetTime() - oldTime;

    //oldTime = TEST_GetTime();
    //for (uint32_t i = 0; i < FFT_TEST_LOOP; i++)
    {
        /* The CMSIS function changes the input data, so the input data is initialized every loop. */
        memcpy(inout, inputSave, sizeof(inputSave));
        arm_cfft_q31(&instance, inout, 0, 1);
    }
    //allTime = TEST_GetTime() - oldTime;

    //PRINTF("%s: %d ms\r\n", __func__, allTime - memCopyTime);

    for (uint32_t i = 0; i < ARRAY_SIZE(ref); i++)
    {
        PRINTF("[%4d]:", i);
        //TEST_ASSERT_TRUE(abs(inout[i] - ref[i]) <= 20);
        if (abs(inout[i] - ref[i]) <= 20)
        {
            PRINTF("GOOD\r\n");
        }
        else
        {
            PRINTF("BAD\r\n");
        }
    }
}


/* EOF. */

