#include "os_types.h"
#include "os_mem_api.h"
#include "dbg_io.h"
#include "iot_diag.h"
#include "iot_io.h"

#include "ahb_hw.h"
#include "afft_hw.h"

#include "iot_afft_api.h"
#include "iot_simd_api.h"
#include "cpu.h"
#include "math.h"

#include "fft_input_real.h"
#include "ifft_input_real.h"
#include "fft_output_real.h"
#include "ifft_output_real.h"

#include "fft_input_complex.h"
#include "ifft_input_complex.h"
#include "fft_output_complex.h"
#include "ifft_output_complex.h"

// test different data mode
#include "input_real.h"
#include "input_ireal.h"

#define AFFT_FFT_REAL_24     (1 << 0)
#define AFFT_IFFT_REAL_24    (1 << 1)
#define AFFT_FFT_COMPLEX_24  (1 << 2)
#define AFFT_IFFT_COMPLEX_24 (1 << 3)
#define AFFT_ALL            (AFFT_FFT_REAL_24 | AFFT_IFFT_REAL_24 \
                            | AFFT_FFT_COMPLEX_24 | AFFT_IFFT_COMPLEX_24)
#define TEST_CASE_AFFT      (AFFT_ALL)

#define AFFT_FFT_LOW_24     (1 << 0)
#define AFFT_FFT_HIGH_24    (1 << 1)
#define AFFT_FFT_16_REAL    (1 << 2)
#define AFFT_FFT_FLOAT      (1 << 3)
#define AFFT_IFFT_LOW_24    (1 << 4)
#define AFFT_IFFT_HIGH_24   (1 << 5)
#define AFFT_IFFT_16_REAL   (1 << 6)
#define AFFT_IFFT_FLOAT     (1 << 7)
#define AFFT_TEST_DM_ALL    (AFFT_IFFT_LOW_24 | AFFT_IFFT_HIGH_24 | \
        AFFT_IFFT_16_REAL |AFFT_IFFT_FLOAT)

#define TEST_CASE_DATA_MODE (AFFT_FFT_FLOAT)

typedef struct _mcycle_cnt {
    uint64_t fft_float;     // 512w fft float cost
    uint64_t sw_dma_itoi;   // 512w
    uint64_t sw_dma_ftoi;
    uint32_t sw_dma_itof;
} mcycle_cnt_t;

typedef struct _cycle_cpy {
    uint64_t memcpy_ram2ram;
    uint64_t memcpy_ram2fft;
    uint64_t memcpy_fft2ram;
    uint64_t simd_ram2ram;
    uint64_t simd_ram2fft;
    uint64_t simd_fft2ram;
    uint64_t swdma_ram2ram;
    uint64_t swdma_ram2fft;
    uint64_t swdma_fft2ram;
} mcycle_cpy_t;

mcycle_cnt_t g_mcycle_cnt = {0};

mcycle_cpy_t g_mcycle_cpy = {0};

uint32_t result[2048] = {0};

void output_compare(uint32_t *dst, uint32_t *src, uint32_t size)
{
    uint32_t s = 0;
    uint32_t d = 0;
    uint32_t inc = 0;
    for(uint32_t i = 0; i < size; i++) {
        s = (*(src + i) & ~0xff000000);
        d = (*(dst + i) & ~0xff000000);
        if (s >= d) {
            inc = s - d;
        } else {
            inc = d - s;
        }

        if (inc > 5) {
            iot_printf("error i: %d, val: [0x%08x-0x%08x], inc: %d\n",
                i, s, d, inc);
            //return;
        }

    }

    iot_printf("successful.....\n");
}

void output_print(uint32_t *dst, uint32_t *src, uint32_t size)
{
    for(uint32_t i = 0; i < size; i++) {
        int32_t tmp;
        tmp = (int32_t )(*(dst+i));
        iot_printf("%08x\n", tmp);
    }

    iot_printf("successful.....\n");
}

