#include "speaker.h"

static const char *TAG = "speaker";

typedef struct {
    /// The path to the file being played.
    char* file_name;
	/// The handle to the file being played.
	FILE* file_handle;
    /// A number that all samples will be shifted right by.
    uint8_t prescaler;
    /// A flag for repeating.
    bool repeat;
} playing_audio_clip_t;

i2s_chan_handle_t tx_chan;

/// A queue of `audio_clip_t`s to be played once there is nothing being played
QueueHandle_t sequential_clip_queue;

/// The clips that are currently playing
std::vector<playing_audio_clip_t> playing_clips;

static void speaker_task(void* arg) {
	audio_clip_t next_clip;
	playing_audio_clip_t playing_next_clip;

	int16_t* audio_buf = (int16_t*) malloc(AUDIO_BLOCK_SIZE * sizeof(int16_t));
	int16_t* file_buf = (int16_t*) malloc(AUDIO_BLOCK_SIZE * sizeof(int16_t));

	// wait for a clip request.
	while (!xQueueReceive(sequential_clip_queue, &next_clip, portMAX_DELAY));
	ESP_LOGI(TAG, "opening %s", next_clip.file_name);
	FILE* fh = fopen(next_clip.file_name, "rb");
	if (fh != NULL) {
		ESP_LOGI(TAG, "fh was not null!");
		// skip the .wav header
		fseek(fh, 44, SEEK_SET);

		playing_next_clip = {
			.file_name = next_clip.file_name,
			.file_handle = fh,
			.prescaler = next_clip.prescaler,
			.repeat = next_clip.repeat,
		};
		playing_clips.push_back(playing_next_clip);
	} else {
		ESP_LOGI(TAG, "null :(");
	}

	i2s_channel_enable(tx_chan);
	while (1) {
		std::memset(audio_buf, 0, AUDIO_BLOCK_SIZE * sizeof(int16_t));

		ESP_LOGI(TAG, "size: %d", playing_clips.size());
		for (int clip_idx = playing_clips.size()-1; clip_idx >= 0; clip_idx--) {
			ESP_LOGI(TAG, "i: %d", clip_idx);
			playing_audio_clip_t& clip = playing_clips.at(clip_idx);
			ESP_LOGI(TAG, "r %d", (size_t) clip.file_handle);
			// ESP_LOGI(TAG, "reading from %s", clip.file_name);
			size_t samples_read = fread(file_buf, sizeof(uint16_t), AUDIO_BLOCK_SIZE, clip.file_handle);
			ESP_LOGI(TAG, "read! %d", samples_read);
			if (samples_read == 0) {
				if (clip.repeat) {
					ESP_LOGI(TAG, "repeating...");
					fseek(fh, 44, SEEK_SET);
				} else {
					ESP_LOGI(TAG, "deleting...");
					fclose(clip.file_handle);
					free(clip.file_name);
					playing_clips.erase(playing_clips.begin() + clip_idx);
				}
				continue;
			}

			for (int i = 0; i < samples_read; i++) {
				audio_buf[i] += (file_buf[i] >> clip.prescaler);
			}
		}

		size_t bytes_written;
		i2s_channel_write(tx_chan, audio_buf, AUDIO_BLOCK_SIZE * sizeof(int16_t), &bytes_written, portMAX_DELAY);
	}


	
	// xQueuePeek(sequential_clip_queue, &next_clip, 0);

    // QueueHandle_t stop_music = xQueueCreate(2, sizeof (uint8_t));
    // if (stop_music == 0) {
    //     ESP_LOGW(TAG, "Could not create stop_music queue!");
    //     vTaskDelete(NULL);
    // }

    // while (playing_music) {
    //     play_raw_stop_queue(MOUNT_POINT "/tetris.pcm", stop_music);
    // }
    
    vTaskDelete(NULL);
}

