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

/*
* 1. Niobe4的PowerQuad不能算定点数,必须转化成浮点数装入PQ后方能正确计算(workaround)
* 2. 计算结果需要进行移位操作方能得到正确数量级的数
* 3. 计算完成后直接返回定点数
*
* PS: CosQ31的Vector函数也需要修改.否则不能输出正常的计算结果.
* PS: 对于三角函数的输入参数:
*       - 若为定点数计算,传入参数为弧度制,有效值为(-1, 1),对应的是(-Pi, Pi)
*       - 若为浮点数计算,传入参数为弧度值,直接对应有效范围为(-PI, PI)
*/
q31_t _PQ_CosQ31(q31_t x)
{
    PQFLT val;
    uint32_t cppre; /* keep pre reg. */

    /* 于大神试出来的关于移位的workaround */
    cppre = POWERQUAD_NS->CPPRE;
    POWERQUAD_NS->CPPRE = (31u) << 8u; //POWERQUAD_NS->CPPRE = POWERQUAD_CPPRE_CPPRE_OUT(31);

    val.for_cp = 0x30c90fdb;
    val.x =  (float) x *  val.x;

    _pq_cos0(val.for_cp); // _pq_sin0(val.for_cp);
    val.for_cp = _pq_readAdd0();
    val.for_cp = _pq_readAdd0_fx();

    POWERQUAD_NS->CPPRE = cppre;

    return val.for_cp;
    //return ret;
}

#define TEST_PQ_COS_DATA_LENGTH  40u
void TEST_PQ_Cos_Q31(void)
{
    uint32_t i;
    float32_t f32In[TEST_PQ_COS_DATA_LENGTH], f32Out[TEST_PQ_COS_DATA_LENGTH];
    q31_t     q31In[TEST_PQ_COS_DATA_LENGTH], q31Out[TEST_PQ_COS_DATA_LENGTH];

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

    for (i = 0u; i < TEST_PQ_COS_DATA_LENGTH; i++)
    {
        f32In[i] = -1.0f + (0.1f * (i % 20u) );
    }
    arm_float_to_q31(f32In, q31In, TEST_PQ_COS_DATA_LENGTH); /* convert numbers from float to q31. */

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

    /* powerquad calc. */
    for (i = 0u; i < TEST_PQ_COS_DATA_LENGTH; i++)
    {
        q31Out[i] = _PQ_CosQ31(q31In[i]);
    }
    arm_q31_to_float(q31Out, f32Out, TEST_PQ_COS_DATA_LENGTH); /* convert numbers from q31 to float. */

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

void TEST_PQ_Cos_F32(void)
{
    uint32_t i;
    float32_t f32In[TEST_PQ_COS_DATA_LENGTH], f32Out[TEST_PQ_COS_DATA_LENGTH];

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

    for (i = 0u; i < TEST_PQ_COS_DATA_LENGTH; i++)
    {
        //f32In[i] = -1.0f + (0.1f * (i % 20u) );   /* 假定有效输入参数是(-1, 1) */
        f32In[i] = -PI + (PI/10.0 * (i % 20u) );   /* 假定有效输入参数是(-PI, PI), Bingo. */
        //f32In[i] = -180.0f+ ( 20.0f * (i % 18) );
    }

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

    /* powerquad calc. */
    for (i = 0u; i < TEST_PQ_COS_DATA_LENGTH; i++)
    {
        //PRINTF("[%2d]: %f\r\n", i, f32In[i]);
        PQ_CosF32(&f32In[i], &f32Out[i]);
    }

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

/* EOF. */
