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

Copyright(c) 2019 by Aerospace C.Power (Chongqing) Microelectronics. ALL RIGHTS RESERVED.

This Information is proprietary to Aerospace C.Power (Chongqing) Microelectronics and MAY NOT
be copied by any method or incorporated into another program without
the express written consent of Aerospace C.Power. This Information or any portion
thereof remains the property of Aerospace C.Power. The Information contained herein
is believed to be accurate and Aerospace C.Power assumes no responsibility or
liability for its use in any way and conveys no license or title under
any patent or copyright and makes no representation or warranty that this
Information is free from patent or copyright infringement.

****************************************************************************/
/* os shim includes */
#include "os_types.h"
#include "os_utils.h"
#include "os_mem.h"
#include "iot_diag.h"
#include "iot_io.h"
#include "dbg_io.h"
#include "hw_reg_api.h"
#include "ahb.h"
#include "iot_uart_api.h"

#include "iot_matrix_api.h"
#include "chol_hw.h"
#include "input.h"

/****************************************************************************/
// test case config
#define INVERSE             (1 << 0)
#define DETERMINANT         (1 << 1)
#define INVERSE_DETERMINANT (1 << 2)
#define CHOLESKY            (1 << 3)
#define OP_TYPE_ALL (INVERSE | DETERMINANT | INVERSE_DETERMINANT | CHOLESKY)
#define TEST_CASE_CHOL  (OP_TYPE_ALL)

#define EIGENVALUE_VECTOR   (1 << 0)
#define TEST_CASE_EIG   (EIGENVALUE_VECTOR)

#define MTX_MUL_AB          (1 << 0)
#define MTX_MUL_ABC         (1 << 1)
#define MTX_MUL_ABCD        (1 << 2)
#define MTX_MUL_TEST        (1 << 3)
#define MTX_MUL_DIAG        (1 << 4)
#define TEST_CMM_ALL        (MTX_MUL_AB | MTX_MUL_ABC \
                            | MTX_MUL_ABCD | MTX_MUL_TEST | MTX_MUL_DIAG)
#define TEST_CASE_CMM       (TEST_CMM_ALL)

/****************************************************************************/
// test data
typedef struct _mtx_format_t {
    float real;
    float imag;
} mtx_format_t;

mtx_format_t test_m_4x4[16] = {
    {2.0, 0}, {0, 0}, {0, 0}, {0, 0},
    {0, 0}, {2.0, 0}, {0, 0}, {0, 0},
    {0, 0}, {0, 0}, {2.0, 0}, {0, 0},
    {0, 0}, {0, 0}, {0, 0}, {2.0, 0},
};

