#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_FFT_COMPLEX_LOW_24  (1 << 8)
#define AFFT_IFFT_COMPLEX_LOW_24 (1 << 9)
#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_COMPLEX_LOW_24|AFFT_IFFT_COMPLEX_LOW_24|AFFT_FFT_LOW_24|AFFT_IFFT_LOW_24|AFFT_IFFT_HIGH_24|AFFT_FFT_HIGH_24) //(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[1024] = {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;
    uint32_t err_cnt = 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) {

            if(err_cnt<20)
                iot_printf("error i: %d, val: [result:%d - %d], inc: %d\n",
                    i, (int32_t)(*(src + i)), (int32_t)(*(dst + i)), inc);
            err_cnt++;
            //return;
        }

    }
    if(err_cnt==0)
        iot_printf("successful.....\n");
    else
        iot_printf("ERROR: err_cnt %d.....\n",err_cnt);
}

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 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 256 low 24bit real data\n");
    os_mem_set(result, 0, 1024);

    iot_afft_real_fft(result, (uint32_t *)input_real_256, 256,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_256);  //result
    output_compare((uint32_t *)result, (uint32_t *)output_real_256, 256);


    iot_printf("afft test fft 512 low 24bit real data\n");
    os_mem_set(result, 0, 1024);

    iot_afft_real_fft(result, (uint32_t *)input_real_512, 512,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_512);  //result
    output_compare((uint32_t *)result, (uint32_t *)output_real_512, 512);

    iot_printf("afft test fft 1024 low 24bit real data\n");
    os_mem_set(result, 0, 1024);

    iot_afft_real_fft(result, (uint32_t *)input_real_1024, 1024,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_1024);  //result
    output_compare((uint32_t *)result, (uint32_t *)output_real_1024, 1024);


    iot_printf("afft test fft 64 low 24bit real data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_init();
    iot_afft_real_fft(result, (uint32_t *)input_real_64, 64,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_64);  //result
    output_compare((uint32_t *)result, (uint32_t *)output_real_64, 64);

    iot_printf("afft test fft 128 low 24bit real data\n");
    os_mem_set(result, 0, 1024);

    iot_afft_real_fft(result, (uint32_t *)input_real_128, 128,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_128);  //result
    output_compare((uint32_t *)result, (uint32_t *)output_real_128, 128);

#endif

#if (TEST_CASE_DATA_MODE & AFFT_FFT_HIGH_24)
    iot_printf("afft test fft 256 high 24bit real data\n");
    os_mem_set(result, 0, 1024);
    data_left_shift(input_real_256_h24, input_real_256, 256, 8);
    iot_afft_real_fft(result, (uint32_t *)input_real_256_h24, 256,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_256);  //result
    data_right_shift(result, result, 256, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_real_256, 256);


    iot_printf("afft test fft 512 high 24bit real data\n");
    os_mem_set(result, 0, 1024);
    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);
    data_right_shift(result, result, 512, 8);

    output_compare((uint32_t *)result, (uint32_t *)output_real_512, 512);

    iot_printf("afft test fft 1024 high 24bit real data\n");
    os_mem_set(result, 0, 1024);
    data_left_shift(input_real_1024_h24, input_real_1024, 1024, 8);
    iot_afft_real_fft(result, (uint32_t *)input_real_1024_h24, 1024,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_1024);  //result
    data_right_shift(result, result, 1024, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_real_1024, 1024);



    iot_printf("afft test fft 64 high 24bit real data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_init();
    data_left_shift(input_real_64_h24, input_real_64, 64, 8);
    iot_afft_real_fft(result, (uint32_t *)input_real_64_h24, 64,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_64);
    data_right_shift(result, result, 64, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_real_64, 64);

    iot_printf("afft test fft 128 high 24bit real data\n");
    os_mem_set(result, 0, 1024);
    data_left_shift(input_real_128_h24, input_real_128, 128, 8);
    iot_afft_real_fft(result, (uint32_t *)input_real_128_h24, 128,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_128);
    data_right_shift(result, result, 128, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_real_128, 128);

#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, 1024);
    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);
    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, 1024);
    org = cpu_get_mcycle();
    iot_afft_real_fft(result, (uint32_t *)input_float_512, 512,
        AFFT_FMT_FLOAT_32BIT, AFFT_REAL_512);
    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 256 low 24bit real data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_256, 256,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_256);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_256, 256);

    iot_printf("afft test ifft 512 low 24bit real data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_512, 512,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_512);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_512, 512);

    iot_printf("afft test ifft 1024 low 24bit real data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_1024, 1024,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_1024);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_1024, 1024);

    iot_printf("afft test ifft 64 low 24bit real data\n");
    iot_afft_init();
    os_mem_set(result, 0, 1024);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_64, 64,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_64);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_64, 64);

    iot_printf("afft test ifft 128 low 24bit real data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_128, 128,
        AFFT_FMT_LOW_24BIT, AFFT_REAL_128);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_128, 128);

