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

#define TEST_PQ_SQRT_DATA_LENGTH  40u

/* 浮点数计算Sqrt */
void TEST_PQ_Sqrt_F32(void)
{
    PRINTF("%s()\r\n\r\n", __func__);

    float32_t f32In[TEST_PQ_SQRT_DATA_LENGTH], f32Out[TEST_PQ_SQRT_DATA_LENGTH];
    uint32_t i;

    for (i = 0u; i < TEST_PQ_SQRT_DATA_LENGTH; i++)
    {
        f32In[i] = (0.1 * (i % 10u));
        f32In[i] *= f32In[i];
    }

    PRINTF("INPUT:\r\n");
    for (i = 0u; i < TEST_PQ_SQRT_DATA_LENGTH; i++)
    {
        PRINTF("[%2d]: %f\r\n", i, f32In[i]);
    }
    PRINTF("\r\n");

    PQ_VectorSqrtF32(f32In, f32Out, TEST_PQ_SQRT_DATA_LENGTH);

    PRINTF("OUTPUT:\r\n");
    for (i = 0u; i < TEST_PQ_SQRT_DATA_LENGTH; i++)
    {
        PRINTF("[%2d]: %f\r\n", i, f32Out[i]);
    }
    PRINTF("\r\n");
}

/* 定点数计算Sqrt */
void TEST_PQ_Sqrt_Q31(void)
{
    PRINTF("%s()\r\n\r\n", __func__);

    float32_t f32In[TEST_PQ_SQRT_DATA_LENGTH], f32Out[TEST_PQ_SQRT_DATA_LENGTH];
    q31_t     q31In[TEST_PQ_SQRT_DATA_LENGTH], q31Out[TEST_PQ_SQRT_DATA_LENGTH];
    uint32_t i;
    uint32_t cppre; /* keep pre reg. */

    for (i = 0u; i < TEST_PQ_SQRT_DATA_LENGTH; i++)
    {
        f32In[i] = (0.1 * (i % 10u));
        f32In[i] *= f32In[i];
    }

    arm_float_to_q31(f32In, q31In, TEST_PQ_SQRT_DATA_LENGTH);

    PRINTF("INPUT:\r\n");
    for (i = 0u; i < TEST_PQ_SQRT_DATA_LENGTH; i++)
    {
        PRINTF("[%2d]: %f  \t %d\r\n", i, f32In[i], q31In[i]);
    }
    PRINTF("\r\n");

    /* 于大神试出来的关于移位的workaround */
    cppre = POWERQUAD_NS->CPPRE;
    POWERQUAD_NS->CPPRE = POWERQUAD_CPPRE_CPPRE_IN(31); /* 在输入数据上左移31位数. */

    PQ_VectorSqrtQ31(q31In, q31Out, TEST_PQ_SQRT_DATA_LENGTH);
    POWERQUAD_NS->CPPRE = cppre;

    arm_q31_to_float(q31Out, f32Out, TEST_PQ_SQRT_DATA_LENGTH);

    PRINTF("OUTPUT:\r\n");
    for (i = 0u; i < TEST_PQ_SQRT_DATA_LENGTH; i++)
    {
        PRINTF("[%2d]: %f  \t %d\r\n", i, f32Out[i], q31Out[i]);
    }
    PRINTF("\r\n");
}

/* EOF. */

