#include <Arduino.h>
#include <FS.h>
#include <SD.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2s.h"
#include "esp_system.h"

#define I2S_OUT_BCK A0
#define I2S_OUT_LCK A1
#define I2S_OUT_DIN A2

#define I2S_CH_NUM       I2S_NUM_0
#define I2S_BUFFER_COUNT 4
#define I2S_BUFFER_SIZE  512
char    i2s_buf[I2S_BUFFER_SIZE];

#define SD_CS   A3
#define SDSPEED 40000000

// The Canonical WAVE file format
typedef struct {
	uint8_t  ChunkID[4];
	uint32_t ChunkSize;
	uint8_t  Format[4];
	uint8_t  Subchunk1ID[4];
	uint32_t Subchunk1Size;
	uint16_t AudioFormat;
	uint16_t NumChannels;
	uint32_t SampleRate;
	uint32_t ByteRate;
	uint16_t BlockAlign;
	uint16_t BitPerSample;
	uint8_t  Subchunk2ID[4];
	uint32_t Subchunk2Size;
	uint8_t  *wavdata;
} WAV_FORMAT;

File file;

size_t i2s_bytes_write = 0;

void I2S_Init(uint32_t sampleRate, uint16_t bitPerSample, uint16_t numChannels) {

	i2s_bits_per_sample_t i2s_bitPerSample;
	i2s_channel_fmt_t     i2s_channelFormat;

	switch(bitPerSample) {
		case  8: i2s_bitPerSample = I2S_BITS_PER_SAMPLE_8BIT;  break;
		case 16: i2s_bitPerSample = I2S_BITS_PER_SAMPLE_16BIT; break;
		case 24: i2s_bitPerSample = I2S_BITS_PER_SAMPLE_24BIT; break;
		case 32: i2s_bitPerSample = I2S_BITS_PER_SAMPLE_32BIT; break;
	}

	switch(numChannels) {
		case  1: i2s_channelFormat = I2S_CHANNEL_FMT_ONLY_LEFT;  break;
		case  2: i2s_channelFormat = I2S_CHANNEL_FMT_RIGHT_LEFT; break;
	}

	i2s_config_t i2s_config = {
		.mode                 = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX),
		.sample_rate          = sampleRate,
		.bits_per_sample      = i2s_bitPerSample,
		.channel_format       = i2s_channelFormat,
		.communication_format = (i2s_comm_format_t)(I2S_COMM_FORMAT_STAND_I2S | I2S_COMM_FORMAT_STAND_MSB),
		.intr_alloc_flags     = 0,
		.dma_buf_count        = I2S_BUFFER_COUNT,
		.dma_buf_len          = I2S_BUFFER_SIZE,
		.use_apll             = false,
		.tx_desc_auto_clear   = true,
		.fixed_mclk           = 0
	};

	i2s_pin_config_t pin_config = {
		.bck_io_num   = I2S_OUT_BCK,
		.ws_io_num    = I2S_OUT_LCK,
		.data_out_num = I2S_OUT_DIN,
		.data_in_num  = -1   //Not used
	};

	i2s_driver_install((i2s_port_t)I2S_CH_NUM, &i2s_config, 0, NULL);
	i2s_set_pin((i2s_port_t)I2S_CH_NUM, &pin_config);
	i2s_set_clk((i2s_port_t)I2S_CH_NUM, sampleRate, (i2s_bits_per_sample_t)bitPerSample, (i2s_channel_t)numChannels);
}

void I2S_Write(char* data, int numData) {
    i2s_write((i2s_port_t)0, (const char *)data, numData, &i2s_bytes_write, portMAX_DELAY);
}

boolean getWavFormat(WAV_FORMAT *pwf) {

	long len;
	short cbsize;

	if (file.read((uint8_t *)pwf,20) == -1) return false;

	if (memcmp(pwf->Subchunk1ID,"fmt ",4)==0) {
		if((len = file.read((uint8_t *)pwf+20,24))==-1) return false;
	} else if (memcmp(pwf->Subchunk1ID,"LIST",4)==0) {
		if (file.seek(20+pwf->Subchunk1Size)) {
			if((len = file.read((uint8_t *)pwf+12,24))==-1) return false;
			if (pwf->Subchunk1Size == 18) {
				// skip cbsize on WAVEFORMATEX
				if((len = file.read((uint8_t *)&cbsize,2))==-1) return false;
			}
			if((len = file.read((uint8_t *)pwf+36,8))==-1) return false;
		} else {
			return false;
		}
	} else {
		return false;
	}

	if(  memcmp(pwf->ChunkID,     "RIFF", 4)
	   ||memcmp(pwf->Format,      "WAVE", 4)
	   ||memcmp(pwf->Subchunk1ID, "fmt ", 4)
	   ||memcmp(pwf->Subchunk2ID, "data", 4))
	     return false;
	else return true;
}

boolean play(WAV_FORMAT *pwf, char *filename) {

	if(!(file = SD.open(filename))) return false;

	if (getWavFormat(pwf)) {
		I2S_Init(pwf->SampleRate,pwf->BitPerSample, pwf->NumChannels);
		long len = pwf->Subchunk2Size;
		while(len>0) {
			if (len>I2S_BUFFER_SIZE) {
				file.readBytes(i2s_buf, I2S_BUFFER_SIZE);
				I2S_Write(i2s_buf, I2S_BUFFER_SIZE);
			} else {
				file.readBytes(i2s_buf, len);
				I2S_Write(i2s_buf, len);
				break;
			}
			len -= I2S_BUFFER_SIZE;
		}
		file.close();
	}
	return true;
}

void setup() {

	WAV_FORMAT *pwf[1];

	if (SD.begin( SD_CS, SPI, SDSPEED)) {
		pwf[0] = (WAV_FORMAT *)malloc(sizeof(WAV_FORMAT));
		play(pwf[0],"/sound/03_toccatina_low.wav");
	}
}

void loop() {}