// Based on https://github.com/mpflaga/Arduino_Library-vs1053_for_SdFat/blob/master/src/vs1053_SdFat.cpp

#include "player.h"
#include "spi_master.h"

//Player::_spi_settings

Player::Player(SPIMaster* s) {
	_spi = s;
	PIN_VS1053_XRESET_SETUP();
	PIN_VS1053_XRESET(HIGH);
	PIN_SPEAKER_L_SETUP();
	PIN_SPEAKER_R_SETUP();
	_speaker_off();
	_spi->disable();
	PIN_VS1053_DREQ_SETUP();

	_init();
}

void Player::_reset() {
	PIN_VS1053_XRESET(LOW);
	delay(100);
	PIN_VS1053_XRESET(HIGH);
	delay(100);
	_state = uninitialized;
	_spi_settings = &_spi_settings_slow; // After reset, communication has to be slow
}

void Player::_init() {
	DEBUG("Resetting VS1053...\n");
	_reset();

	uint16_t result = _read_control_register(SCI_MODE);
	DEBUG("SCI_MODE: 0x%04X\n", result);
	if (result != 0x4800) {
		ERROR("SCI_MODE was 0x%04X, expected was 0x4800. Rebooting.\n", result);
		delay(500);
		ESP.restart();
	}
	result = _read_control_register(SCI_STATUS);
	DEBUG("SCI_STATUS: 0x%04X\n", result);
	if (result != 0x0040 && result != 0x0048) {
		ERROR("SCI_STATUS was 0x%04X, expected was 0x0040 or 0x0048. Rebooting.\n", result);
		delay(500);
		ESP.restart();
	}
	result = _read_control_register(SCI_CLOCKF);
	DEBUG("SCI_CLOCKF: 0x%04X\n", result);

	DEBUG("VS1053 Init looking good.\n");
	DEBUG("Upping VS1053 multiplier...\n");

	_write_control_register(SCI_CLOCKF, 0x6000);
	delay(10);

	_spi_settings = &_spi_settings_fast;

	result = _read_control_register(SCI_CLOCKF);
	DEBUG("SCI_CLOCKF: 0x%04X\n", result);
	if (result != 0x6000) {
		ERROR("Error: SCI_CLOCKF was 0x%04X, expected was 0x6000. Rebooting.\n", result);
		delay(500);
		ESP.restart();
	}

	set_volume(VOLUME_DEFAULT);

	INFO("VS1053 initialization completed.\n");

	INFO("Checking system sounds...\n");
	_spi->select_sd();
	_check_system_sound("no_prev_song.mp3");
	_check_system_sound("no_next_song.mp3");
	_check_system_sound("volume_max.mp3");
	_check_system_sound("volume_min.mp3");
	_spi->select_sd(false);

	_state = idle;
}

void Player::_speaker_off() {
	DEBUG("Speaker off\n");
	PIN_SPEAKER_L(LOW);
	PIN_SPEAKER_R(LOW);
}

void Player::_speaker_on() {
	DEBUG("Speaker on\n");
	PIN_SPEAKER_L(HIGH);
	PIN_SPEAKER_R(HIGH);
}

void Player::_sleep() {
	DEBUG("VS1053 going to sleep.\n");
	_speaker_off();
	_write_control_register(SCI_CLOCKF, 0x0000);
	_spi_settings = &_spi_settings_slow;
	_write_control_register(SCI_AUDATA, 0x0010);
	set_volume(0, false);
	_state = sleeping;
}

void Player::_wakeup() {
	if (_state != sleeping && _state != recording) return;
	_stopped_at = millis();
	DEBUG("Waking VS1053...\n");
	set_volume(_volume, false);
	_write_control_register(SCI_AUDATA, 0x0000);
	_write_control_register(SCI_CLOCKF, 0x6000);
	_write_control_register(SCI_MODE, 0x4800 | SM_RESET);
	delay(10);
	//_speaker_on();
	_spi_settings = &_spi_settings_fast;
	_state = idle;
}

void Player::_record() {
	DEBUG("Starting recording.\n");
	_set_volume(1, false);
	_write_control_register(SCI_AICTRL0, 8000U); // Sampling rate 8000 Hz
	_write_control_register(SCI_AICTRL1, 2048U); // Manual gain, 2x
	_write_control_register(SCI_AICTRL2, 2048U); // Maximum gain for autogain - ignored
	_write_control_register(SCI_AICTRL3, 6); // 2 (left channel) + 4 (PCM)
	_write_control_register(SCI_MODE, _read_control_register(SCI_MODE) | SM_ADPCM);
	DEBUG("SCI_MODE is now 0x%04X\n", _read_control_register(SCI_MODE));
	_patch_adpcm();
	_state = recording;
}

