// 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::Player(SDCard* c) {
	pinMode(XRESET, OUTPUT);
	digitalWrite(XRESET, HIGH);
	pinMode(DREQ, INPUT);

	_init();
}

void Player::_reset() {
	digitalWrite(XRESET, LOW);
	delay(50);
	digitalWrite(XRESET, HIGH);
	delay(50);
}

void Player::_init() {
	SPI.setClockDivider(SPI_CLOCK_DIV16);

	_reset();
	uint16_t result;
	for(uint8_t x=0; x<255; x++) {
		result = _read_register(SCI_MODE, 0, x);
		if (result != 0) Serial.printf("Try %3d: 0x%04X\n", x, result);
	}
	delay(10);
	/*_read_register(SCI_MODE, SPI_CLOCK_DIV16); // First read fails for some unknown reason...
	_read_register(SCI_MODE, SPI_CLOCK_DIV16);
	_read_register(SCI_STATUS, SPI_CLOCK_DIV16);

	_read_register(SCI_STATUS, SPI_CLOCK_DIV16);


	_read_register(SCI_MODE, SPI_CLOCK_DIV16);
	_read_register(SCI_MODE, SPI_CLOCK_DIV16);
	uint16_t response = _read_register(SCI_MODE, SPI_CLOCK_DIV16);
	if (response != 0x4800) {
		Serial.printf("Initialization failed. SCI_MODE was: 0x%04X. Expected: 0x4800\n", response);
		return;
	}*/
	/*_write_register(SCI_CLOCKF, 0x6000); // Set multiplier to 3x
	delay(10);
	response = _read_register(SCI_CLOCKF);
	if (response != 0x6000) {
		Serial.printf("Initialization failed. SCI_CLOCKF was: 0x%04X. Expected: 0x6000\n", response);
		return;
	}*/
	//Serial.println(result, HEX);
}

inline void Player::_wait() {
	delayMicroseconds(100);
	//Serial.print("Waiting for DREQ...");
	while(!digitalRead(DREQ));
	//Serial.printf(" done (%d cycles).\n", i);
}

uint16_t Player::_read_register(uint8_t address, uint32_t spi_speed, uint16_t t) {
	Serial.printf("Try %03d, querying register 0x%02X...", t, address);
	//SPI.setClockDivider(spi_speed);
	if (t & (1<<1)) _wait();
	SPIMaster::enable(PIN_VS1053_XCS);
	if (t & (1<<2)) _wait();
	//_wait();
	SPI.transfer(CMD_READ);
	if (t & (1<<3)) _wait();
	SPI.transfer(address);
	if (t & (1<<4)) _wait();
	uint8_t b1 = SPI.transfer(0xFF);
	if (t & (1<<5)) _wait();
	uint8_t b2 = SPI.transfer(0xFF);
	if (t & (1<<6)) _wait();

	uint16_t result = b1 << 8 | b2;
	Serial.printf("Response: 0x%02X 0x%02X -> 0x%04X\n", b1, b2, result);
	if (t & (1<<7)) delay(10);

	//_wait();
	//SPIMaster::disable();

	return result;
}

void Player::_write_register(uint8_t address, uint16_t value, uint32_t spi_speed) {
	Serial.printf("Writing to register 0x%02X...", address);
	SPI.setClockDivider(spi_speed);
	SPIMaster::enable(PIN_VS1053_XCS);
	SPI.transfer(CMD_WRITE);
	SPI.transfer(address);
	SPI.transfer(value >> 8);
	SPI.transfer(value & 0xFF);
	_wait();
	SPIMaster::disable();
	Serial.println(" done.");
}