void afft_test()
{
#if 0
    volatile uint32_t *reg = (volatile uint32_t *) 0x56604000;
#endif
#if (TEST_CASE_AFFT & AFFT_FFT_REAL_24)
    iot_printf("afft test fft 2K 24bit real data\n");
#if 0
    // afft config
    afft_cfg(AFFT_OP_FFT, AFFT_FMT_LOW_24BIT, AFFT_DATA_REAL, AFFT_REAL_2048, 1, 0, 5);

    // data write
    for(uint32_t i = 0; i < 2048; i++){
        *(reg + i) = fft_input_real[i];
    }

    afft_start();

    while(!afft_get_done_bit());

    afft_clr_done_bit();

    output_compare((uint32_t *)reg, fft_output_real, 2048);
#else
    os_mem_set(result, 0, 2048);
    iot_afft_real_fft(result, fft_input_real, 2048,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_2048, 1, 0, 5);
    output_compare((uint32_t *)result, fft_output_real, 2048);
#endif

    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif

#if (TEST_CASE_AFFT & AFFT_IFFT_REAL_24)
#if 0
    iot_printf("afft test ifft 2K 24bit real data\n");
    // afft config
    afft_cfg(AFFT_OP_IFFT, AFFT_FMT_LOW_24BIT, AFFT_DATA_REAL, AFFT_REAL_2048, 0, 1, 0);

    // data write
    for(uint32_t i = 0; i < 2048; i++){
        *(reg + i) = ifft_input_real[i];
    }

    afft_start();

    while(!afft_get_done_bit());
    afft_clr_done_bit();

    output_compare((uint32_t *)reg, ifft_output_real, 2048);
#else
    os_mem_set(result, 0, 2048);
    iot_afft_real_ifft(result, ifft_input_real, 2048,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_2048, 0, 1, 0);
    output_compare((uint32_t *)result, ifft_output_real, 2048);
#endif

    iot_printf("AFFT_IFFT_REAL_24 done....\n");
#endif


#if (TEST_CASE_AFFT & AFFT_FFT_COMPLEX_24)
    iot_printf("afft test fft 2K 24bit complex data\n");
#if 0
    // afft config
    afft_cfg(AFFT_OP_FFT, AFFT_FMT_LOW_24BIT,
            AFFT_DATA_COMPLEX, AFFT_COMPLEX_1024, 1, 0, 5);

    // data write
    for(uint32_t i = 0; i < 2048; i++){
        //*(reg + i) = fft_input_complex[i];
        if (i % 2 == 1) {
            *(reg + i) = 0;
        } else {
            *(reg + i) = fft_input_complex[i/2];
        }
    }

    afft_start();

    while(!afft_get_done_bit());
    afft_clr_done_bit();

    output_compare((uint32_t *)reg, fft_output_complex, 2048);
#else
    os_mem_set(result, 0, 2048);
    iot_afft_complex_fft(result, fft_input_complex, 2048,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_1024, 1, 0, 5);
    output_compare((uint32_t *)result, fft_output_complex, 2048);
#endif
    iot_printf("AFFT_FFT_COMPLEX_24 done....\n");
#endif


#if (TEST_CASE_AFFT & AFFT_IFFT_COMPLEX_24)
    iot_printf("afft test ifft 2K 24bit complex data\n");
#if 0
    // afft config
    afft_cfg(AFFT_OP_IFFT, AFFT_FMT_LOW_24BIT,
            AFFT_DATA_COMPLEX, AFFT_COMPLEX_1024);

    // data write
    for(uint32_t i = 0; i < 2048; i++){
        *(reg + i) = ifft_input_complex[i];
    }

    afft_start();

    while(!afft_get_done_bit());
    afft_clr_done_bit();

    output_compare((uint32_t *)reg, ifft_output_complex, 2048, 0, 1, 0);
#else
    os_mem_set(result, 0, 2048);
    iot_afft_complex_ifft(result, ifft_input_complex, 2048,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_1024, 0, 1, 0);
    output_compare((uint32_t *)result, ifft_output_complex, 2048);
#endif

    iot_printf("AFFT_IFFT_COMPLEX_24 done....\n");
#endif
}


void data_left_shift(void *dst, void *src, uint32_t len, uint8_t bit)
{
    uint32_t *d = (uint32_t *)dst;
    uint32_t *s = (uint32_t *) src;
    for(uint32_t i = 0; i < len; i++) {
       *(d + i)  = (*(s +i) << bit);
    }
}

void data_bind(void *dst, void *src, uint32_t len)
{
    uint32_t *d = (uint32_t *)dst;
    uint32_t *s = (uint32_t *) src;
    for(uint32_t i = 0; i < len/2; i++) {
       *(d + i)  = (*(s +i*2)&0xffff) + (*(s + 2*i+1) << 16);
    }
}

void data_right_shift(void *dst, void *src, uint32_t len, uint8_t bit)
{
    int32_t *d = (int32_t *)dst;
    int32_t *s = (int32_t *) src;
    for(uint32_t i = 0; i < len; i++) {
       *(d + i)  = (*(s +i) >> bit);
    }
}