void Player::_check_system_sound(String filename) {
	String path = String("/system/") + filename;
	if (!SD.exists(path)) {
		ERROR("System sound %s is missing on the sd card!\n", path.c_str());
	} else {
		DEBUG("%s found.\n", path.c_str());
	}
}

inline void Player::_wait() {
	while(!PIN_VS1053_DREQ());
}

uint16_t Player::_read_control_register(uint8_t address) {
	_wait();
	_spi->select_vs1053_xcs();
	SPI.beginTransaction(*_spi_settings);
	SPI.transfer(CMD_READ);
	SPI.transfer(address);
	uint8_t b1 = SPI.transfer(0xFF);
	_wait();
	uint8_t b2 = SPI.transfer(0xFF);
	_wait();
	SPI.endTransaction();
	_spi->select_vs1053_xcs(false);

	return (b1 << 8) | b2;
}

void Player::_write_control_register(uint8_t address, uint16_t value) {
	_wait();
	_spi->select_vs1053_xcs();
	SPI.beginTransaction(*_spi_settings);
	SPI.transfer(CMD_WRITE);
	SPI.transfer(address);
	SPI.transfer(value >> 8);
	SPI.transfer(value & 0xFF);
	SPI.endTransaction();
	_spi->select_vs1053_xcs(false);
	_wait();
}

void Player::_patch_adpcm() {
	const uint16_t patch_data[] = {
		0x0007,0x0001, /*copy 1*/
		0x8050,
		0x0006,0x0042, /*copy 66*/
		0x0000,0x1790,0xf400,0x5400,0x0000,0x0a10,0xf400,0x5600,
		0xb080,0x0024,0x0007,0x9257,0x3f00,0x0024,0x0030,0x0297,
		0x3f00,0x0024,0x0000,0x004d,0x0014,0x958f,0x0000,0x1b4e,
		0x280f,0xe100,0x0006,0x2016,0x2a00,0x17ce,0x3e12,0xb817,
		0x3e14,0xf812,0x3e01,0xb811,0x0007,0x9717,0x0020,0xffd2,
		0x0030,0x11d1,0x3111,0x8024,0x3704,0xc024,0x3b81,0x8024,
		0x3101,0x8024,0x3b81,0x8024,0x3f04,0xc024,0x2808,0x4800,
		0x36f1,0x9811,0x2814,0x9c91,0x0000,0x004d,0x2814,0x9940,
		0x003f,0x0013,
		0x000a,0x0001, /*copy 1*/
		0x0050,
	};
	const uint8_t patch_size = 74;
	DEBUG("Executing patch_adpcm()...\n");

	_spi->select_vs1053_xcs();
	SPI.beginTransaction(*_spi_settings);
	
	for (int i=0; i<patch_size; i++) {
		unsigned short addr, n, val;
		addr = patch_data[i++];
		n = patch_data[i++];
		SPI.transfer(CMD_WRITE);
		SPI.transfer(addr & 0XFF);
		if (n & 0x8000U) { /* RLE run, replicate n samples */
			n &= 0x7FFF;
			val = patch_data[i++];
			while (n--) {
				SPI.transfer(val >> 8);
				SPI.transfer(val & 0xFF);
				_wait();
			}
		} else {           /* Copy run, copy n samples */
			while (n--) {
				val = patch_data[i++];
				SPI.transfer(val >> 8);
				SPI.transfer(val & 0xFF);
				_wait();
			}
		}
	}

	SPI.endTransaction();
	_spi->select_vs1053_xcs(false);

	DEBUG("Patch sent.\n");
}

void Player::_write_data(uint8_t* buffer) {
	_spi->select_vs1053_xdcs();
	SPI.beginTransaction(*_spi_settings);
	for (uint i=0; i<sizeof(_buffer); i++) {
		SPI.transfer(_buffer[i]);
	}
	SPI.endTransaction();
	_spi->select_vs1053_xdcs(false);
}

