// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string.h>
#include "unity.h"
#include "dsp_platform.h"
#include "esp_log.h"

#include "dsps_view.h"
#include "dsps_fft2r.h"
#include "dsps_fft4r.h"
#include "dsp_tests.h"


static const char *TAG = "dsps_fft4r_ansi";

TEST_CASE("dsps_fft4r_fc32_ansi functionality", "[dsps]")
{
    float* data =  (float*)malloc(sizeof(float) * 1024*2);
    float* check_data_fft = (float*)malloc(sizeof(float) * 1024*2);
    esp_err_t ret;
    ret = dsps_fft2r_init_fc32(NULL, 1024);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT2R. Error = %i", ret);
        return;
    }
    ret = dsps_fft4r_init_fc32(NULL, 1024);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT4R. Error = %i", ret);
        return;
    }
    int N_check = 256;
    for (size_t pow = 2; pow < 6; pow++)
    {
        N_check = 1<< (pow*2);
        for (size_t i = 0; i < N_check; i++)
        {
            data[i*2] = cosf(2*M_PI*4/256*i);
            data[i*2+1] = sinf(2*M_PI*18/256*i);

            check_data_fft[i*2] = data[i*2];
            check_data_fft[i*2+1] = data[i*2 + 1];
        }

        dsps_fft2r_fc32_ansi(data, N_check);
        dsps_bit_rev_fc32_ansi(data, N_check);

        dsps_fft4r_fc32_ansi(check_data_fft, N_check);
        dsps_bit_rev4r_fc32(check_data_fft, N_check);

        float diff = 0;
        for (size_t i = 0; i < N_check*2; i++)
        {
            diff += fabs(data[i] - check_data_fft[i]); 
        }
        diff = diff/N_check;
        ESP_LOGI(TAG, "diff[%i] = %f\n", N_check, diff);
        if (diff > 0.00001)
        {
            dsps_view(data, N_check*2, 128, 16, -N_check, N_check, '.');   
            dsps_view(check_data_fft, N_check*2, 128, 16, -N_check, N_check, '.');   
            
            TEST_ASSERT_MESSAGE (false, "Result out of range!\n");
        }
    }
    
    dsps_view(data, N_check*2, 128, 16, -N_check, N_check, '.');   
    dsps_view(check_data_fft, N_check*2, 128, 16, -N_check, N_check, '.');   

    dsps_fft2r_deinit_fc32();
    dsps_fft4r_deinit_fc32();
    free(data);
    free(check_data_fft);
}

TEST_CASE("dsps_fft4r_fc32_ansi benchmark", "[dsps]")
{
    float* check_data_fft = (float*)malloc(sizeof(float) * 4096*2);

    unsigned int start_b;
    float cycles;

    esp_err_t ret = dsps_fft4r_init_fc32(NULL, CONFIG_DSP_MAX_FFT_SIZE);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
        return;
    }
    int N_check = 256;
    for (size_t pow = 2; pow < 7; pow++)
    {
        N_check = 1<< (pow*2);
        for (size_t i = 0; i < N_check; i++)
        {
            check_data_fft[i*2] = cosf(2*M_PI*4/256*i);
            check_data_fft[i*2+1] = sinf(2*M_PI*18/256*i);
        }

        start_b = xthal_get_ccount();
        dsps_fft4r_fc32_ansi(check_data_fft, N_check);
        dsps_bit_rev4r_fc32(check_data_fft, N_check);
        cycles = xthal_get_ccount() - start_b;

        ESP_LOGI(TAG, "Benchmark dsps_fft4r_fc32_ansi - %6i cycles for %6i points FFT.", (int)cycles, N_check);
    }
    
    dsps_fft4r_deinit_fc32();
    free(check_data_fft);
}


TEST_CASE("dsps_cplx2real_fc32 functionality", "[dsps]")
{
    float* data =  (float*)malloc(sizeof(float) * 1024*2);
    float* check_data_fft = (float*)malloc(sizeof(float) * 1024*2);

    esp_err_t ret = dsps_fft4r_init_fc32(NULL, 1024);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
        return;
    }
    ret = dsps_fft2r_init_fc32(NULL, 1024);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
        return;
    }
    
    int N_check = 256;
    for (size_t pow = 4; pow < 11; pow++)
    {
        N_check = 1<< (pow);
        for (size_t i = 0; i < N_check*2; i++)
        {
            data[i] = 0;
            check_data_fft[i] = data[i];
        }
        data[1] = N_check;
        check_data_fft[1] = data[1];

        dsps_fft2r_fc32_ansi(data, N_check);
        dsps_bit_rev_fc32_ansi(data, N_check);
        dsps_cplx2real_fc32_ansi(data, N_check);

        dsps_fft2r_fc32_ansi(check_data_fft, N_check);
        dsps_bit_rev_fc32_ansi(check_data_fft, N_check);
        dsps_cplx2real_fc32_ae32(check_data_fft, N_check);

        float diff = 0;
        for (size_t i = 0; i < N_check*2; i++)
        {
            diff += fabs(data[i] - check_data_fft[i]);
        }
        diff = diff/N_check;
        if (diff > 0.00001)
        {
            for (size_t i = 0; i < N_check*2; i++)
            {
                ESP_LOGD(TAG, "data[%i]= %f,    %f = check_data_fft[%i], diff=%f\n", i, data[i], check_data_fft[i], i, data[i] - check_data_fft[i]);
            }
            
            dsps_view(data, N_check*2, 128, 16, -N_check, N_check, '.');
            dsps_view(check_data_fft, N_check*2, 128, 16, -N_check, N_check, '.');   
            ESP_LOGE(TAG, "Error diff[%i] = %f\n", N_check, diff);
            TEST_ASSERT_MESSAGE (false, "Result out of range!\n");
        }

        ESP_LOGI(TAG, "diff[%i] = %f\n", N_check, diff);
    }

    free(data);
    free(check_data_fft);

    dsps_fft4r_deinit_fc32();
    dsps_fft2r_deinit_fc32();
}

static portMUX_TYPE testnlock = portMUX_INITIALIZER_UNLOCKED;

TEST_CASE("dsps_cplx2real_fc32_ansi benchmark", "[dsps]")
{
    float* check_data_fft = (float*)malloc(sizeof(float) * 4096*2);
    unsigned int start_b;
    float cycles;

    esp_err_t ret = dsps_fft4r_init_fc32(NULL, CONFIG_DSP_MAX_FFT_SIZE);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
        return;
    }
    int N_check = 256;
    for (size_t pow = 4; pow < 13; pow++)
    {
        N_check = 1<< (pow);
        for (size_t i = 0; i < N_check; i++)
        {
            check_data_fft[i*2] = cosf(2*M_PI*4/256*i);
            check_data_fft[i*2+1] = sinf(2*M_PI*18/256*i);
        }

        portENTER_CRITICAL(&testnlock);
        start_b = xthal_get_ccount();
        dsps_cplx2real_fc32_ansi(check_data_fft, N_check);
        cycles = xthal_get_ccount() - start_b;
        portEXIT_CRITICAL(&testnlock);

        ESP_LOGI(TAG, "Benchmark dsps_cplx2real_fc32_ansi - %6i cycles for %6i points FFT.", (int)cycles, N_check);
    }
    
    dsps_fft4r_deinit_fc32();
    free(check_data_fft);
}

TEST_CASE("dsps_gen_bitrev4r_table bitrev table generation.", "[dsps]")
{
    for (int i= 2 ; i< 7 ; i++)
    {
        int N_check = 1<<(i*2);
        dsps_gen_bitrev4r_table(N_check, 8, "fc32");
    }    
}