esp_err_t play_raw_stop_queue(const char *fp, QueueHandle_t stop_handle) {
	FILE *fh = fopen(fp, "rb");
	if (fh == NULL) {
		ESP_LOGE(TAG, "Failed to open file");
		return ESP_ERR_INVALID_ARG;
	}
	

	// create a writer buffer
	uint8_t *read_buf = (uint8_t*) calloc(AUDIO_BLOCK_SIZE, sizeof(uint8_t));
	uint16_t *write_buf = (uint16_t*) calloc(AUDIO_BLOCK_SIZE, sizeof(uint16_t));
	size_t bytes_read = 0;
	size_t bytes_written = 0;

	esp_err_t channel_enable_result = i2s_channel_enable(tx_chan);
	ESP_ERROR_CHECK_WITHOUT_ABORT(channel_enable_result);
	if (channel_enable_result != ESP_OK) {
		return channel_enable_result;
	}

	bytes_read = fread(read_buf, sizeof(uint8_t), AUDIO_BLOCK_SIZE, fh);
	for (int i = 0; i < bytes_read; i++) {
		write_buf[i] = read_buf[i] << 4;
	}

	//   i2s_channel_enable(tx_handle);

	while (bytes_read > 0) {
		// write the buffer to the i2s
		// ESP_LOGI(TAG, "Writing: ");
		// for (int i = 0; i < words_read; i++) {
		//   int16_t val = buf[i];
		//   printf("%s0x%04X ", (val < 0 ? "-" : "+"), (val < 0 ? -val : val));
		// }>
		i2s_channel_write(tx_chan, write_buf, bytes_read * sizeof(int16_t), &bytes_written, portMAX_DELAY);
		bytes_read = fread(read_buf, sizeof(uint8_t), AUDIO_BLOCK_SIZE, fh);
		for (int i = 0; i < bytes_read; i++) {
			write_buf[i] = read_buf[i] << 4;
		}
		uint8_t recv  = 0;
		if (xQueueReceive(stop_handle, &recv, 0)) {
			break;
		}
		vTaskDelay(pdMS_TO_TICKS(10));
	}

	i2s_channel_disable(tx_chan);
	free(read_buf);
	free(write_buf);
	fclose(fh);

	return ESP_OK;
}

esp_err_t play_wav(const char *fp) {
	FILE *fh = fopen(fp, "rb");
	if (fh == NULL) {
		ESP_LOGE(TAG, "Failed to open file");
		return ESP_ERR_INVALID_ARG;
	}

	// skip the header...
	fseek(fh, 44, SEEK_SET);

	int16_t *buf = (int16_t*) calloc(AUDIO_BLOCK_SIZE, sizeof(int16_t));
	size_t bytes_read = 0;
	size_t bytes_written = 0;

	bytes_read = fread(buf, sizeof(int16_t), AUDIO_BLOCK_SIZE, fh);
	for (int i = 0; i < bytes_read; i++) {
		buf[i] = buf[i] >> 2;
	}

	i2s_channel_preload_data(tx_chan, buf, bytes_read * sizeof(int16_t), &bytes_written);
	i2s_channel_enable(tx_chan);

	while (bytes_read > 0)
	{
		bytes_read = fread(buf, sizeof(int16_t), AUDIO_BLOCK_SIZE, fh);
		for (int i = 0; i < bytes_read; i++) {
			buf[i] = buf[i] >> 2;
		}
		// write the buffer to the i2s
		i2s_channel_write(tx_chan, buf, bytes_read * sizeof(int16_t), &bytes_written, portMAX_DELAY);
		// ESP_LOGI(TAG, "Bytes read: %d", bytes_read);
	}

	i2s_channel_disable(tx_chan);
	free(buf);

	return ESP_OK;
}