uint16_t Player::_read_wram(uint16_t address) {
	DEBUG("Reading WRAM address 0x%04X...\n", address);
	_write_control_register(SCI_WRAMADDR, address);
	uint16_t r1 = _read_control_register(SCI_WRAM);
	_write_control_register(SCI_WRAMADDR, address);
	uint16_t r2 = _read_control_register(SCI_WRAM);
	if (r1 == r2) return r1;
	DEBUG("Reading WRAM resulted in different values: 0x%04X and 0x%04X.\n", r1, r2);
	_write_control_register(SCI_WRAMADDR, address);
	r1 = _read_control_register(SCI_WRAM);
	if (r1 == r2) return r1;
	DEBUG("Reading WRAM resulted in different values: 0x%04X and 0x%04X.\n", r1, r2);
	_write_control_register(SCI_WRAMADDR, address);
	r2 = _read_control_register(SCI_WRAM);
	if (r1 == r2) return r1;
	DEBUG("Reading WRAM resulted in different values: 0x%04X and 0x%04X.\n", r1, r2);
	DEBUG("Returning last value (0x%04X)...\n", r2);
	return r2;
}

int8_t Player::_get_endbyte() {
	int8_t endbyte = _read_wram(ADDR_ENDBYTE) & 0xFF;
	DEBUG("Endbyte is 0x%02X.\n", endbyte);
	return endbyte;
}


void Player::set_volume(uint8_t vol, bool save) {
	if (save) {
		_volume = vol;
	}
	INFO("Setting volume to %d\n", vol);
	vol = 0xFF - vol;
	uint16_t value = (vol<<8)|vol;
	DEBUG("Setting volume register to 0x%04X\n", value);
	_write_control_register(SCI_VOL, value);
}

void Player::vol_up() {
	if (_volume == VOLUME_MAX) play_system_sound("volume_max.mp3");
	else if (_volume + VOLUME_STEP > VOLUME_MAX) set_volume(VOLUME_MAX);
	else set_volume(_volume + VOLUME_STEP);
}

void Player::vol_down() {
	if (_volume >= VOLUME_MIN + VOLUME_STEP) set_volume(_volume - VOLUME_STEP);
	else if (_volume == VOLUME_MIN) play_system_sound("volume_min.mp3");
	else set_volume(VOLUME_MIN);
}

void Player::_mute() {
	INFO("Muting.\n");
	_speaker_off();
	set_volume(1, false);
}

void Player::_unmute() {
	INFO("Unmuting.\n");
	set_volume(_volume, false);
	_speaker_on();
}

void Player::track_next() {
	if (_state != playing) return;
	if (_playing_index + 1 >= _playing_album_songs) {
		play_system_sound("no_next_song.mp3");
		return;
	}
	stop();
	play_song(_playing_album, _playing_index + 1);
}

void Player::track_prev() {
	if (_state != playing) return;
	if (_current_play_position > 100000) {
		stop();
		play_song(_playing_album, _playing_index);
	} else {
		if (_playing_index == 0) {
			play_system_sound("no_prev_song.mp3");
			return;
		}
		stop();
		play_song(_playing_album, _playing_index - 1);
	}
}

bool Player::is_playing() {
	return _state == playing;
}

std::list<String> Player::ls(String path, bool withFiles, bool withDirs, bool withHidden) {
	_spi->select_sd();
	std::list<String> result;
	if (!SD.exists(path)) return result;
	File dir = SD.open(path);
	File entry;
	while (entry = dir.openNextFile()) {
		if (!withDirs && entry.isDirectory()) continue;
		if (!withFiles && !entry.isDirectory()) continue;
		String filename = entry.name();
		if (!withHidden && filename.startsWith(".")) continue;
		if (entry.isDirectory()) filename.concat("/");
		result.push_back(filename);
	}
	_spi->select_sd(false);
	result.sort();
	return result;
}

String Player::_find_album_dir(String id) {
	_spi->select_sd();
	if (id.endsWith("/")) id = id.substring(0, id.length() - 1);
	String id_with_divider = id + " - ";
	File root = SD.open("/");
	File entry;
	String result = String("");
	while ((result.length()==0) && (entry = root.openNextFile())) {
		String name = entry.name() + 1;
		TRACE("Checking if '%s' startsWith '%s'...\n", name.c_str(), id.c_str());
		if (entry.isDirectory() && (name.startsWith(id_with_divider) || name.equals(id))) {
			result = name;
		}
		entry.close();
	}
	root.close();
	_spi->select_sd(false);
	return result;
}

