/*
 * audio_voip.c
 *
 *  Created on: 2 mar. 2020
 *      Author: dismuntel
 */

#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_event_loop.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "audio_element.h"
#include "audio_pipeline.h"
#include "audio_event_iface.h"
#include "audio_common.h"
#include "i2s_stream.h"
#include "esp_peripherals.h"
#include "board.h"

#include "../configuration.h"

#include "audio_mem.h"
#include "raw_stream.h"
#include "filter_resample.h"
#include "esp_sip.h"
#include "g711.h"
#include "audio_voip.h"
#include "audio_def.h"

static const char *TAG = "VOI";

#define RTP_HEADER_LEN 12
#ifndef AUDIO_FRAME_SIZE
#define AUDIO_FRAME_SIZE (160)
#endif

#define G711_SAMPLE_RATE    8000
#define G711_CHANNEL        1

static esp_err_t g711enc_pipeline_open(void);
static esp_err_t g711dec_pipeline_open(void);
static int _g711_encode(char *data);
static int _g711_decode(char *data, int len);

//static sip_handle_t sip;
static audio_element_handle_t raw_reader;
static audio_element_handle_t raw_writer;
audio_pipeline_handle_t speaker;
audio_pipeline_handle_t recorder;
audio_element_handle_t filter_reader;
audio_element_handle_t filter_writer;
audio_element_handle_t * i2s_voip_reader;
audio_element_handle_t * i2s_voip_writer;

void audio_voip_init(void){
	g711enc_pipeline_open();
	g711dec_pipeline_open();
}

int audio_voip_read_Microphone(uint8_t *buf){
	return _g711_encode((char *)buf);
}

void audio_voip_reproduce(uint8_t *buf, uint32_t len){
	_g711_decode((char *)buf, len);
}


static esp_err_t g711enc_pipeline_open(void)
{
	audio_pipeline_cfg_t pipeline_cfg = DEFAULT_AUDIO_PIPELINE_CONFIG();
	recorder = audio_pipeline_init(&pipeline_cfg);
	if (NULL == recorder) {
		return ESP_FAIL;
	}

	i2s_voip_reader = audio_get_i2s_reader();

	rsp_filter_cfg_t rsp_cfg = DEFAULT_RESAMPLE_FILTER_CONFIG();
	rsp_cfg.src_rate = I2S_SAMPLE_RATE;
	rsp_cfg.src_ch = I2S_CHANNEL;
	rsp_cfg.dest_rate = G711_SAMPLE_RATE;
	rsp_cfg.dest_ch = G711_CHANNEL;
	rsp_cfg.task_core = 1;
	filter_reader = rsp_filter_init(&rsp_cfg);

	raw_stream_cfg_t raw_cfg = RAW_STREAM_CFG_DEFAULT();
	raw_cfg.type = AUDIO_STREAM_READER;
	raw_reader = raw_stream_init(&raw_cfg);
	audio_element_set_output_timeout(raw_reader, portMAX_DELAY);

	audio_pipeline_register(recorder, *i2s_voip_reader, "i2s");
	audio_pipeline_register(recorder, filter_reader, "filter");
	audio_pipeline_register(recorder, raw_reader, "raw");
	audio_pipeline_link(recorder, (const char *[]) {"i2s", "filter", "raw"}, 3);
	audio_pipeline_run(recorder);
	ESP_LOGI(TAG, "Recorder has been created");
	return ESP_OK;
}

static int _g711_encode(char *data)
{
	int out_len_bytes;

	char enc_buffer[2 * AUDIO_FRAME_SIZE];
	out_len_bytes = raw_stream_read(raw_reader, enc_buffer, 2 * AUDIO_FRAME_SIZE);
	if (out_len_bytes > 0) {
		int16_t *enc_buffer_16 = (int16_t *)(enc_buffer);
		for (int i = 0; i < AUDIO_FRAME_SIZE; i++) {
#ifdef CONFIG_SIP_CODEC_G711A
			data[i] = esp_g711a_encode(enc_buffer_16[i]);
#else
			data[i] = esp_g711u_encode(enc_buffer_16[i]);
#endif
		}
		return AUDIO_FRAME_SIZE;
	} else {
		return 0;
	}
}

