#ifndef I2S_H
#define I2S_H

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "driver/i2s.h"

template <int PIN_DATA, int PIN_CLK, int PIN_LR> class I2S {

	static constexpr i2s_port_t port = I2S_NUM_0;
	i2s_config_t cfg;

	struct Setup {
		uint32_t sampleRate = 0;
		i2s_bits_per_sample_t bitsPerSample = I2S_BITS_PER_SAMPLE_8BIT;
		i2s_channel_t channels = I2S_CHANNEL_MONO;
	} cur;

public:

	/** ctor */
	I2S() {

		#ifdef CONFIG_A2DP_SINK_OUTPUT_INTERNAL_DAC
			cfg.mode = static_cast<i2s_mode_t>(I2S_MODE_MASTER | I2S_MODE_TX | I2S_MODE_DAC_BUILT_IN);
		#else
			cfg.mode = static_cast<i2s_mode_t>(I2S_MODE_MASTER | I2S_MODE_TX);
		#endif

		cfg.dma_buf_count = 6;
		cfg.dma_buf_len = 128;

		cfg.sample_rate = 44100;
		cfg.bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT;
		cfg.channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT;                          //2-channels
		cfg.communication_format = I2S_COMM_FORMAT_I2S_MSB;


		cfg.intr_alloc_flags = 0;                                                  //Default interrupt priority
		cfg.tx_desc_auto_clear = true;                                           //Auto clear tx descriptor on underflow

		i2s_driver_install(port, &cfg, 0, nullptr);

		#ifdef CONFIG_A2DP_SINK_OUTPUT_INTERNAL_DAC
			i2s_set_dac_mode(I2S_DAC_CHANNEL_BOTH_EN);
			i2s_set_pin(0, NULL);
		#else
			i2s_pin_config_t pins;
			pins.bck_io_num = PIN_CLK;//CONFIG_I2S_BCK_PIN;
			pins.ws_io_num = PIN_LR;//CONFIG_I2S_LRCK_PIN;
			pins.data_out_num = PIN_DATA;//CONFIG_I2S_DATA_PIN;
			pins.data_in_num = -1;                                                       //Not used
			i2s_set_pin(port, &pins);
		#endif

		configure(44100, I2S_BITS_PER_SAMPLE_16BIT, I2S_CHANNEL_STEREO);

	}

	/** change the i2s configuration */
	void configure(const uint32_t sampleRate, const i2s_bits_per_sample_t bitsPerSample, const i2s_channel_t channels) {

		// only update when changed!
		if (cur.sampleRate != sampleRate || cur.bitsPerSample != bitsPerSample || cur.channels != channels) {
			i2s_set_clk(port, sampleRate, bitsPerSample, channels);	// update
			cur.sampleRate = sampleRate;
			cur.bitsPerSample = bitsPerSample;
			cur.channels = channels;
		}

	}

	/** add the given samples for playback. returns the number of added samples, which might be less in case of timeouts */
	size_t add(const uint8_t* data, const size_t len) {
		size_t written;
		i2s_write(port, data, len, &written, portMAX_DELAY);
		return written;
	}

	uint16_t tmp[2048];

	size_t addSignedToUnsigned(const uint8_t* data, const size_t len) {
		const int16_t* src = (const int16_t*) data;
		uint16_t* dst = (uint16_t*) data;
		for (int i = 0; i < len/2; ++i) {
			//dst[i] = (int)src[i] + (int)32768;
			//dst[i] = (int)src[i] / 270 + 128;
			tmp[i] = (int)src[i] + 32768;
		}
		//return add(data, len);
		return add((const uint8_t*)tmp, len);
	}

//	/** add the given samples for playback. blocks until all have been added */
//	void addAll(const uint8_t* data, const size_t len) {
//		size_t added = add(data, len);
//		len -= added;
//		data +=
//	}






};

#endif // I2S_H