std::list<String> Player::_files_in_dir(String path) {
	_spi->select_sd();
	TRACE("Examining folder %s...\n", path.c_str());
	if (!path.startsWith("/")) path = String("/") + path;
	//if (!path.endsWith("/")) path.concat("/");
	std::list<String> result;
	if (!SD.exists(path)) {
		DEBUG("Could not open path '%s'.\n", path.c_str());
		_spi->select_sd(false);
		return result;
	}
	File dir = SD.open(path);
	File entry;
	while (entry = dir.openNextFile()) {
		String filename = entry.name();
		filename = filename.substring(path.length() + 1);
		if (!entry.isDirectory() &&
		    !filename.startsWith(".") &&
		    ( filename.endsWith(".mp3") ||
		      filename.endsWith(".ogg") ||
		      filename.endsWith(".wma") ||
		      filename.endsWith(".mp4") ||
		      filename.endsWith(".mpa"))) {
			TRACE("    Adding entry %s\n", entry.name());
			result.push_back(entry.name());
		} else {
			TRACE("  Ignoring entry %s\n", filename.c_str());
		}
		entry.close();
	}
	dir.close();
	_spi->select_sd(false);
	result.sort();

	return result;
}

String Player::_random_album() {
	std::list<String> albums = ls("/", false, true, false);
	uint8_t rnd = random(albums.size());
	std::list<String>::iterator it = albums.begin();
	for (int i=0; i<rnd; i++) { it++; }
	return *it;
}

void Player::play_random_album() {
	play_album(_random_album());
}

bool Player::play_album(String album) {
	album_state s = _last_tracks[album.c_str()];
	DEBUG("Last index for album %s was %d,%d\n", album.c_str(), s.index, s.position);
	return play_song(album, s.index, s.position);
}

bool Player::play_song(String album, uint8_t index, uint32_t skip_to) {
	if (_state == sleeping || _state == recording) _wakeup();
	if (_state != idle) return false;
	DEBUG("Trying to play song at index %d, offset %d of album %s\n", index, skip_to, album.c_str());
	String path = _find_album_dir(album);
	if (path.length()==0) {
		ERROR("Could not find album.\n");
		return false;
	} else {
		INFO("Found album in dir '%s'.\n", path.c_str());
	}
	std::list<String> files = _files_in_dir(path);
	_playing_album_songs = files.size();
	DEBUG("Found %d songs in album\n", files.size());
	if (index >= files.size()) {
		ERROR("No matching file found - not playing.\n");
		return false;
	}
	//std::list<String>::iterator it = files.begin();
	//std::advance(it, index);
	String file = *(std::next(files.begin(), index));
	_state = playing;
	_playing_album = album;
	_playing_index = index;
	_set_last_track(album.c_str(), index, skip_to);
	_play_file(file, skip_to);
	return true;
}

void Player::play_system_sound(String filename) {
	//String file = String("/system/") + filename;
	String file = filename;
	if (!SD.exists(file)) {
		ERROR("File %s does not exist!\n", file.c_str());
		return;
	}
	if (_state == playing) {
		stop();
		_state = system_sound_while_playing;
	} else {
		_state = system_sound_while_stopped;
	}
	_play_file(file, 0);
}

void Player::_play_file(String file, uint32_t file_offset) {
	INFO("play_file('%s', %d)\n", file.c_str(), file_offset);
	_spi->select_sd();
	_file = SD.open(file);
	_spi->select_sd(false);
	if (!_file) {
		DEBUG("Could not open file %s", file.c_str());
		return;
	}

	DEBUG("Resetting SCI_DECODE_TIME...\n");
	_write_control_register(SCI_DECODE_TIME, 0);
	DEBUG("Resetting SS_DO_NOT_JUMP...\n");
	_write_control_register(SCI_STATUS, _read_control_register(SCI_STATUS) & ~SS_DO_NOT_JUMP);
	delay(100);

	_spi->select_sd();
	if (file_offset == 0) {
		_file.seek(_id3_tag_offset(_file));
	}
	_refills = 0;
	_current_play_position = _file.position();
	_spi->select_sd(false);
	_skip_to = file_offset;
	if (_skip_to>0) _mute();
	else _speaker_on();
	INFO("Now playing.\n");
}

uint32_t Player::_id3_tag_offset(File f) {
	uint32_t original_position = f.position();
	uint32_t offset = 0;
	if (f.read()=='I' && f.read()=='D' && f.read()=='3') {
		DEBUG("ID3 tag found\n");
		// Skip ID3 tag version
		f.read(); f.read();
		byte tags = f.read();
		bool footer_present = tags & 0x10;
		DEBUG("ID3 footer found: %d\n", footer_present);
		for (byte i=0; i<4; i++) {
			offset <<= 7;
			offset |= (0x7F & f.read());
		}
		offset += 10;
		if (footer_present) offset += 10;
		DEBUG("ID3 tag length is %d bytes.\n", offset);
	} else {
		DEBUG("No ID3 tag found\n");
	}
	f.seek(original_position);
	return offset;
}

