// line 79 - change filter length to adjust echo cancellation

#include "bsp_board.h"

#include <stdbool.h>
#include "esp_err.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "string.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "driver/i2s_std.h"
#include "driver/i2s_types.h"

#include "esp_aec.h"
#include "tca9555_driver.h"


static const char *TAG = "board";
static int s_play_sample_rate = 16000;
static int s_play_channel_format = 1;
static int s_bits_per_chan = 16;

#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
static i2s_chan_handle_t            tx_handle = NULL;        // I2S tx channel handler
static i2s_chan_handle_t            rx_handle = NULL;        // I2S rx channel handler
#endif
static const audio_codec_data_if_t *record_data_if  = NULL;
static const audio_codec_ctrl_if_t *record_ctrl_if  = NULL;
static const audio_codec_if_t *record_codec_if      = NULL;
static esp_codec_dev_handle_t record_dev      = NULL;

static const audio_codec_data_if_t *play_data_if    = NULL;
static const audio_codec_ctrl_if_t *play_ctrl_if    = NULL;
static const audio_codec_gpio_if_t *play_gpio_if    = NULL;
static const audio_codec_if_t *play_codec_if        = NULL;
static esp_codec_dev_handle_t play_dev        = NULL;
static i2c_master_bus_handle_t i2c_bus_handle = NULL;
static bool s_volume_buttons_task_started = false;

static aec_handle_t *aec = NULL;
static int frame_size = 0;
static int16_t *mic = NULL;
static int16_t *ref = NULL;
static int16_t *out = NULL;

static esp_err_t bsp_aec_init(void)
{
    /* aec_create_from_config() is used instead of aec_create() so the NLP
     * (residual echo suppression) level can be pushed to its strongest
     * setting; a longer filter_length also helps cancel longer echo tails. */
    aec_config_t aec_cfg = {
        .mic_num       = 1,
        .ref_num       = 1,
        .out_num       = 1,
        .filter_length = 8,
        .sample_rate   = 16000,
        .caps          = MALLOC_CAP_8BIT,
        .mode          = AEC_MODE_VOIP_HIGH_PERF,
        .nlp_level     = AEC_NLP_LEVEL_AGGR,
    };
    aec = aec_create_from_config(&aec_cfg);
    if (aec == NULL) {
        ESP_LOGE(TAG, "AEC create failed");
        return ESP_ERR_NO_MEM;
    }

    frame_size = aec_get_chunksize(aec);
    mic = heap_caps_aligned_alloc(16, frame_size * sizeof(int16_t), MALLOC_CAP_8BIT);
    ref = heap_caps_aligned_alloc(16, frame_size * sizeof(int16_t), MALLOC_CAP_8BIT);
    out = heap_caps_aligned_alloc(16, frame_size * sizeof(int16_t), MALLOC_CAP_8BIT);
    if (mic == NULL || ref == NULL || out == NULL) {
        ESP_LOGE(TAG, "AEC buffer allocation failed for %d samples", frame_size);
        free(mic);
        free(ref);
        free(out);
        mic = NULL;
        ref = NULL;
        out = NULL;
        aec_destroy(aec);
        aec = NULL;
        frame_size = 0;
        return ESP_ERR_NO_MEM;
    }

    ESP_LOGI(TAG, "AEC ready: %d samples/frame at 16 kHz", frame_size);
    return ESP_OK;
}

static esp_err_t bsp_codec_deinit(void);

static void bsp_aec_deinit(void)
{
    if (aec != NULL) {
        aec_destroy(aec);
        aec = NULL;
    }
    free(mic);
    free(ref);
    free(out);
    mic = NULL;
    ref = NULL;
    out = NULL;
    frame_size = 0;
}

esp_err_t bsp_aec_reset(void)
{
    bsp_aec_deinit();
    return bsp_aec_init();
}

int bsp_aec_get_frame_size(void)
{
    return frame_size;
}

esp_err_t bsp_aec_process_frame(const int16_t *near_end,
                                const int16_t *far_end,
                                int16_t *processed)
{
    if (aec == NULL || mic == NULL || ref == NULL || out == NULL ||
        near_end == NULL || far_end == NULL || processed == NULL) {
        return ESP_ERR_INVALID_STATE;
    }

    memcpy(mic, near_end, frame_size * sizeof(int16_t));
    memcpy(ref, far_end, frame_size * sizeof(int16_t));
    aec_process(aec, mic, ref, out);
    memcpy(processed, out, frame_size * sizeof(int16_t));
    return ESP_OK;
}