#endif

#if (TEST_CASE_DATA_MODE & AFFT_IFFT_HIGH_24)
    iot_printf("afft test ifft 256 high 24bit real data\n");
    os_mem_set(result, 0, 1024);
    data_left_shift(input_ireal_256_h24, input_ireal_256, 256, 8);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_256_h24, 256,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_256);
    data_right_shift(result, result, 256, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_256, 256);

    iot_printf("afft test ifft 512 high 24bit real data\n");
    os_mem_set(result, 0, 1024);
    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);
    data_right_shift(result, result, 512, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_512, 512);

    iot_printf("afft test ifft 1024 high 24bit real data\n");
    os_mem_set(result, 0, 1024);
    data_left_shift(input_ireal_1024_h24, input_ireal_1024, 1024, 8);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_1024_h24, 1024,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_1024);
    data_right_shift(result, result, 1024, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_1024, 1024);

    iot_printf("afft test ifft 64 high 24bit real data\n");
    iot_afft_init();
    os_mem_set(result, 0, 1024);
    data_left_shift(input_ireal_64_h24, input_ireal_64, 64, 8);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_64_h24, 64,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_64);
    data_right_shift(result, result, 64, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_64, 64);

    iot_printf("afft test ifft 128 high 24bit real data\n");

    os_mem_set(result, 0, 1024);
    data_left_shift(input_ireal_128_h24, input_ireal_128, 128, 8);
    iot_afft_real_ifft(result, (uint32_t *)input_ireal_128_h24, 128,
        AFFT_FMT_HIGH_24BIT, AFFT_REAL_128);
    data_right_shift(result, result, 128, 8);
    output_compare((uint32_t *)result, (uint32_t *)output_ireal_128, 128);

#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, 1024);
    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);
    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, 1024);
    iot_afft_real_ifft(result, (uint32_t *)input_ifloat_512, 512,
        AFFT_FMT_FLOAT_32BIT, AFFT_REAL_512);
    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_FFT_COMPLEX_LOW_24)
    iot_printf("afft test fft 256 24bit complex data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_complex_fft(result, fft_input_256_complex, 256,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_128);
    output_compare((uint32_t *)result, fft_output_256_complex, 256);

    iot_printf("afft test fft 512 24bit complex data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_complex_fft(result, fft_input_512_complex, 512,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_256);
    output_compare((uint32_t *)result, fft_output_512_complex, 512);

    iot_printf("afft test fft 1K 24bit complex data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_complex_fft(result, fft_input_complex, 1024,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_512);
    output_compare((uint32_t *)result, fft_output_complex, 1024);

    iot_printf("afft test fft 64 24bit complex data\n");
iot_afft_init();
    os_mem_set(result, 0, 1024);
    iot_afft_complex_fft(result, fft_input_64_complex, 64,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_32);
    output_compare((uint32_t *)result, fft_output_64_complex, 64);


    iot_printf("afft test fft 128 24bit complex data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_complex_fft(result, fft_input_128_complex, 128,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_64);
    output_compare((uint32_t *)result, fft_output_128_complex, 128);

#endif


#if (TEST_CASE_DATA_MODE & AFFT_IFFT_COMPLEX_LOW_24)
    iot_printf("afft test ifft 256 24bit complex data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_complex_ifft(result, ifft_input_256_complex, 256,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_128);
    output_compare((uint32_t *)result, ifft_output_256_complex, 256);

    iot_printf("afft test ifft 512 24bit complex data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_complex_ifft(result, ifft_input_512_complex, 512,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_256);
    output_compare((uint32_t *)result, ifft_output_512_complex, 512);

    iot_printf("afft test ifft 1K 24bit complex data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_complex_ifft(result, ifft_input_complex, 1024,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_512);
    output_compare((uint32_t *)result, ifft_output_complex, 1024);

    iot_printf("afft test ifft 64 24bit complex data\n");
    iot_afft_init();
    os_mem_set(result, 0, 1024);
    iot_afft_complex_ifft(result, ifft_input_64_complex, 64,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_32);
    output_compare((uint32_t *)result, ifft_output_64_complex, 64);

    iot_printf("afft test ifft 128 24bit complex data\n");
    os_mem_set(result, 0, 1024);
    iot_afft_complex_ifft(result, ifft_input_128_complex, 128,
        AFFT_FMT_LOW_24BIT, AFFT_COMPLEX_64);
    output_compare((uint32_t *)result, ifft_output_128_complex, 128);

#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);


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();


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

    while(1);

    return 0;
}