void Player::_flush(uint count, int8_t byte) {
	_spi->select_vs1053_xdcs();
	SPI.beginTransaction(*_spi_settings);
	for(uint i=0; i<count; i++) {
		_wait();
		SPI.transfer(byte);
	}
	SPI.endTransaction();
	_spi->select_vs1053_xdcs(false);
}

void Player::_finish_playing() {
	uint8_t endbyte = _get_endbyte();
	_flush(2052, endbyte);
	_write_control_register(SCI_MODE, _read_control_register(SCI_MODE) | SM_CANCEL);
	for (int i=0; i<64; i++) {
		_flush(32, endbyte);
		uint16_t mode = _read_control_register(SCI_MODE);
		if ((mode & SM_CANCEL) == 0) return;
	}
	// If we reached this, the Chip didn't stop. That should not happen.
	// (That's written in the manual.)
	// Reset the chip.
	_init();
}

void Player::stop(bool turn_speaker_off) {
	if (_state != playing /* && _state != system_sound_while_playing && _state != system_sound_while_stopped*/) return;
	INFO("Stopping...\n");
	if (_state == playing) {
		_set_last_track(_playing_album.c_str(), _playing_index, (uint32_t)_file.position());
	}
	_state = stopping;
	_stop_delay = 0;
	_write_control_register(SCI_MODE, _read_control_register(SCI_MODE) | SM_CANCEL);
	uint8_t endbyte = _get_endbyte();
	while (true) {
		_refill();
		uint16_t mode = _read_control_register(SCI_MODE);
		if ((mode & SM_CANCEL) == 0) {
			_flush(2052, endbyte);
			_finish_stopping(turn_speaker_off);
			break;
		} else if (_stop_delay > 2048) {
			init();
			break;
		}
		_stop_delay++;
	}
}

void Player::_finish_stopping(bool turn_speaker_off) {
	if (turn_speaker_off) _speaker_off();
	_state = idle;
	_stopped_at = millis();
	if (_file) {
		_file.close();
	}
	INFO("Stopped.\n");
}

void Player::_refill() {
	_spi->select_sd();
	_refills++;
	if (_refills % 1000 == 0) DEBUG(".");
	uint8_t result = _file.read(_buffer, sizeof(_buffer));
	_spi->select_sd(false);
	if (result == 0) {
		// File is over.
		DEBUG("EOF reached.\n");
		_skip_to = 0;
		_finish_playing();
		if (_state == system_sound_while_playing) {
			_finish_stopping(false);
			play_album(_playing_album);
			return;
		} else if (_state == system_sound_while_stopped) {
			_finish_stopping(true);
			return;
		}
		_finish_stopping(false);
		bool result = play_song(_playing_album, _playing_index + 1);
		if (!result) {
			_set_last_track(_playing_album.c_str(), 0, 0);
		}
		return;
	}
	_current_play_position+=result;
	_write_data(_buffer);

	if (_skip_to > 0) {
		if (_skip_to > _file.position()) {
			uint16_t status = _read_control_register(SCI_STATUS);
			if ((status & SS_DO_NOT_JUMP) == 0) {
				DEBUG("Skipping to %d.\n", _skip_to);
				_flush(2048, _get_endbyte());
				_spi->select_sd();
				_file.seek(_skip_to);
				_spi->select_sd(false);
				_skip_to = 0;
				_unmute();
			}
		} else {
			_skip_to = 0;
			_unmute();
		}
	}
}

bool Player::_refill_needed() {
	return _state==playing ||
		_state==stopping ||
		_state==system_sound_while_playing ||
		_state==system_sound_while_stopped;
}

bool Player::loop() {
	if (PIN_VS1053_DREQ() && _refill_needed()) {
		_refill();
		return true;
	}
	
	if (_state == recording) {
		uint16_t samples_available = _read_control_register(SCI_HDAT1);
		//DEBUG("Samples available: %d\n", samples_available);
		if (samples_available >= 500) {
			unsigned long sum = 0;
			for (int i=0; i<500; i++) {
				uint16_t sample = _read_control_register(SCI_HDAT0);
				sum += sample * sample;
			}
			double result = sqrt(sum / 500);
			DEBUG("Loudness: %f", result);
		}
	}

	if (_state == idle && _stopped_at < millis() - VS1053_SLEEP_DELAY) {
		//_sleep();
		_record();
	}
	return false;
}

void Player::_set_last_track(const char* album, uint8_t index, uint32_t position) {
	DEBUG("Setting _last_track[%s]=%d,%d.\n", album, index, position);
	_last_tracks[album] = {index, position};
}