esp_codec_dev_handle_t esp_ret_play_dev(void)
{
    return play_dev;
}

i2c_master_bus_handle_t esp_ret_i2c_handle(void)
{
    return i2c_bus_handle;
}

static esp_err_t i2c_master_init(void)
{
    const i2c_master_bus_config_t bus_config = {
        .i2c_port = I2C_NUM,
        .sda_io_num = GPIO_I2C_SDA,
        .scl_io_num = GPIO_I2C_SCL,
        .clk_source = I2C_CLK_SRC_DEFAULT,
    };

    esp_err_t ret = i2c_new_master_bus(&bus_config, &i2c_bus_handle);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to initialize I2C bus: %s", esp_err_to_name(ret));
        return ret;
    }

    ESP_LOGI(TAG, "I2C bus initialized successfully");
    return ESP_OK; 
}


esp_err_t bsp_codec_adc_init(int sample_rate)
{    ESP_LOGI(TAG,"ADC INIT START");
    esp_err_t ret_val = ESP_OK;

    audio_codec_i2s_cfg_t i2s_cfg = {
        .port = I2S_NUM_1,
#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
        .rx_handle = rx_handle,
        .tx_handle = NULL,
#endif
    };

    record_data_if = audio_codec_new_i2s_data(&i2s_cfg);

    if(record_data_if == NULL)
{
    ESP_LOGE(TAG,"record_data_if creation failed");
    return ESP_FAIL;
}


    audio_codec_i2c_cfg_t i2c_cfg = {
        .addr = ES7210_CODEC_DEFAULT_ADDR,
        .bus_handle = i2c_bus_handle
    };


    record_ctrl_if = audio_codec_new_i2c_ctrl(&i2c_cfg);


    es7210_codec_cfg_t es7210_cfg = {
        .ctrl_if = record_ctrl_if,

        .mic_selected =
            ES7210_SEL_MIC1 |
            ES7210_SEL_MIC2 |
            ES7210_SEL_MIC3 |
            ES7210_SEL_MIC4,
    };


    record_codec_if = es7210_codec_new(&es7210_cfg);

    ESP_LOGI(TAG,"ES7210 IF=%p", record_codec_if);


    esp_codec_dev_cfg_t dev_cfg = {
        .codec_if = record_codec_if,
        .data_if = record_data_if,
        .dev_type = ESP_CODEC_DEV_TYPE_IN,
    };


    record_dev = esp_codec_dev_new(&dev_cfg);


    if(record_dev == NULL)
    {
        ESP_LOGE(TAG,"Failed creating ADC codec");
        return ESP_FAIL;
    }


    esp_codec_dev_sample_info_t fs = {
        .sample_rate = sample_rate,

        .channel = 2,
        .bits_per_sample = 16,
    };


    ret_val = esp_codec_dev_open(record_dev, &fs);


    if(ret_val != ESP_OK)
    {
        ESP_LOGE(TAG,
                 "Codec open failed %s",
                 esp_err_to_name(ret_val));
        return ret_val;
    }


    esp_codec_dev_set_in_channel_gain(
        record_dev,
        ESP_CODEC_DEV_MAKE_CHANNEL_MASK(0),
        RECORD_VOLUME);

    esp_codec_dev_set_in_channel_gain(
        record_dev,
        ESP_CODEC_DEV_MAKE_CHANNEL_MASK(1),
        RECORD_VOLUME);


    return ret_val;
}

esp_err_t bsp_codec_dac_init(int sample_rate, int channel_format, int bits_per_chan)
{
    esp_err_t ret_val = ESP_OK;

    audio_codec_i2s_cfg_t i2s_cfg = {
        .port = I2S_NUM_1,
#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
        .rx_handle = NULL,
        .tx_handle = tx_handle,
#endif
    };
    play_data_if = audio_codec_new_i2s_data(&i2s_cfg);

    audio_codec_i2c_cfg_t i2c_cfg = {.addr = ES8311_CODEC_DEFAULT_ADDR,.bus_handle = i2c_bus_handle};
    play_ctrl_if = audio_codec_new_i2c_ctrl(&i2c_cfg);
    play_gpio_if = audio_codec_new_gpio();
    // New output codec interface
    es8311_codec_cfg_t es8311_cfg = {
        .codec_mode = ESP_CODEC_DEV_WORK_MODE_DAC,
        .ctrl_if = play_ctrl_if,
        .gpio_if = play_gpio_if,
        .pa_pin = GPIO_PWR_CTRL,
        .use_mclk = false,
    };
    play_codec_if = es8311_codec_new(&es8311_cfg);
    // New output codec device
    esp_codec_dev_cfg_t dev_cfg = {
        .codec_if = play_codec_if,
        .data_if = play_data_if,
        .dev_type = ESP_CODEC_DEV_TYPE_OUT,
    };
    play_dev = esp_codec_dev_new(&dev_cfg);

    esp_codec_dev_sample_info_t fs = {
        .bits_per_sample = bits_per_chan,
        .sample_rate = sample_rate,
        .channel = channel_format,
    };
    esp_codec_dev_set_out_vol(play_dev, PLAYER_VOLUME);
    esp_codec_dev_open(play_dev, &fs);

    return ret_val;
}