mtx_format_t test_m_6x6[36] = {
    {2.0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0},
    {0, 0}, {2.0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0},
    {0, 0}, {0, 0}, {2.0, 0}, {0, 0}, {0, 0}, {0, 0},
    {0, 0}, {0, 0}, {0, 0}, {2.0, 0}, {0, 0}, {0, 0},
    {0, 0}, {0, 0}, {0, 0}, {0, 0}, {2.0, 0}, {0, 0},
    {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {2.0, 0},
};

mtx_format_t test_m_b_4x4[16] = {
    {3.0, 0}, {0, 0}, {0, 0}, {0, 0},
    {0, 0}, {3.0, 0}, {0, 0}, {0, 0},
    {0, 0}, {0, 0}, {3.0, 0}, {0, 0},
    {1, 0}, {1, 0}, {1, 0}, {3.0, 0},
};

mtx_format_t test_adiag_5x5[16] = {
    {2.0, 213.123}, {2, 0.21379}, {3, 123.67777}, {4, -110}, {5, 0.000213},
};

mtx_format_t test_bdiag_5x5[16] = {
    {100, 2384.1037}, {-213, -0.12}, {0.213, 1203.12}, {-324, 0.123}, {0.0000345, 1},
};

mtx_format_t test_det_4x4[16] = {
    {0.000073359617,0.000000000000},{0.000072876348,0.000000000000},
    {0.000049108014,0.000000000000},{0.000033062126,0.000000000000},
    {0.000072876348,0.000000000000},{0.000006834176,0.000000000000},
    {0.000025137975,0.000000000000},{0.000003169524,0.000000000000},
    {0.000049108014,0.000000000000},{0.000025137975,0.000000000000},
    {0.000004611953,0.000000000000},{-0.000008321789,0.000000000000},
    {0.000033062126,0.000000000000},{0.000003169524,0.000000000000},
    {-0.000008321789,0.000000000000},{-0.000014774589,0.000000000000},
};

mtx_format_t eigen_src_4x4[16] = {
    {0.000013537996,0.000000000000},{0.000021811342,0.000007851141},
    {0.000015675963,-0.000000539224},{0.000019629195,0.000001672964},
    {0.000021811342,-0.000007851141},{0.000030159135,0.000000000000},
    {0.000023983768,-0.000008469722},{0.000029418617,-0.000006122700},
    {0.000015675963,0.000000539224},{0.000023983768,0.000008469722},
    {0.000018281047,0.000000000000},{0.000022573047,0.000002196881},
    {0.000019629195,-0.000001672964},{0.000029418617,0.000006122700},
    {0.000022573047,-0.000002196881},{0.000027791539,0.000000000000},
};

uint32_t test_det_hex_4x4[32] = {
0x3899d8a5,0x00000000,
0x3898d531,0x00000000,
0x384df954,0x00000000,
0x380aac30,0x00000000,
0x3898d531,0x00000000,
0x36e55120,0x00000000,
0x37d2df64,0x00000000,
0x3654b402,0x00000000,
0x384df954,0x00000000,
0x37d2df64,0x00000000,
0x369ac060,0x00000000,
0xb70b9dd0,0x00000000,
0x380aac30,0x00000000,
0x3654b402,0x00000000,
0xb70b9dd0,0x00000000,
0xb777e060,0x00000000
};

float src[8]={0.012300251983, 0.000000000000, 0.017685770988, 0.000000000000,
    0.030364101753, 0.000000000000, 0.023820413277, 0.000000000000};
float src1[8]={0.012300251983, 0.000000000000, 0.017685770988, 0.000000000000,
    0.030364101753, 0.000000000000, 0.023820413277, 0.000000000000};

mtx_format_t test_det_tran_4x4[16] = {0};

mtx_format_t result[100] = {0};
mtx_format_t g_det[100] = {0};
mtx_format_t g_norm[100] = {0};
mtx_format_t g_eigval[100] = {0};

static void output_clear()
{
    for(uint32_t i = 0; i < 100; i++) {
        result[i].imag = 0;
        result[i].real = 0;
    }
}

static void dump_output()
{
    for(uint32_t i = 0; i < 100; i++) {
        iot_printf("(%04f, %04f) ", result[i].real, result[i].imag);
        if (i%4 == 3) {
            iot_printf("\n");
        }
    }
}

void chol_test()
{
    // test case : input a simple 4x4 matrix - test_m_4x4, and repeat count is 1
    // test inverse, determinant and cholesky
    iot_printf("[chol test]\n");
    uint32_t i = 0;
    iot_printf("test matrix data:\n");
    for(i = 0; i < 16; i++) {
        iot_printf("(%04f, %04f) ", test_m_4x4[i].real, test_m_4x4[i].imag);
        if (i%4 == 3) {
            iot_printf("\n");
        }
    }

#if (TEST_CASE_CHOL & INVERSE)
    output_clear();
    iot_math_mtx_inverse((float *)result,  (float*)test_m_4x4, CHOL_SIZE_4X4, 1);
    iot_printf("INVERSE case output 4x4 matrix\n");
    dump_output();
#endif

#if (TEST_CASE_CHOL & DETERMINANT)
    output_clear();
    iot_math_mtx_det((float *)result, (float*)test_m_4x4, CHOL_SIZE_4X4, 1);
    iot_printf("DETERMINANT case ouput 1 word data:\n");
    dump_output();
#endif

#if (TEST_CASE_CHOL & INVERSE_DETERMINANT)
    output_clear();
    iot_math_mtx_inv_det((float *)result, (float *)g_det,
            (float*)test_m_4x4, CHOL_SIZE_4X4, 1);
    iot_printf("INVERSE_DETERMINANT case output 4x4 matrix:\n");
    dump_output();
#endif

#if (TEST_CASE_CHOL & CHOLESKY)
    output_clear();
    iot_math_mtx_cholesky((float *)result, (float*)test_m_4x4, CHOL_SIZE_4X4, 1);
    iot_printf("CHOLESKY case output 4x4 matrix:\n");
    dump_output();
#endif
}

void eig_test()
{
    // test case : input a 4x4 matrix - test_m_4x4, and repeat count is 1
    // test eigenvalue of maximum and eigenvector
    iot_printf("[eigen test]\n");
    uint32_t i = 0;
    iot_printf("test matrix data:\n");
    for(i = 0; i < 16; i++) {
        iot_printf("(%04f, %04f) ", test_m_4x4[i].real, test_m_4x4[i].imag);
        if (i%4 == 3) {
            iot_printf("\n");
        }
    }

#if (TEST_CASE_EIG & EIGENVALUE_VECTOR)
    output_clear();
    iot_math_mtx_eig((float *)result, (float *)g_norm, (float *)g_eigval,
            (float*)test_m_4x4, CHOL_SIZE_4X4, 1);
    iot_printf("EIGENVALUE_VECTOR case ouput 4x1 vector norm^2 and value:\n");
    dump_output();
#endif
}

void cmm_test()
{
#if 1
    // test case : input a 4x4 matrix - test_m_4x4, and repeat count is 1
    // test matrix multiplication
    iot_printf("[matrix mul test]\n");
    uint32_t i = 0;
    iot_printf("test matrix A data:\n");
    for(i = 0; i < 16; i++) {
        iot_printf("(%04f, %04f) ", test_m_4x4[i].real, test_m_4x4[i].imag);
        if (i%4 == 3) {
            iot_printf("\n");
        }
    }
    iot_printf("test matrix B data:\n");
    for(i = 0; i < 16; i++) {
        iot_printf("(%04f, %04f) ", test_m_b_4x4[i].real, test_m_b_4x4[i].imag);
        if (i%4 == 3) {
            iot_printf("\n");
        }
    }
#if (TEST_CASE_CMM & MTX_MUL_AB)
    iot_math_cmm_cfg((float *) test_m_4x4, 4, 4, 0,
            (float *)test_m_b_4x4, 4, 4, 0);
    // AxB
    output_clear();
    iot_math_cmm_ab((float *)result, CMM_CONV_A, CMM_CONV_B, 1);
    iot_printf("EIGENVALUE_VECTOR AxB case 4x4 matrix:\n");
    dump_output();

    output_clear();
    iot_math_cmm_ab((float *)result, CMM_CONV_A, CMM_CONV_B_TRAN, 1);
    iot_printf("EIGENVALUE_VECTOR AxB' case 4x4 matrix:\n");
    dump_output();
#endif

#if (TEST_CASE_CMM& MTX_MUL_ABC)
    output_clear();

    iot_math_cmm_cfg((float *) test_m_4x4, 4, 4, 0,
            (float *)test_m_b_4x4, 4, 4, 0);
    // AxBxA
    iot_math_cmm_abc((float *)result,
        CMM_CONV_A, CMM_CONV_B, CMM_CONV_C, CMM_CONV_A, 1);

    iot_printf("EIGENVALUE_VECTOR AxBxA case 4x4 matrix:\n");
    dump_output();
#endif

#if (TEST_CASE_CMM& MTX_MUL_ABCD)
    output_clear();

    iot_math_cmm_cfg((float *) test_m_4x4, 4, 4, 0,
            (float *)test_m_b_4x4, 4, 4, 0);
    // (AxB)xA.'xB.'
    //iot_math_cmm_ab((float *)result, CMM_CONV_A, CMM_CONV_B, 1);
    iot_math_cmm_abc((float *)result,
        CMM_CONV_A, CMM_CONV_B, CMM_CONV_C, CMM_CONV_C_CONJ_TRAN, 1);

    iot_printf("EIGENVALUE_VECTOR AxBxA.'xB.' case 4x4 matrix:\n");
    dump_output();
#endif

#if (TEST_CASE_CMM & MTX_MUL_TEST)
    iot_math_cmm_cfg((float *) src, 4, 1, 0,
            (float *)src1, 1, 4, 0);
    // AxB
    output_clear();
    iot_math_cmm_ab((float *)result, CMM_CONV_A, CMM_CONV_B, 1);
    iot_printf("EIGENVALUE_VECTOR AxB case 4x4 matrix:\n");
    dump_output();
#endif

#if (TEST_CASE_CMM & MTX_MUL_DIAG)
    iot_math_cmm_cfg((float *) test_adiag_5x5, 5, 5, 1,
            (float *)test_bdiag_5x5, 5, 5, 1);
    // AxB
    output_clear();
    iot_math_cmm_ab((float *)result, CMM_CONV_A, CMM_CONV_B, 1);
    iot_printf("EIGENVALUE_VECTOR AxB case 4x4 matrix:\n");
    dump_output();
#endif

#endif
}

int main(void)
{
    dbg_uart_init();

    //iot_dbg_uart_set_port(IOT_UART_PORT0, 3000000, 0, 8, 1);

    iot_printf("\n-------MATRIX TEST---------\n");

    while(1) {
        chol_test();
        eig_test();
        cmm_test();
    }
    iot_printf("\n-------MATRIX TEST FINISH---------\n");
    while(1);

    return 0;
}