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


uint32_t gu32PowerQuadMarks[10];

static uint32_t App_PowerQuad_MatrixAdd_Mark(void);
static uint32_t App_PowerQuad_VectorCosF32(void);
static uint32_t App_PowerQuad_VectorCosQ31(void);

//static void PQ_VectorCosF32NoneTest(void);
static void PQ_VectorCosQ31Test(void);



/* Cos. */
void TEST_PQ_Cos_Q31(void);
void TEST_PQ_Cos_F32(void);

/* Sin. */
void TEST_PQ_Sin_F32(void);

/* Sqrt. */
void TEST_PQ_Sqrt_F32(void);
void TEST_PQ_Sqrt_Q31(void);

/* FFT. */
void TEST_PQ_CFFT_Q31(void);
void TEST_PQ_CFFT_Q31_F32(void);

/* Matrix. */
void TEST_PQ_Matrix_Addition(void);
void TEST_PQ_Matrix_Multiplication(void);
void TEST_PQ_Matrix_Inversion(void);

void App_PowerQuad_MarkAll(void)
{
    SYSCON_NS->AHBCLKCTRLX[2] |= 1u << 19u; /* enable the AHB clock to access PQ. */


    //PQ_Init(POWERQUAD_NS);


    //App_PowerQuad_VectorCosF32(); /* DONE */
    //App_PowerQuad_VectorCosQ31();


    //gu32PowerQuadMarks[0] = App_PowerQuad_MatrixAdd_Mark();

    //PQ_VectorCosF32NoneTest();
    //PQ_VectorCosQ31Test();


    /* Cos(x). */
    //TEST_PQ_Cos_Q31();
    //TEST_PQ_Cos_F32();

    /* Sin(x). */
    //TEST_PQ_Sin_F32();

    /* Sqrt(x). */
    //TEST_PQ_Sqrt_F32();
    //TEST_PQ_Sqrt_Q31();

    //TEST_PQ_CFFT_Q31();
    //TEST_PQ_CFFT_Q31_F32();
    //pq_cfft_q31Test();

    TEST_PQ_Matrix_Addition();
    TEST_PQ_Matrix_Multiplication();
    TEST_PQ_Matrix_Inversion();

}

/* Demo the usage of PQ_VectorCosF32(). */
static uint32_t App_PowerQuad_VectorCosF32(void)
{
    PRINTF("%s()\r\n", __func__);

    //float32_t f32Input[32];
    //float32_t f32Output[32];

    float32_t f32Input[32], f32Output[32];
    uint32_t i;

    //PQ_Init(POWERQUAD_NS);

    /* Prepare the input values. */
    for (i = 0u; i < 32; i++)
    {
        f32Input[i] = (PI / 6.0) * i;
    }

    //PQ_CosF32(f32Input, f32Output); /* f32Output = cos(f32Input); */
    PQ_VectorCosF32(f32Input, f32Output, 32);

    /* Powerquad & Compiler calc. */
    for (i = 0u; i < 32; i++)
    {
        PRINTF("f32Output[%d]\t: %f  \t%f\r\n", i, f32Output[i], cos(f32Input[i]) );
    }

    return 0u;
}


/* Demo the usage of PQ_VectorCosQ31(). */
static uint32_t App_PowerQuad_VectorCosQ31(void)
{
    float32_t f32Input[32], f32Output[32];
    q31_t     q31Input[32], q31Output[32];
    uint32_t  i;
    pq_prescale_t prescaleConfig;

    PRINTF("App_PowerQuad_VectorCosQ31()\r\n");


    /* Prepare the input values. */
    for (i = 0u; i < 32u; i++)
    {
        f32Input[i] = 0.1f * (i % 10u);
    }

    /* Convert the float number into Q31 number. */
    arm_float_to_q31 (f32Input, q31Input, 32u);

    PRINTF("\r\n");
    for (i = 0u; i < 32u; i++)
    {
        PRINTF("%f\t %d\r\n", f32Input[i], q31Input[i]);
    }

#if 0
    prescaleConfig.inputPrescale  = 0u;
    prescaleConfig.outputPrescale = 31u;
    prescaleConfig.outputSaturate = 0u;
    PQ_SetCoprocessorScaler(POWERQUAD_NS , &prescaleConfig);

    /* Cos() with Q31. */
    PQ_VectorCosQ31(q31Input, q31Output, 32);
#endif

    for (i = 0u;i < 32; i++)
    {
      q31Output[i] = App_CosQ31(q31Input[i]);
    }


    PRINTF("PQ_VectorCosQ31()\r\n\r\n\r\n");
    /* Convert the Q31 number into float number. */
    arm_q31_to_float(q31Output, f32Output, 32u);

    for (i = 0u; i < 32u; i++)
    {
        PRINTF("f32Output[%d]\t: %f  \t%d\r\n", i, f32Output[i], q31Output[i] /*, cos(f32Input[i]) */ );
        //PRINTF("%f\r\n", f32Output[i]);
        //PRINTF("f32Output[%d]\t: \r\n", i);
    }

    //
    return 0u;
}

static uint32_t App_PowerQuad_FFT(void)
{
    return 0u;
}




/* EOF. */