static esp_err_t bsp_codec_adc_deinit()
{
    esp_err_t ret_val = ESP_OK;

    if (record_dev) {
        esp_codec_dev_close(record_dev);
        esp_codec_dev_delete(record_dev);
        record_dev = NULL;
    }

    // Delete codec interface
    if (record_codec_if) {
        audio_codec_delete_codec_if(record_codec_if);
        record_codec_if = NULL;
    }
    
    // Delete codec control interface
    if (record_ctrl_if) {
        audio_codec_delete_ctrl_if(record_ctrl_if);
        record_ctrl_if = NULL;
    }
    
    // Delete codec data interface
    if (record_data_if) {
        audio_codec_delete_data_if(record_data_if);
        record_data_if = NULL;
    }

    return ret_val;
}

static esp_err_t bsp_codec_dac_deinit()
{
    esp_err_t ret_val = ESP_OK;

    if (play_dev) {
        esp_codec_dev_close(play_dev);
        esp_codec_dev_delete(play_dev);
        play_dev = NULL;
    }

    // Delete codec interface
    if (play_codec_if) {
        audio_codec_delete_codec_if(play_codec_if);
        play_codec_if = NULL;
    }
    
    // Delete codec control interface
    if (play_ctrl_if) {
        audio_codec_delete_ctrl_if(play_ctrl_if);
        play_ctrl_if = NULL;
    }
    
    if (play_gpio_if) {
        audio_codec_delete_gpio_if(play_gpio_if);
        play_gpio_if = NULL;
    }
    
    // Delete codec data interface
    if (play_data_if) {
        audio_codec_delete_data_if(play_data_if);
        play_data_if = NULL;
    }

    return ret_val;
}

esp_err_t esp_audio_set_play_vol(int volume)
{
    if (!play_dev) {
        ESP_LOGE(TAG, "DAC codec init fail");
        return ESP_FAIL;
    }
    esp_codec_dev_set_out_vol(play_dev, volume);
    return ESP_OK;
}

esp_err_t esp_audio_get_play_vol(int *volume)
{
    if (!play_dev) {
        ESP_LOGE(TAG, "DAC codec init fail");
        return ESP_FAIL;
    }
    esp_codec_dev_get_out_vol(play_dev, volume);
    return ESP_OK;
}

static esp_err_t bsp_i2s_init(int i2s_num, uint32_t sample_rate, int channel_format, int bits_per_chan)
{
    esp_err_t ret_val = ESP_OK;

#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
    i2s_slot_mode_t channel_fmt;
    if (channel_format == 1) {
        channel_fmt = I2S_SLOT_MODE_MONO;
    } else if (channel_format == 2) {
        channel_fmt = I2S_SLOT_MODE_STEREO;
    } else {
        ESP_LOGE(TAG, "Unable to configure channel_format %d", channel_format);
        channel_format = 1;
        channel_fmt = I2S_SLOT_MODE_MONO;
    }

    if (bits_per_chan != 16 && bits_per_chan != 32) {
        ESP_LOGE(TAG, "Unable to configure bits_per_chan %d", bits_per_chan);
        bits_per_chan = 32;
    }

    i2s_chan_config_t chan_cfg = I2S_CHANNEL_DEFAULT_CONFIG(i2s_num, I2S_ROLE_MASTER);
    ret_val |= i2s_new_channel(&chan_cfg, &tx_handle, &rx_handle);
    i2s_std_config_t std_cfg = I2S_CONFIG_DEFAULT(sample_rate, channel_fmt, bits_per_chan);
    ret_val |= i2s_channel_init_std_mode(tx_handle, &std_cfg);
    ret_val |= i2s_channel_init_std_mode(rx_handle, &std_cfg);
    ret_val |= i2s_channel_enable(tx_handle);
    ret_val |= i2s_channel_enable(rx_handle);
#else
    i2s_channel_fmt_t channel_fmt = I2S_CHANNEL_FMT_RIGHT_LEFT;
    if (channel_format == 1) {
        channel_fmt = I2S_CHANNEL_FMT_ONLY_LEFT;
    } else if (channel_format == 2) {
        channel_fmt = I2S_CHANNEL_FMT_RIGHT_LEFT;
    } else {
        ESP_LOGE(TAG, "Unable to configure channel_format %d", channel_format);
        channel_format = 1;
        channel_fmt = I2S_CHANNEL_FMT_ONLY_LEFT;
    }

    if (bits_per_chan != 16 && bits_per_chan != 32) {
        ESP_LOGE(TAG, "Unable to configure bits_per_chan %d", bits_per_chan);
        bits_per_chan = 16;
    }

    i2s_config_t i2s_config = I2S_CONFIG_DEFAULT(sample_rate, channel_fmt, bits_per_chan);

    i2s_pin_config_t pin_config = {
        .bck_io_num = GPIO_I2S_SCLK,
        .ws_io_num = GPIO_I2S_LRCK,
        .data_out_num = GPIO_I2S_DOUT,
        .data_in_num = GPIO_I2S_SDIN,
        .mck_io_num = GPIO_I2S_MCLK,
    };

    ret_val |= i2s_driver_install(i2s_num, &i2s_config, 0, NULL);
    ret_val |= i2s_set_pin(i2s_num, &pin_config);
#endif

    return ret_val;
}

