// 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 "dsp_tests.h"

static const char *TAG = "dsps_fft2r_ae32_s16";

static int16_t data[1024*2];
static int16_t check_data[1024*2];
static float result_data[1024*2];

TEST_CASE("dsps_fft2r_sc16_ae32 functionality", "[dsps]")
{
    int N = sizeof(data) / sizeof(float) / 2;
    N = 1024;
    int check_bin = 64;
    for (int i = 0 ; i < N ; i++) {
        data[i * 2 + 0] = (INT16_MAX)*sin(M_PI / N * check_bin *2* i)*0.5 * (1 - cosf(i * 2 * M_PI / (float)(N-1)));
        data[i * 2 + 1] = 0;
    }

    esp_err_t ret = dsps_fft2r_init_sc16(NULL, CONFIG_DSP_MAX_FFT_SIZE);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
        return;
    }

    dsps_fft2r_sc16_ae32(data, N);
    unsigned int start_b = xthal_get_ccount();
    dsps_bit_rev_sc16_ansi(data, N);
    unsigned int end_b = xthal_get_ccount();


    for (int i=0 ; i< N ; i++)
    {
        ESP_LOGD(TAG, "Data[%i] %04x\n", i/2, data[i]);
    }

    float min = 10000;
    float max = -10000;
    int max_pos = 0;
    for (int i = 0 ; i < (N*2) ; i++)
    {
        result_data[i] = data[i];
        result_data[i] = result_data[i]/INT16_MAX;
    }
    
    for (int i = 0 ; i < N ; i++) {
        result_data[i] = 10 * log10f(0.0000000000001 + result_data[i * 2 + 0] * result_data[i * 2 + 0] + result_data[i * 2 + 1] * result_data[i * 2 + 1]);
        if (result_data[i] < min) {
            min = result_data[i];
        }
        if (result_data[i] > max) {
            max = result_data[i];
            max_pos = i;
        }
        ESP_LOGD(TAG, "FFT Data[%i] =%8.4f dB", i, result_data[i]);
    }
    dsps_view_spectrum(result_data, N, -100, 0);
    float round_pow = round(max*5);

    ESP_LOGI(TAG, "max_bin=%i, check_bin=%i, round_pow=%f\n", max_pos, check_bin, round_pow);

    if (max_pos < N/2) TEST_ASSERT_EQUAL( check_bin, max_pos);
    else TEST_ASSERT_EQUAL( N - check_bin, max_pos);
    TEST_ASSERT_EQUAL( -12*5, round_pow);
    ESP_LOGI(TAG, "Calculation error is less then 0.2 dB");
    ESP_LOGI(TAG, "cycles - %i", end_b - start_b);
    dsps_fft2r_deinit_sc16();
}


TEST_CASE("dsps_fft2r_sc16_ae32 overflow check", "[dsps]")
{
    int N = sizeof(data) / sizeof(float) / 2;
    N = 1024;
    int check_bin = 32;
    int bins_count = 4;
    for (int i = 0 ; i < N ; i++) {
            data[i * 2 + 0] = 0;
            data[i * 2 + 1] = 0;
        for (int n=1; n<=bins_count ; n++ )
        {
            data[i * 2 + 0] += (INT16_MAX)*cos(M_PI / N * check_bin *2* i*n)/bins_count;
            data[i * 2 + 1] += (INT16_MAX)*sin(M_PI / N * check_bin *2* i*n)/bins_count;
        }
    }

    esp_err_t ret = dsps_fft2r_init_sc16(NULL, CONFIG_DSP_MAX_FFT_SIZE);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
        return;
    }

    dsps_fft2r_sc16_ae32(data, N);
    unsigned int start_b = xthal_get_ccount();
    dsps_bit_rev_sc16_ansi(data, N);
    unsigned int end_b = xthal_get_ccount();


    for (int i=0 ; i< N ; i++)
    {
        ESP_LOGD(TAG, "Data[%i] %04x\n", i/2, data[i]);
    }

    float min = 10000;
    float max = -10000;
    int max_pos = 0;
    for (int i = 0 ; i < (N*2) ; i++)
    {
        result_data[i] = data[i];
        result_data[i] = result_data[i]/INT16_MAX;
    }
    
    for (int i = 0 ; i < N ; i++) {
        result_data[i] = 10 * log10f(0.0000000000001 + result_data[i * 2 + 0] * result_data[i * 2 + 0] + result_data[i * 2 + 1] * result_data[i * 2 + 1]);
        if (result_data[i] < min) {
            min = result_data[i];
        }
        if (result_data[i] > max) {
            max = result_data[i];
            max_pos = i;
        }
        ESP_LOGD(TAG, "FFT Data[%i] =%8.4f dB", i, result_data[i]);
    }
    dsps_view_spectrum(result_data, N, -100, 0);
    float round_pow = round(max*5);

    float noise_pow = -100;
    for (int i=(bins_count*check_bin + 10) ; i< N ; i++)
    {
        if (result_data[i] > noise_pow)
        {
            noise_pow = result_data[i];
        }
    }
    ESP_LOGI(TAG, "max_bin=%i, check_bin=%i, round_pow=%f, noise power=%f\n", max_pos, check_bin, round_pow, noise_pow);

    if (noise_pow > (-65)) 
    {
        TEST_ASSERT_MESSAGE (false, "Exec time takes more than expected!");
    }

    ESP_LOGI(TAG, "cycles - %i", end_b - start_b);
    dsps_fft2r_deinit_sc16();
}

TEST_CASE("dsps_fft2r_sc16_ae32 benchmark", "[dsps]")
{    
    esp_err_t ret = dsps_fft2r_init_sc16(NULL, CONFIG_DSP_MAX_FFT_SIZE);
    if (ret  != ESP_OK)
    {
        ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
        return;
    }
    for (int i= 5 ; i< 10 ; i++)
    {
        int N_check = 2<<i;
        unsigned int start_b = xthal_get_ccount();
        dsps_fft2r_sc16_ae32(data, N_check);

        unsigned int end_b = xthal_get_ccount();
        float total_b = end_b - start_b;
        float cycles = total_b;
        ESP_LOGI(TAG, "Benchmark dsps_fft2r_sc16_ae32 - %6i cycles for %6i points FFT.", (int)cycles, N_check);
        float min_exec = 3;
        float max_exec = 330000*3;
        TEST_ASSERT_EXEC_IN_RANGE(min_exec, max_exec, cycles);
    }
    dsps_fft2r_deinit_sc16();
}
