#pragma once

#include "../../io/GPIO.h"
#include "../../Debug.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


// 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);
		debugMod(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 == 4) {debugMod(NAME, "init failed"); return false;}
			delay(50);
		}
		
		
		// TODO?
		// Version 2.0 Card
		Rcmd8 r8 = cmd8();
		
		if (r8.state.raw == 1) {
			debugMod(NAME, "V2/V3 Card");
			if (r8.data[2] == 0x01 && r8.data[3] == 0xAA) {
				debugMod(NAME, "Pattern Correct");
			} else {
				debugMod(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)		{debugMod(NAME, "init failed"); return false;}
			if (r1.raw == 0) {break;}	// finished
			if (r1.raw == 1) {continue;}	// card is still idle
			if (r1.raw  > 1) {debugMod(NAME, "init failed"); return false;}
		}
		
		debugMod(NAME, "init OK");
		return true;
		
	}
	
	uint32_t read(uint32_t addr, uint32_t size, uint8_t* dst) {
		
		uint32_t read = 0;
		uint32_t readAddr = addr & (0xFFFFFFFF - 512);	// 512 byte aligned staring address
		
		if (readAddr != addr) {							// non-aligned first read?
			uint8_t buf[512];
			uint16_t cOff = (addr-readAddr);			// skip the unneeded bytes
			uint16_t cSize = 512 - cOff;
			readSingleBlock(readAddr, buf);				// read a full block
			memcpy(dst, buf+cOff, cSize);				// copy only the required bytes
			size -= cSize;
			dst += cSize;
			read += cSize;
			readAddr += 512;
		}
		
		while(size >= 512) {
			readSingleBlock(readAddr, dst);
			readAddr += 512;
			dst += 512;
			read += 512;
			size -= 512;
		}
		
		if (size > 0) {
			uint8_t buf[512];
			readSingleBlock(readAddr, buf);
			memcpy(dst, buf, size);
			read += size;
		}
		
		return read;
		
	}
	
	/** read a single block of 512 bytes */
	bool readSingleBlock(uint32_t addr, uint8_t* dst) {
		
		addr = addr / 512;
		sendCMD(17, addr>>24, addr>>16, addr>>8, addr>>0, 0xFF);
		R1 res; readResponse(&res.raw, 1);
		debugMod2(NAME, "readSingleBlock(%08x): %02x", addr, res.raw);
		
		// read command OK?
		if (res.raw != 0) {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)	{debugMod(NAME, "invalid"); endCMD(); return false;}	// invalid response
			if (i > 1024)		{debugMod(NAME, "timeout"); endCMD(); return false;}	// timeout
		}
		
		// read data
		for (uint16_t i = 0; i < 512; ++i) {
			dst[i] = spi.readWriteByte(0xFF);
		}
		
		// done
		endCMD();
		return true;
				
	}
	
private:
	

	/** 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();
		debugMod1(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();
		debugMod5(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();
		debugMod5(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();
		debugMod1(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();
		debugMod1(NAME, "acmd41: %02x", res.raw);
		return res;
	}
	
	
	
	
	
	
	
	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 < 4; ++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);
		}

	}

	
	
	/** 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);
	}
	
};