static esp_err_t bsp_codec_init(
        int adc_sample_rate,
        int dac_sample_rate,
        int dac_channel_format,
        int dac_bits_per_chan)
{
    esp_err_t ret;

    ret = bsp_codec_adc_init(adc_sample_rate);

    if(ret != ESP_OK)
    {
        ESP_LOGE(TAG,"ADC init failed: %s",
                 esp_err_to_name(ret));
        return ret;
    }


    ret = bsp_codec_dac_init(
            dac_sample_rate,
            dac_channel_format,
            dac_bits_per_chan);


    if(ret != ESP_OK)
    {
        ESP_LOGE(TAG,"DAC init failed: %s",
                 esp_err_to_name(ret));
    }


    ret = bsp_aec_init();
    if (ret != ESP_OK) {
        bsp_codec_deinit();
        return ret;
    }

    return ret;
}

static esp_err_t bsp_codec_deinit()
{
    esp_err_t ret_val = ESP_OK;

    ret_val |= bsp_codec_adc_deinit();
    ret_val |= bsp_codec_dac_deinit();
    bsp_aec_deinit();
    return ret_val;
}

esp_err_t esp_audio_play(const int16_t* data, int length, uint32_t ticks_to_wait)
{
    esp_err_t ret = ESP_OK;
    if (!play_dev) {
        return ESP_FAIL;
    }

    int out_length= length;
    int audio_time = 1;
    audio_time *= (16000 / s_play_sample_rate);
    audio_time *= (2 / s_play_channel_format);

    int *data_out = NULL;
    if (s_bits_per_chan != 32) {
        out_length = length * 2;
        data_out = malloc(out_length);
        for (int i = 0; i < length / sizeof(int16_t); i++) {
            int ret = data[i];
            data_out[i] = ret << 16;
        }
    }

    int *data_out_1 = NULL;
    if (s_play_channel_format != 2 || s_play_sample_rate != 16000) {
        out_length *= audio_time;
        data_out_1 = malloc(out_length);
        int *tmp_data = NULL;
        if (data_out != NULL) {
            tmp_data = data_out;
        } else {
            tmp_data = (int *)data;
        }

        for (int i = 0; i < out_length / (audio_time * sizeof(int)); i++) {
            for (int j = 0; j < audio_time; j++) {
                data_out_1[audio_time * i + j] = tmp_data[i];
            }
        }
        if (data_out != NULL) {
            free(data_out);
            data_out = NULL;
        }
    }

    if (data_out != NULL) {
        ret = esp_codec_dev_write(play_dev, (void *)data_out, out_length);
        free(data_out);
    } else if (data_out_1 != NULL) {
        ret = esp_codec_dev_write(play_dev, (void *)data_out_1, out_length);
        free(data_out_1);
    } else {
        ret = esp_codec_dev_write(play_dev, (void *)data, length);
    }

    return ret;
}
esp_err_t esp_get_feed_data(
        bool is_get_raw_channel,
        int16_t *buffer,
        int buffer_len)
{
    if(record_dev == NULL)
    {
        ESP_LOGE(TAG,"record_dev NULL");
        return ESP_ERR_INVALID_STATE;
    }

    esp_err_t ret;

    ret = esp_codec_dev_read(
            record_dev,
            buffer,
            buffer_len);

    if(ret != ESP_OK)
    {
        return ret;
    }


    // Check microphone data
    int64_t sum = 0;

    for(int i=0;i<buffer_len/sizeof(int16_t);i++)
    {
        sum += abs(buffer[i]);
    }


    ESP_LOGI(TAG,
             "MIC level=%lld",
             sum/(buffer_len/sizeof(int16_t)));


    return ESP_OK;
}