void afft_new_test()
{
    uint64_t org = cpu_get_mcycle();
    iot_printf("mcycle org: %d\n", org);

#if (TEST_CASE_DATA_MODE & AFFT_FFT_LOW_24)
    iot_printf("afft test fft 512 low 24bit real data\n");

    os_mem_set(result, 0, 2048);
    iot_afft_real_fft(result, (uint32_t *)input_real_512, 512,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_512, 1, 0, 5);
    iot_printf("output fft real 24:\n");
    for(uint32_t i = 0; i < 512; i++) {
        uint32_t tmp;
        tmp = (uint32_t )(*(result+i));
        iot_printf("0x%08x\n", tmp);
    }
    for(uint32_t i = 0; i < 512; i++) {
        int32_t tmp;
        tmp = (int32_t )(*(result+i));
        iot_printf("%d\n", tmp);
    }
    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif

#if (TEST_CASE_DATA_MODE & AFFT_FFT_HIGH_24)
    iot_printf("afft test fft 512 high 24bit real data\n");

    os_mem_set(result, 0, 2048);
    data_left_shift(input_real_512_h24, input_real_512, 512, 8);
    iot_afft_real_fft(result, (uint32_t *)input_real_512_h24, 512,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_512, 1, 0, 5);
    iot_printf("output fft real 24:\n");
    for(uint32_t i = 0; i < 512; i++) {
        uint32_t tmp;
        tmp = (uint32_t )(*(result+i));
        iot_printf("0x%08x\n", tmp);
    }
    data_right_shift(result, result, 512, 8);
    for(uint32_t i = 0; i < 512; i++) {
        int32_t tmp;
        tmp = (int32_t )(*(result+i));
        iot_printf("%d\n", tmp);
    }
    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif

#if (TEST_CASE_DATA_MODE & AFFT_FFT_16_REAL)
    iot_printf("afft test fft 512 16-16bit real data\n");

    os_mem_set(result, 0, 2048);
    data_bind(input_real_512_1616, input_real_512, 512);
    iot_afft_real_fft(result, (uint32_t *)input_real_512_1616, 256,
        AFFT_FMT_16BIT, AFFT_REAL_512, 1, 0, 5);
    iot_printf("output fft real 24:\n");

    for(uint32_t i = 0; i < 256; i++) {
        uint32_t tmp;
        tmp = (uint32_t )(*(result+i));
        iot_printf("0x%08x\n", tmp);
        iot_printf("-\n");
    }

    for(uint32_t i = 0; i < 256; i++) {
        int16_t low;
        int16_t high;

        low = (int16_t)(result[i] & 0xffff);
        high = (int16_t)((result[i] & 0xffff0000)  >> 16);
        iot_printf("%d\n", low);
        iot_printf("%d\n", high);
    }

    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif

#if (TEST_CASE_DATA_MODE & AFFT_FFT_FLOAT)
    iot_printf("afft test fft 512 float real data\n");

    os_mem_set(result, 0, 2048);
    org = cpu_get_mcycle();
    iot_afft_real_fft(result, (uint32_t *)input_float_512, 512,
        AFFT_FMT_FLOAT_32BIT, AFFT_REAL_512, 1, 0, 5);
    g_mcycle_cnt.fft_float = cpu_get_mcycle() - org;
    iot_printf("output fft real 24:\n");
    for(uint32_t i = 0; i < 512; i++) {
        uint32_t tmp;
        tmp = (uint32_t )(*(result+i));
        iot_printf("0x%08x\n", tmp);
    }

    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif

#if (TEST_CASE_DATA_MODE & AFFT_IFFT_LOW_24)
    iot_printf("afft test ifft 512 low 24bit real data\n");

    os_mem_set(result, 0, 2048);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_512, 512,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_512, 0, 1, 0);
    iot_printf("output fft real 24:\n");

    for(uint32_t i = 0; i < 512; i++) {
        uint32_t tmp;
        tmp = (uint32_t )(*(result+i));
        iot_printf("0x%08x\n", tmp);
    }

    for(uint32_t i = 0; i < 512; i++) {
        int32_t tmp;
        tmp = (int32_t )(*(result+i));
        iot_printf("%d\n", tmp);
    }

    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif

#if (TEST_CASE_DATA_MODE & AFFT_IFFT_HIGH_24)
    iot_printf("afft test ifft 512 high 24bit real data\n");

    os_mem_set(result, 0, 2048);
    data_left_shift(input_ireal_512_h24, input_ireal_512, 512, 8);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_512_h24, 512,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_512, 0, 1, 0);
    iot_printf("output fft real 24:\n");

    for(uint32_t i = 0; i < 512; i++) {
        uint32_t tmp;
        tmp = (uint32_t )(*(result+i));
        iot_printf("0x%08x\n", tmp);
    }

    data_right_shift(result, result, 512, 8);

    for(uint32_t i = 0; i < 512; i++) {
        int32_t tmp;
        tmp = (int32_t )(*(result+i));
        iot_printf("%d\n", tmp);
    }

    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif

#if (TEST_CASE_DATA_MODE & AFFT_IFFT_16_REAL)
    iot_printf("afft test ifft 512 16-16bit real data\n");

    os_mem_set(result, 0, 2048);
    data_bind(input_ireal_512_1616, input_ireal_512, 512);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_512_1616, 256,
        AFFT_FMT_16BIT, AFFT_REAL_512, 0, 1, 0);
    iot_printf("output fft real 24:\n");
    for(uint32_t i = 0; i < 256; i++) {
        uint32_t tmp;
        tmp = (uint32_t )(*(result+i));
        iot_printf("0x%08x\n", tmp);
        iot_printf("-\n");
    }
    for(uint32_t i = 0; i < 256; i++) {
        int16_t low;
        int16_t high;

        low = (int16_t)(result[i] & 0xffff);
        high = (int16_t)((result[i] & 0xffff0000)  >> 16);
        iot_printf("%d\n", low);
        iot_printf("%d\n", high);
    }

    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif

#if (TEST_CASE_DATA_MODE & AFFT_IFFT_FLOAT)
    iot_printf("afft test ifft 512 float data\n");

    os_mem_set(result, 0, 2048);
    iot_afft_real_ifft(result, (uint32_t *)input_ifloat_512, 512,
        AFFT_FMT_FLOAT_32BIT, AFFT_REAL_512, 0, 1, 0);
    iot_printf("output fft real 24:\n");
    for(uint32_t i = 0; i < 512; i++) {
        uint32_t tmp;
        tmp = (uint32_t )(*(result+i));
        iot_printf("0x%08x\n", tmp);
    }
    iot_printf("AFFT_FFT_REAL_24 done....\n");
#endif
}


float sw_dma_test[512] = {0};

float sw_out[512] = {0};
extern void sw_dma_memcpy(void *dest, void *src, uint16_t total_len);
extern void* memcpy(void* dest, const void* src, uint32_t size);

void mem_cpy_test()
{
    iot_simd_enable();

    uint32_t *reg = (uint32_t *) 0x56604000;
    uint64_t org = 0;

    // sw dma memcpy
    org = cpu_get_mcycle();
    sw_dma_memcpy(sw_dma_test, input_real_512, 512*4);
    g_mcycle_cpy.swdma_ram2ram = cpu_get_mcycle() - org;

    org = cpu_get_mcycle();
    sw_dma_memcpy(reg, input_real_512, 512*4);
    g_mcycle_cpy.swdma_ram2fft = cpu_get_mcycle() - org;

    org = cpu_get_mcycle();
    sw_dma_memcpy(sw_dma_test, reg, 512*4);
    g_mcycle_cpy.swdma_fft2ram = cpu_get_mcycle() - org;

    // cpu memcpy
    org = cpu_get_mcycle();
    memcpy(sw_dma_test, input_real_512, 512*4);
    g_mcycle_cpy.memcpy_ram2ram = cpu_get_mcycle() - org;

    org = cpu_get_mcycle();
    memcpy(reg, input_real_512, 512*4);
    g_mcycle_cpy.memcpy_ram2fft = cpu_get_mcycle() - org;

    org = cpu_get_mcycle();
    memcpy(sw_dma_test, reg, 512*4);
    g_mcycle_cpy.memcpy_fft2ram = cpu_get_mcycle() - org;

    // simd memcpy
    org = cpu_get_mcycle();
    iot_simd_memcpy_512(sw_dma_test, input_real_512);
    g_mcycle_cpy.simd_ram2ram= cpu_get_mcycle() - org;

    org = cpu_get_mcycle();
    iot_simd_memcpy_512(reg, input_real_512);
    g_mcycle_cpy.simd_ram2fft = cpu_get_mcycle() - org;

    org = cpu_get_mcycle();
    iot_simd_memcpy_512(sw_dma_test, reg);
    g_mcycle_cpy.simd_fft2ram = cpu_get_mcycle() - org;
}

int main(void)
{
    volatile uint32_t mstatus = 0x8001f888;
    asm volatile ("csrw mstatus, %0" : "=r"(mstatus));
    dbg_uart_init();

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

    iot_printf("\n-------AUDIO FFT TEST---------\n");

    iot_afft_init();

    //mem_cpy_test();

    while(1) {
        //afft_test();
        afft_new_test();
    }
    iot_printf("\n-------AUDIO FFT TEST FINISH---------\n");

    while(1);

    return 0;
}