static esp_err_t g711dec_pipeline_open(void)
{
	audio_pipeline_cfg_t pipeline_cfg = DEFAULT_AUDIO_PIPELINE_CONFIG();
	speaker = audio_pipeline_init(&pipeline_cfg);
	if (NULL == speaker) {
		return ESP_FAIL;
	}

	raw_stream_cfg_t raw_cfg = RAW_STREAM_CFG_DEFAULT();
	raw_cfg.type = AUDIO_STREAM_WRITER;
	raw_writer = raw_stream_init(&raw_cfg);

	rsp_filter_cfg_t rsp_cfg = DEFAULT_RESAMPLE_FILTER_CONFIG();
	rsp_cfg.src_rate = G711_SAMPLE_RATE;
	rsp_cfg.src_ch = G711_CHANNEL;
	rsp_cfg.dest_rate = I2S_SAMPLE_RATE;
	rsp_cfg.dest_ch = I2S_CHANNEL;
	rsp_cfg.complexity = 5;
	rsp_cfg.task_core = 1;
	filter_writer = rsp_filter_init(&rsp_cfg);

	i2s_voip_writer = audio_get_i2s_writer();

	audio_pipeline_register(speaker, raw_writer, "raw");
	audio_pipeline_register(speaker, filter_writer, "filter");
	audio_pipeline_register(speaker, *i2s_voip_writer, "i2s");
	audio_pipeline_link(speaker, (const char *[]) {"raw", "filter", "i2s"}, 3);
	audio_pipeline_run(speaker);
	ESP_LOGI(TAG, "Speaker has been created");
	return ESP_OK;
}

static int _g711_decode(char *data, int len)
{
	int16_t dec_buffer[(2 * (len - RTP_HEADER_LEN))];

	for (int i = 0; i < (len - RTP_HEADER_LEN); i++) {
#ifdef CONFIG_SIP_CODEC_G711A
		dec_buffer[i] = esp_g711a_decode((unsigned char)data[RTP_HEADER_LEN + i]);
#else
		dec_buffer[i] = esp_g711u_decode((unsigned char)data[RTP_HEADER_LEN + i]);
#endif
	}

	raw_stream_write(raw_writer, (char *)dec_buffer, 2 * (len - RTP_HEADER_LEN));
	return 2 * (len - RTP_HEADER_LEN);
}

void audio_voip_stop(void){


	//audio_pipeline_reset_ringbuffer(speaker);
	//audio_pipeline_reset_elements(speaker);
	ESP_LOGI(TAG, "[ 4.0] Clean audio_pipeline");

	audio_pipeline_unregister(speaker, *i2s_voip_writer);
	audio_pipeline_unregister(speaker, filter_writer);
	audio_pipeline_unregister(speaker, raw_writer);
	ESP_LOGI(TAG, "[ 4.1 ] Pipeline unregister");

	audio_element_terminate(filter_writer);
	audio_element_stop(filter_writer);
	audio_element_wait_for_stop(raw_writer);
	ESP_LOGI(TAG, "[ 4.2 ] Element stop 1");
	audio_element_terminate(filter_writer);
	audio_element_stop(raw_writer);
	audio_element_wait_for_stop(raw_writer);
	ESP_LOGI(TAG, "[ 4.2 ] Element stop 2");

	audio_pipeline_remove_listener(speaker);
	ESP_LOGI(TAG, "[ 4.3 ] Pipeline remove");

	SLEEP_MS(100);

	audio_element_deinit(filter_writer);
	audio_element_deinit(raw_writer);
	ESP_LOGI(TAG, "[ 4.4 ] Element deinit");


	//audio_pipeline_reset_ringbuffer(recorder);
	//audio_pipeline_reset_elements(recorder);
	ESP_LOGI(TAG, "[ 4.0 ] Clean audio_pipeline");

	audio_pipeline_unregister(recorder, raw_reader);
	audio_pipeline_unregister(recorder, filter_reader);
	audio_pipeline_unregister(recorder, *i2s_voip_reader);
	ESP_LOGI(TAG, "[ 4.1 ] Pipeline unregister");

	audio_element_stop(raw_reader);
	audio_element_stop(filter_reader);
	ESP_LOGI(TAG, "[ 4.2 ] Element stop");

	audio_pipeline_remove_listener(recorder);
	ESP_LOGI(TAG, "[ 4.3 ] Pipeline remove");

	SLEEP_MS(100);

	ESP_LOGI(TAG, "[ 4.4 ] Element deinit");
	audio_element_deinit(raw_reader);
	audio_element_deinit(filter_reader);

	ESP_LOGI(TAG, "[ 5 ] Audio RAW stoped");
}