esp_err_t play_raw(const char *fp) {
	FILE *fh = fopen(fp, "rb");
	if (fh == NULL) {
		ESP_LOGE(TAG, "Failed to open file");
		return ESP_ERR_INVALID_ARG;
	}

	// create a writer buffer
	uint8_t *read_buf = (uint8_t*) calloc(AUDIO_BLOCK_SIZE, sizeof(uint8_t));
	uint16_t *write_buf = (uint16_t*) calloc(AUDIO_BLOCK_SIZE, sizeof(uint16_t));
	size_t bytes_read = 0;
	size_t bytes_written = 0;

	bytes_read = fread(read_buf, sizeof(uint8_t), AUDIO_BLOCK_SIZE, fh);
	for (int i = 0; i < bytes_read; i++) {
		write_buf[i] = read_buf[i] << 4;
	}

	i2s_channel_preload_data(tx_chan, write_buf, bytes_read * sizeof(int16_t), &bytes_written);
	esp_err_t channel_enable_result = i2s_channel_enable(tx_chan);
	ESP_ERROR_CHECK_WITHOUT_ABORT(channel_enable_result);
	if (channel_enable_result != ESP_OK) {
		return channel_enable_result;
	}

	while (bytes_read > 0) {
		// write the buffer to the i2s
		// ESP_LOGI(TAG, "Writing: ");
		// for (int i = 0; i < words_read; i++) {
		//   int16_t val = buf[i];
		//   printf("%s0x%04X ", (val < 0 ? "-" : "+"), (val < 0 ? -val : val));
		// }>
		bytes_read = fread(read_buf, sizeof(uint8_t), AUDIO_BLOCK_SIZE, fh);
		for (int i = 0; i < bytes_read; i++) {
			write_buf[i] = read_buf[i] << 4;
		}
		vTaskDelay(pdMS_TO_TICKS(10));
		i2s_channel_write(tx_chan, write_buf, bytes_read * sizeof(int16_t), &bytes_written, portMAX_DELAY);
	}

	i2s_channel_disable(tx_chan);
	free(read_buf);
	free(write_buf);
	fclose(fh);

	return ESP_OK;
}

void init_speaker(void) {
	ESP_LOGI(TAG, "Initializing speaker...");

	i2s_chan_config_t tx_chan_cfg = I2S_CHANNEL_DEFAULT_CONFIG(I2S_NUM_AUTO, I2S_ROLE_MASTER);
	ESP_ERROR_CHECK(i2s_new_channel(&tx_chan_cfg, &tx_chan, NULL));

	i2s_std_config_t tx_std_cfg = {
		.clk_cfg  = I2S_STD_CLK_DEFAULT_CONFIG(SAMPLE_RATE),
		.slot_cfg = I2S_STD_MSB_SLOT_DEFAULT_CONFIG(I2S_DATA_BIT_WIDTH_16BIT, I2S_SLOT_MODE_MONO),
		.gpio_cfg = {
			.mclk = I2S_GPIO_UNUSED,
			.bclk = SPEAKER_PIN_BCLK,
			.ws   = SPEAKER_PIN_WS,
			.dout = SPEAKER_PIN_DOUT,
			.din  = GPIO_NUM_NC,
			.invert_flags = {
				.mclk_inv = false,
				.bclk_inv = false,
				.ws_inv   = false,
			},
		},
	};
	ESP_ERROR_CHECK(i2s_channel_init_std_mode(tx_chan, &tx_std_cfg));

	// init queues and list
	sequential_clip_queue = xQueueCreate(8, sizeof(audio_clip_t));

	// start task
	// TODO: make stack 4096 again :(
    xTaskCreate(speaker_task, "play_audio", 16384, NULL, 5, NULL);

	// ESP_ERROR_CHECK_WITHOUT_ABORT(play_wav(MOUNT_POINT "/startup.wav"));
	// ESP_ERROR_CHECK_WITHOUT_ABORT(play_raw(MOUNT_POINT "/boot.pcm"));

	char* file_name = (char*) malloc(sizeof(MOUNT_POINT "/startup.wav"));
	memcpy(file_name, MOUNT_POINT "/startup.wav", sizeof(MOUNT_POINT "/startup.wav"));

	audio_clip_t startup_clip = {
		.file_name = file_name,
		.prescaler = 1,
		.repeat = false,
		.play_immediatly = false,
	};

	xQueueSend(sequential_clip_queue, &startup_clip, 0);
	vTaskDelay(pdMS_TO_TICKS(5000));

	ESP_LOGI(TAG, "Speaker initialized!");
}

void play_example() {
	ESP_ERROR_CHECK_WITHOUT_ABORT(play_raw("/sdcard/o.pcm"));
}