static void bsp_volume_buttons_task(void *arg)
{

    int last_key1 = 1;
    int last_key3 = 1;

    while (io_expander == NULL) {
        vTaskDelay(pdMS_TO_TICKS(VOLUME_BUTTON_POLL_MS));
    }

    while (1) {
        uint32_t mask = 0;
        if (esp_io_expander_get_level(io_expander, KEY_VOLUME_UP_PIN | KEY_VOLUME_DOWN_PIN, &mask) != ESP_OK) {
            vTaskDelay(pdMS_TO_TICKS(VOLUME_BUTTON_POLL_MS));
            continue;
        }

        int key1 = (mask & KEY_VOLUME_UP_PIN) ? 1 : 0;
        int key3 = (mask & KEY_VOLUME_DOWN_PIN) ? 1 : 0;

        if (key1 == 0 && last_key1 != 0) {
            int current_vol = 0;
            if (esp_audio_get_play_vol(&current_vol) == ESP_OK) {
                current_vol += VOLUME_STEP;
                if (current_vol > VOLUME_MAX_LEVEL) {
                    current_vol = VOLUME_MAX_LEVEL;
                }
                if (esp_audio_set_play_vol(current_vol) == ESP_OK) {
                    ESP_LOGI(TAG, "Volume up -> %d", current_vol);
                }
            }
        }

        if (key3 == 0 && last_key3 != 0) {
            int current_vol = 0;
            if (esp_audio_get_play_vol(&current_vol) == ESP_OK) {
                current_vol -= VOLUME_STEP;
                if (current_vol < VOLUME_MIN_LEVEL) {
                    current_vol = VOLUME_MIN_LEVEL;
                }
                if (esp_audio_set_play_vol(current_vol) == ESP_OK) {
                    ESP_LOGI(TAG, "Volume down -> %d", current_vol);
                }
            }
        }

        last_key1 = key1;
        last_key3 = key3;
        vTaskDelay(pdMS_TO_TICKS(VOLUME_BUTTON_POLL_MS));
    }
}

static esp_err_t bsp_volume_buttons_init(void)
{
    if (s_volume_buttons_task_started) {
        return ESP_OK;
    }

    BaseType_t task_created = xTaskCreate(bsp_volume_buttons_task, "vol_btns", 4096, NULL, 5, NULL);
    if (task_created != pdPASS) {
        return ESP_FAIL;
    }

    s_volume_buttons_task_started = true;
    return ESP_OK;
}

esp_err_t esp_board_init(uint32_t sample_rate, int channel_format, int bits_per_chan)
{
    /*!< Initialize I2C bus, used for audio codec*/

    i2c_master_init();
    s_play_sample_rate = sample_rate;

    if (channel_format != 2 && channel_format != 1) {
        ESP_LOGE(TAG, "Unable to configure channel_format");
        channel_format = 2;
    }
    s_play_channel_format = channel_format;

    if (bits_per_chan != 32 && bits_per_chan != 16) {
        ESP_LOGE(TAG, "Unable to configure bits_per_chan");
        bits_per_chan = 32;
    }
    s_bits_per_chan = bits_per_chan;

    bsp_i2s_init(I2S_NUM_1, 16000, 2, 16);
    // Because record and play use the same i2s.
esp_err_t codec_ret;

codec_ret = bsp_codec_init(16000,16000,2,16);

if(codec_ret != ESP_OK)
{
    ESP_LOGE(TAG,
             "bsp_codec_init failed: %s",
             esp_err_to_name(codec_ret));
}
else
{
    ESP_LOGI(TAG,"bsp_codec_init OK");
}

    gpio_set_level(GPIO_PWR_CTRL, 1);

    esp_err_t button_ret = bsp_volume_buttons_init();
    if (button_ret != ESP_OK) {
        ESP_LOGW(TAG, "Failed to initialize volume buttons: %s", esp_err_to_name(button_ret));
    }

    return ESP_OK;
}