/*
 * Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and
 * its subsidiaries and affiliates (collectly called MKSEMI).
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form, except as embedded into an MKSEMI
 *    integrated circuit in a product or a software update for such product,
 *    must reproduce the above copyright notice, this list of conditions and
 *    the following disclaimer in the documentation and/or other materials
 *    provided with the distribution.
 *
 * 3. Neither the name of MKSEMI nor the names of its contributors may be used
 *    to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * 4. This software, with or without modification, must only be used with a
 *    MKSEMI integrated circuit.
 *
 * 5. Any software provided in binary form under this license must not be
 *    reverse engineered, decompiled, modified and/or disassembled.
 *
 * THIS SOFTWARE IS PROVIDED BY MKSEMI "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL MKSEMI OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "mk_trace.h"
#include "mk_wdt.h"
#include "mk_reset.h"
#include "mk_calib.h"
#include "mk_lsp.h"
#include "libc_rom.h"
#include "board.h"
#include "stdio.h"

static float test_buf_a[256];
static float test_buf_b[256];
static float test_buf_c[256];

static int32_t test_buf_e[256];
static int32_t test_buf_f[256];

static void lsp_test(void)
{
    lsp_open(0, NULL);

    float *a = (float *)&test_buf_a[0];
    float *b = (float *)&test_buf_b[0];
    float *c = (float *)&test_buf_c[0];

    int8_t *x = (int8_t *)&test_buf_e[0];
    int8_t *y = (int8_t *)&test_buf_f[0];
    int16_t *zz = (int16_t *)&test_buf_c[0];

    int16_t *xx = (int16_t *)&test_buf_e[0];
    int16_t *yy = (int16_t *)&test_buf_f[0];
    int32_t *zzz = (int32_t *)&test_buf_c[0];

    for (uint32_t i = 0; i < 100; i++)
    {
        *a++ = 1.23f + ((float)i / 100.0f);
        *b++ = 0.45f + ((float)i / 100.0f);
        *c++ = 0;
    }
    mk_printf("\r\n ---- lsp_add_f32 ---- \r\n");

    // c[i] = a[i] + b[i]
    a = (float *)&test_buf_a[0];
    b = (float *)&test_buf_b[0];
    c = (float *)&test_buf_c[0];

    lsp_add_f32(a, b, c, 3);

    mk_printf("%f + %f = %f\r\n", a[0], b[0], c[0]);
    mk_printf("%f + %f = %f\r\n", a[1], b[1], c[1]);
    mk_printf("%f + %f = %f\r\n", a[2], b[2], c[2]);

    mk_printf("\r\n ---- lsp_sub_f32 ---- \r\n");
    // c[i] = a[i] - b[i]
    a = (float *)&test_buf_a[0];
    b = (float *)&test_buf_b[0];
    c = (float *)&test_buf_c[0];
    lsp_sub_f32(a, b, c, 3);

    mk_printf("%f - %f = %f\r\n", *a++, *b++, *c++);
    mk_printf("%f - %f = %f\r\n", *a++, *b++, *c++);
    mk_printf("%f - %f = %f\r\n", *a++, *b++, *c++);

    mk_printf("\r\n ---- lsp_mult_f32 ---- \r\n");
    // c[i] = a[i] * b[i]
    a = (float *)&test_buf_a[0];
    b = (float *)&test_buf_b[0];
    c = (float *)&test_buf_c[0];
    lsp_mult_f32(a, b, c, 3);

    mk_printf("%f * %f = %f\r\n", *a++, *b++, *c++);
    mk_printf("%f * %f = %f\r\n", *a++, *b++, *c++);
    mk_printf("%f * %f = %f\r\n", *a++, *b++, *c++);

    mk_printf("\r\n ---- lsp_vinverse_f32 ---- \r\n");
    // c[i] = 1.0 / a[i]
    a = (float *)&test_buf_a[0];
    c = (float *)&test_buf_c[0];
    lsp_vinverse_f32(a, c, 3);

    mk_printf("1.0 / %f = %f\r\n", *a++, *c++);
    mk_printf("1.0 / %f = %f\r\n", *a++, *c++);
    mk_printf("1.0 / %f = %f\r\n", *a++, *c++);

    mk_printf("\r\n ---- lsp_inner_product_f32 ---- \r\n");
    // c =  sum ( a[i] * b[i] )
    a = (float *)&test_buf_a[0];
    b = (float *)&test_buf_b[0];
    c = (float *)&test_buf_c[0];
    lsp_inner_product_f32(a, b, c, 3);

    mk_printf("%f * %f + %f * %f + %f * %f = %f\r\n", *a, *b, *(a + 1), *(b + 1), *(a + 2), *(b + 2), *c++);

    c = (float *)&test_buf_c[0];
    for (uint32_t i = 0; i < 100; i++)
    {
        *c++ = 0;
    }

    mk_printf("\r\n ---- lsp_scale_f32 ---- \r\n");
    // c[i] = scale * a[i]
    a = (float *)&test_buf_a[0];
    float scale = 0.5f;
    c = (float *)&test_buf_c[0];
    lsp_scale_f32(a, scale, c, 3);

    mk_printf("%f * %f = %f\r\n", *a++, scale, *c++);
    mk_printf("%f * %f = %f\r\n", *a++, scale, *c++);
    mk_printf("%f * %f = %f\r\n", *a++, scale, *c++);

    c = (float *)&test_buf_c[0];
    for (uint32_t i = 0; i < 100; i++)
    {
        *c++ = 0;
    }

    mk_printf("\r\n ---- lsp_cmplx_mult_cmplx_int8 ---- \r\n");
    // c[(2*n)+0] = a[(2*n)+0] * b[(2*n)+0] - a[(2*n)+1] * b[(2*n)+1];
    // c[(2*n)+1] = a[(2*n)+0] * b[(2*n)+1] + a[(2*n)+1] * b[(2*n)+0];
    for (int8_t i = 0; i < 100; i++)
    {
        *x++ = i;
        *y++ = i;
    }

    x = (int8_t *)&test_buf_e[0];
    y = (int8_t *)&test_buf_f[0];
    zz = (int16_t *)&test_buf_c[0];
    lsp_cmplx_mult_cmplx_int8(x, y, zz, 16, 0);

    for (int i = 0; i < 16; i++)
    {
        mk_printf("(%d + %di) * (%d + %di) = %d + %di\r\n", x[2 * i], x[2 * i + 1], y[2 * i], y[2 * i + 1], zz[2 * i], zz[2 * i + 1]);
    }

    c = (float *)&test_buf_c[0];
    for (uint32_t i = 0; i < 100; i++)
    {
        *c++ = 0;
    }

    mk_printf("\r\n ---- lsp_cmplx_mult_cmplx_int16 ---- \r\n");
    // c[(2*n)+0] = a[(2*n)+0] * b[(2*n)+0] - a[(2*n)+1] * b[(2*n)+1];
    // c[(2*n)+1] = a[(2*n)+0] * b[(2*n)+1] + a[(2*n)+1] * b[(2*n)+0];
    for (int16_t i = 0; i < 100; i++)
    {
        *xx++ = i;
        *yy++ = i;
    }

    xx = (int16_t *)&test_buf_e[0];
    yy = (int16_t *)&test_buf_f[0];
    zzz = (int32_t *)&test_buf_c[0];
    lsp_cmplx_mult_cmplx_int16(xx, yy, zzz, 16, 0);

    for (int i = 0; i < 16; i++)
    {
        mk_printf("(%d + %di) * (%d + %di) = %d + %di\r\n", xx[2 * i], xx[2 * i + 1], yy[2 * i], yy[2 * i + 1], zzz[2 * i], zzz[2 * i + 1]);
    }

    c = (float *)&test_buf_c[0];
    for (uint32_t i = 0; i < 100; i++)
    {
        *c++ = 0;
    }

    mk_printf("\r\n ---- lsp_cmplx_mult_cmplx_f32 ---- \r\n");
    // c[(2*n)+0] = a[(2*n)+0] * b[(2*n)+0] - a[(2*n)+1] * b[(2*n)+1];
    // c[(2*n)+1] = a[(2*n)+0] * b[(2*n)+1] + a[(2*n)+1] * b[(2*n)+0];
    a = (float *)&test_buf_a[0];
    b = (float *)&test_buf_b[0];
    c = (float *)&test_buf_c[0];
    lsp_cmplx_mult_cmplx_f32(a, b, c, 3, 0);
    mk_printf("(%f + %fi) * (%f + %fi) = %f + %fi\r\n", *a, *(a + 1), *b, *(b + 1), *c, *(c + 1));
    mk_printf("(%f + %fi) * (%f + %fi) = %f + %fi\r\n", *(a + 2), *(a + 3), *(b + 2), *(b + 3), *(c + 2), *(c + 3));
    mk_printf("(%f + %fi) * (%f + %fi) = %f + %fi\r\n", *(a + 4), *(a + 5), *(b + 4), *(b + 5), *(c + 4), *(c + 5));

    c = (float *)&test_buf_c[0];
    for (uint32_t i = 0; i < 100; i++)
    {
        *c++ = 0;
    }

    mk_printf("\r\n ---- lsp_cmplx_inner_product_int8 ---- \r\n");
    // z =  sum ( x[i] * y[i] )
    for (int8_t i = 0; i < 10; i++)
    {
        *x++ = i;
        *y++ = i;
    }

    x = (int8_t *)&test_buf_e[0];
    y = (int8_t *)&test_buf_f[0];
    zzz = (int32_t *)&test_buf_c[0];

    lsp_cmplx_inner_product_int8(x, y, zzz, 4, 0);

    mk_printf("(%d + %di) * (%d + %di) + ", x[0], x[1], y[0], y[1]);
    mk_printf("(%d + %di) * (%d + %di) + ", x[2], x[3], y[2], y[3]);
    mk_printf("(%d + %di) * (%d + %di) + ", x[4], x[5], y[4], y[5]);
    mk_printf("(%d + %di) * (%d + %di) = ", x[6], x[7], y[6], y[7]);
    mk_printf("%d + %di\r\n", zzz[0], zzz[1]);

    c = (float *)&test_buf_c[0];
    for (uint32_t i = 0; i < 100; i++)
    {
        *c++ = 0;
    }

    mk_printf("\r\n ---- lsp_cmplx_inner_product_int16 ---- \r\n");
    // c =  sum ( x[i] * y[i] )
    for (int8_t i = 0; i < 10; i++)
    {
        *xx++ = i;
        *yy++ = i;
    }

    xx = (int16_t *)&test_buf_e[0];
    yy = (int16_t *)&test_buf_f[0];
    c = (float *)&test_buf_c[0];
    lsp_cmplx_inner_product_int16(xx, yy, c, 4, 0);

    mk_printf("(%d + %di) * (%d + %di) + ", x[0], x[1], y[0], y[1]);
    mk_printf("(%d + %di) * (%d + %di) + ", x[2], x[3], y[2], y[3]);
    mk_printf("(%d + %di) * (%d + %di) + ", x[4], x[5], y[4], y[5]);
    mk_printf("(%d + %di) * (%d + %di) = ", x[6], x[7], y[6], y[7]);
    mk_printf("%f + %fi\r\n", c[0], c[1]);

    c = (float *)&test_buf_c[0];
    for (uint32_t i = 0; i < 100; i++)
    {
        *c++ = 0;
    }

    mk_printf("\r\n ---- lsp_cmplx_inner_product_f32 ---- \r\n");
    // c =  sum ( a[i] * b[i] )
    a = (float *)&test_buf_a[0];
    b = (float *)&test_buf_b[0];
    c = (float *)&test_buf_c[0];
    lsp_cmplx_inner_product_f32(a, b, c, 3, 0);

    mk_printf("(%f + %fi) * (%f + %fi) + ", a[0], a[1], b[0], b[1]);
    mk_printf("(%f + %fi) * (%f + %fi) + ", a[2], a[3], b[2], b[3]);
    mk_printf("(%f + %fi) * (%f + %fi) = ", a[4], a[5], b[4], b[5]);
    mk_printf("%f + %fi\r\n", c[0], c[1]);

    c = (float *)&test_buf_c[0];
    for (uint32_t i = 0; i < 100; i++)
    {
        *c++ = 0;
    }

    mk_printf("\r\n ---- lsp_cmplx_scale_f32 ---- \r\n");
    // c[i] = scale * a[i]
    a = (float *)&test_buf_a[0];
    float re_scale = 0.5f;
    float im_scale = 0.1f;
    c = (float *)&test_buf_c[0];
    lsp_cmplx_scale_f32(a, re_scale, im_scale, c, 3, 0);

    mk_printf("(%f + %fi) * (%f + %fi) = %f + %fi\r\n", *a, *(a + 1), re_scale, im_scale, *c, *(c + 1));
    mk_printf("(%f + %fi) * (%f + %fi) = %f + %fi\r\n", *(a + 2), *(a + 3), re_scale, im_scale, *(c + 2), *(c + 3));
    mk_printf("(%f + %fi) * (%f + %fi) = %f + %fi\r\n", *(a + 4), *(a + 5), re_scale, im_scale, *(c + 4), *(c + 5));
}

int main(void)
{
    board_clock_run();
    board_pins_config();
    board_debug_console_open(TRACE_PORT_UART0);
    // Reset reason
    reset_cause_get();
    reset_cause_clear();

    // Chip calibration
    calib_chip();

    // Disable watchdog timer
    wdt_close(WDT_ID0);
    LOG_INFO(TRACE_MODULE_APP, "LSP example start\r\n");

    gpio_open();
    board_configure();

    lsp_test();

    LOG_INFO(TRACE_MODULE_APP, "LSP example end\r\n");
    while (1)
    {
    }
}