#pragma once

#include "../../io/GPIO.h"
#include "../../Debug.h"
#include "CRC16.h"

#define TEENSY_SD_PIN_CS	46
#define TEENSY_SD_PIN_MOSI	45
#define TEENSY_SD_PIN_CLK	44
#define TEENSY_SD_PIN_MISO	43


#include "Types.h"

// http://www.dejazzer.com/ee379/lecture_notes/lec12_sd_card.pdf
// https://www.convict.lu/pdf/ProdManualSDCardv1.9.pdf
// http://rjhcoding.com/avrc-sd-interface-3.php
template <typename SPI, int PIN_CS> class SDCard {
		
	SPI& spi;

	union R1 {	// SanDisk Manual Page 5-13
		uint8_t raw;
		struct {
			uint8_t inIdleState : 1;
			uint8_t eraseReset : 1;
			uint8_t illegalCommand : 1;
			uint8_t crcError : 1;
			uint8_t eraseSeqError : 1;
			uint8_t addressError : 1;
			uint8_t paramError : 1;
			uint8_t zero : 1;
		};
		R1() {}
		R1(uint8_t val) : raw(val) {}
	};
	
	union R3 {	// response to OCR command, SanDisk Manual Page 5-14
		uint8_t raw[5];
		struct {
			R1 state;
			uint8_t busy : 1;
			uint8_t reserved : 7;
			uint32_t allowedVoltages: 20;
			uint8_t checkPattern : 4;
		};
	};
	
	union Rcmd8 {	// response to cmd8
		uint8_t raw[5];
		struct {
			R1 state;
			uint8_t data[4];
		};
	};
		
	
	
	static constexpr const char* NAME = "SDCard";
	
public:
	
	SDCard(SPI& spi) : spi(spi) {

	}
	
	bool init() {

		MyGPIO::setOutput(PIN_CS);
		Log::addInfo(NAME, "init()");
		
		// RESET: MOSI = 1, CS = 1 (deselected!!), at least 74 Clocks
		deselect();
		for (uint8_t i = 0; i < 10; ++i) {spi.writeByte(0xFF);}
		
		// switch to SPI mode
		for (uint8_t i = 0; ; ++i) {
			const R1 res = cmd0();
			if (res.raw == 1) {break;}
			if (i == 8) {Log::addError(NAME, "init failed"); return false;}
			delay(50);
		}
		
		
		// TODO?
		// Version 2.0 Card
		Rcmd8 r8 = cmd8();

		if (r8.state.raw == 1) {
			Log::addInfo(NAME, "V2/V3 Card");
			if (r8.data[2] == 0x01 && r8.data[3] == 0xAA) {
				Log::addInfo(NAME, "Pattern Correct");
			} else {
				Log::addError(NAME, "Pattern Mismatch");
				return false;
			}
		}
	
		// Version 1.0 Card
		//R3 res = cmd58();
		
		
		
		uint32_t hcs = 1<<30;

		for (uint8_t i = 0; ; ++i) {
			delay(100);
			R1 r1 = acmd41(hcs);
			if (i == 8)			{Log::addError(NAME, "init failed"); return false;}
			if (r1.raw == 0)	{break;}	// finished
			if (r1.raw == 1)	{continue;}	// card is still idle
			if (r1.raw  > 1)	{Log::addError(NAME, "init failed"); return false;}
		}
		
		Log::addInfo(NAME, "init OK");
		return true;
		
	}
	

private:

	//LBA512 lastReadBlock = 0xFFFFFFFF;
	//LBA512 lastWriteBlock = 0xFFFFFFFF;

public:

	/** read a single block of 512 bytes, addr = byteAddr/512 */
	bool readBlock(LBA512 addr, uint8_t* dst) {

		/*
		if (lastReadBlock == 0xFFFFFFFF) {
			if (!startMultiRead(addr))	{endCMD(); return false;}
			if (!readMultiBlock(dst))	{endCMD(); return false;}

		} else if ((lastReadBlock + 1) == addr) {
			if (!readMultiBlock(dst))	{endCMD(); return false;}

		} else {
			if (!stopMultiRead())		{endCMD(); return false;}
			if (!startMultiRead(addr))	{endCMD(); return false;}
			if (!readMultiBlock(dst))	{endCMD(); return false;}
		}

		lastReadBlock = addr;
		return true;
		*/

		// bulletproof, but slower
		return readSingleBlock(addr, dst);

	}

	/** write a single block of 512 bytes, addr = byteAddr/512 */
	bool writeBlock(LBA512 addr, const uint8_t* src) {

		// bulletproof, but slow
		return writeSingleBlock(addr, src);

	}
	
private:

	/*
	bool startMultiRead(LBA512 addr) {

		sendCMD(18, addr>>24, addr>>16, addr>>8, addr>>0, 0xFF);
		R1 res; readResponse(&res.raw, 1);

		Log::addInfo(NAME, "startMultiRead(%d*512): %02x", addr, res.raw);

		if (res.raw != 0)		{Log::addError(NAME, "failed"); endCMD(); return false;}

		// wait for data to become available
		for (uint16_t i = 0; ; ++i) {
			uint8_t res = spi.readWriteByte(0xFF);
			if (res == 0xFE)	{break;}													// available!
			if (res != 0xFF)	{Log::addError(NAME, "invalid"); endCMD(); return false;}	// invalid response
			if (i > 1024)		{Log::addError(NAME, "timeout"); endCMD(); return false;}	// timeout
		}

		return true;

	}

	bool readMultiBlock(uint8_t* dst) {

		Log::addInfo(NAME, "readMultiBlock()");

		for (uint16_t i = 0; i < 512; ++i) {
			dst[i] = spi.readWriteByte(0xFF);
		}

		return true;

	}

	bool stopMultiRead() {

		sendCMD(12, 0,0,0,0, 0xFF);
		R1 res; readResponse(&res.raw, 1);

		Log::addInfo(NAME, "stopMultiRead(): %02x", res.raw);

		endCMD();
		lastReadBlock = 0xFFFFFFFF;

		return true;//(res.raw == 0);

	}
	*/

	/** select the card (CS = low) */
	void select() {
		spi.writeByte(0xFF);
		MyGPIO::clear(PIN_CS);
		spi.writeByte(0xFF);
	}

	/** deselect the card (CS = high) */
	void deselect() {
		spi.writeByte(0xFF);
		MyGPIO::set(PIN_CS);
		spi.writeByte(0xFF);
	}

	/** send a new command */
	void sendCMD(uint8_t cmd, uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t arg3, uint8_t crc) {
		uint8_t buf[6];
		buf[0] = (0b01 << 6) | (cmd&0b111111);
		buf[1] = arg0;
		buf[2] = arg1;
		buf[3] = arg2;
		buf[4] = arg3;
		buf[5] = ((crc&0b1111111) << 1) | 1;		
		select();
		spi.write(buf, 6);
	}
	
	/** end the current command */
	void endCMD() {
		deselect();
		//spi.writeByte(0xFF);		// SanDisk Manual Page 5-6 (required for card to finish current operation)
	}
	
	
	R1 cmd0() {		// SanDisk Manual Page 5-1
		sendCMD(0, 0x00, 0x00, 0x00, 0x00, 0x4A);
		R1 res;	readResponse(&res.raw, 1);
		endCMD();
		Log::addInfo(NAME, "cmd0: %02x", res.raw);
		return res;
	}
	
	/** determine whether this is a V2 card?? */
	Rcmd8 cmd8() {
		sendCMD(8, 0x00, 0x00, 0x01, 0xAA, 0b1000011);	// correct?	Needs Checksum!!
		//select(); uint8_t tmp[] = {0b01001000, val>>24, val>>16, val>>8, val>>0, 0b00001111};	spi.write(tmp, 6);
		//R3 res; readResponse(&res.raw[0], 1);	// read the R1 part 
		//if (res.raw[0] == 1) {					// command supported
		//	readResponse(&res.raw[1], 4);		// read the rest of the response
		//}
		Rcmd8 res; readResponse(res.raw, 5);
		endCMD();
		Log::addInfo(NAME, "cmd8: %02x %02x %02x %02x %02x", res.raw[0], res.raw[1], res.raw[2], res.raw[3], res.raw[4]);
		return res;
	}
	
	
	
	/** read OCR (Operating Conditions Register) */
	R3 cmd58() {
		//select();	uint8_t tmp[] = {0b01111010, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b01110101};	spi.write(tmp, 6);
		sendCMD(58, 0x00, 0x00, 0x00, 0x00, 0xFF);//0b0111010);	
		R3 res; readResponse(res.raw, 5);	// typical response: 0x01   0x00 0xff 0x80 0x00
		endCMD();
		Log::addInfo(NAME, "cmd58: %02x %02x %02x %02x %02x", res.raw[0], res.raw[1], res.raw[2], res.raw[3], res.raw[4]);
		return res;
	}
	
	
	
	/** "next command is app specific" */
	R1 cmd55() {
		sendCMD(55, 0x00, 0x00, 0x00, 0x00, 0xFF);
		R1 res; readResponse(&res.raw, 1);
		endCMD();
		Log::addInfo(NAME, "cmd55: %02x", res.raw);
		return res;
	}
	
	R1 acmd41(uint32_t val) {
		cmd55();
		sendCMD(41, val>>24, val>>16, val>>8, val>>0, 0xFF);
		R1 res; readResponse(&res.raw, 1);
		endCMD();
		Log::addInfo(NAME, "acmd41: %02x", res.raw);
		return res;
	}
	
	

	/** read the card's response to the last command */
	void readResponse(uint8_t* dst, uint8_t len) {
		
		// the SD card takes some time to answer, but must receive clock signals meanwhile!
		// we detect the actual response as soon as the received byte starts has the highest bit zero.
		// [while busy the SD card responds with 0xFF]
		// NOTE: it is IMPORTANT to send 0xFF to the card while reading its responses!
		
		// wait for the first byte to arrive and read it
		for (uint8_t i = 0; i < 32; ++i) {
			dst[0] = spi.readWriteByte(0xFF);
			if ( (dst[0] & 0x80) == 0 ) {break;}
		}
		
		// read all subsequent bytes (if any)
		for (uint8_t i = 1; i < len; ++i) {
			dst[i] = spi.readWriteByte(0xFF);
		}

	}

	
	
private:

	/** most simple read operation: read a single block of 512 bytes, addr = byteAddr/512 */
	bool readSingleBlock(LBA512 addr, uint8_t* dst) {

		//spi.setSlowdown(0);

		sendCMD(17, addr>>24, addr>>16, addr>>8, addr>>0, 0xFF);
		R1 res; readResponse(&res.raw, 1);

		Log::addInfo(NAME, "readSingleBlock(%d*512): %02x", addr, res.raw);

		// read command OK?
		if (res.raw != 0)		{Log::addError(NAME, "failed"); endCMD(); return false;}

		// wait for data to become available
		for (uint16_t i = 0; ; ++i) {
			uint8_t res = spi.readWriteByte(0xFF);
			if (res == 0xFE)	{break;}												// available!
			if (res != 0xFF)	{Log::addError(NAME, "invalid"); endCMD(); return false;}	// invalid response
			if (i > 32768)		{Log::addError(NAME, "timeout"); endCMD(); return false;}	// timeout
		}

		// read data
		for (uint16_t i = 0; i < 512; ++i) {
			dst[i] = spi.readWriteByte(0xFF);
		}

		//spi.setSlowdown(128);

		// done
		endCMD();
		return true;

	}

	bool waitWhileBusy() {

		Log::addInfo(NAME, "waitWhileBusy()");

		uint8_t res = 0;
		for (uint16_t i = 0; ; ++i) {
			res = spi.readWriteByte(0xFF);
			if (res == 0xFF)	{return true;}
			if (i >= 1024)		{return false;}	// timeout
		}

	}

	/** most simple write operation: write a single block of 512 bytes, addr = byteAddr/512 */
	bool writeSingleBlock(LBA512 addr, const uint8_t* src) {

		sendCMD(24, addr>>24, addr>>16, addr>>8, addr>>0, 0xFF);
		R1 res; readResponse(&res.raw, 1);

		Log::addInfo(NAME, "writeSingleBlock(%d*512): %02x", addr, res.raw);

		// write command OK?
		if (res.raw != 0)		{Log::addError(NAME, "failed"); endCMD(); return false;}

		// wait while the card is busy
		if (!waitWhileBusy())	{Log::addError(NAME, "busy timeout"); endCMD(); return false;}

		// send BLOCK_START
		spi.writeByte(0xFE);

		// send data
		for (uint16_t i = 0; i < 512; ++i) {
			spi.writeByte(src[i]);
		}

		// send CRC16
		// NOTE: it seems that it doesnt matter what we send here
		// the card is always fine with it?!
		//const uint16_t crc = CRC16::get(src, 512);
		//spi.writeByte(crc >> 8);
		//spi.writeByte(crc);
		spi.writeByte(0xFF);
		spi.writeByte(0xFF);

		//const uint8_t DATA_RESPONSE_MASK = 0x1F;
		//const uint8_t DATA_RESPONSE_DATA_ACCEPTED = ((2 << 1) | 1);

		//uint8_t crcRes = spi.readWriteByte(0xFF);
		const uint8_t crcRes = spi.readWriteByte(0xFF);	// response to CRC
		//R1 res2; readResponse(&res2.raw, 1);
		//Log::addInfo(NAME, "writeSingleBlockCRC(): %02x %02x", tmp, res2.raw);

		// wait for card to finish writing
		// should be a series of 0x00 followed by something != 0x00 and then a series of 0xFF
		// TODO: check the non 0x00 value?
		while(true) {
			if (spi.readWriteByte(0xFF) == 0xFF) {break;}
		}

		endCMD();
		return true;
		//return (crcRes & DATA_RESPONSE_MASK) == DATA_RESPONSE_DATA_ACCEPTED;

	}

	
};