#include <rcc/flash.h>
#include <rcc/rcc.h>
#include <syscfg/syscfg.h>
#include <gpio/gpio.h>
#include <os/time.h>
#include <spi/spi.h>
#include <usb/usb.h>
#include <usb/descriptor.h>

auto dev_desc = device_desc(0x200, 0, 0, 0, 64, 0x1234, 0x56e3, 0, 0, 0, 0, 1);
auto conf_desc = configuration_desc(1, 1, 0, 0xc0, 0,
	// MSC BBB
	interface_desc(0, 0, 2, 0x08, 0x06, 0x50, 0,
		endpoint_desc(0x81, 0x02, 64, 0), // IN
		endpoint_desc(0x01, 0x02, 64, 0)  // OUT
	)
);

desc_t dev_desc_p = {sizeof(dev_desc), (void*)&dev_desc};
desc_t conf_desc_p = {sizeof(conf_desc), (void*)&conf_desc};

USB_l0 usb(USB, dev_desc_p, conf_desc_p);


Pin nfc_sck     = GPIOB[3];
Pin nfc_miso    = GPIOB[4];
Pin nfc_mosi    = GPIOB[5];
Pin nfc_ss      = GPIOB[6];
Pin nfc_irq_out = GPIOB[7];
Pin nfc_irq_in  = GPIOC[13];

class CR95HF {
	private:
		SPI_t& spi;
		Pin& ss;
	
	public:
		CR95HF(SPI_t& spi_bus, Pin& p) : spi(spi_bus), ss(p) {}
		
		void send_cmd(uint8_t cmd, uint32_t len, uint8_t* buf) {
			ss.off();
			
			spi.transfer_byte(0);
			spi.transfer_byte(cmd);
			spi.transfer_byte(len);
			
			while(len--) {
				spi.transfer_byte(*buf++);
			}
			
			ss.on();
		}
		
		uint32_t get_response(uint32_t maxlen, uint8_t* buf) {
			uint32_t len = 0;
			
			while(nfc_irq_out.get());
			
			ss.off();
			
			spi.transfer_byte(2);
			*buf++ = spi.transfer_byte();
			len++;
			
			uint32_t datalen = spi.transfer_byte();
			*buf++ = datalen;
			len++;
			
			while(len < datalen + 2) {
				*buf++ = spi.transfer_byte();
				len++;
			}
			
			ss.on();
			
			return len;
		}
};

CR95HF cr95hf(SPI1, nfc_ss);
/*
class USB_CR95HF : public USB_class_driver {
	private:
		USB_generic& usb;
		
		uint8_t pend_reg;
		uint32_t pend_len;
		uint8_t buf[256];
		
		bool write(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
			if(wLength > sizeof(buf)) {
				return false;
			}
			
			if(wLength == 0) {
				cr95hf.send_cmd(wValue, 0, nullptr);
				
				usb.write(0, nullptr, 0);
				
				return true;
			}
			
			pend_reg = wValue;
			pend_len = wLength;
			// Awaiting OUT packet.
			
			return true;
		}
		
		bool read(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
			if(wLength > sizeof(buf)) {
				return false;
			}
			
			wLength = cr95hf.get_response(wLength, buf);
			
			// TODO: wLength >= 64
			
			usb.write(0, (uint32_t*)buf, wLength);
			return true;
		}
	
	public:
		USB_CR95HF(USB_generic& usbd) : usb(usbd) {
			usb.register_driver(this);
		}
	
	protected:
		virtual SetupStatus handle_setup(uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
			if(bmRequestType == 0x40 && bRequest == 0xf2) {
				return write(wValue, wIndex, wLength) ? SetupStatus::Ok : SetupStatus::Stall;
			}
			
			if(bmRequestType == 0xc0 && bRequest == 0xf3) {
				return read(wValue, wIndex, wLength) ? SetupStatus::Ok : SetupStatus::Stall;
			}
			
			return SetupStatus::Unhandled;
		}
		
		virtual void handle_out(uint8_t ep, uint32_t len) {
			if(ep != 0 || len == 0) {
				return;
			}
			
			if(len != pend_len) {
				return;
			}
			
			usb.read(0, (uint32_t*)buf, len);
			
			cr95hf.send_cmd(pend_reg, len, buf);
			
			usb.write(0, nullptr, 0);
		}
};

USB_CR95HF usb_cr95hf(usb);
*/
struct CBW {
	uint32_t dCBWSignature;
	uint32_t dCBWTag;
	uint32_t dCBWDataTransferLength;
	uint8_t bmCBWFlags;
	uint8_t bCBWLUN;
	uint8_t bCBWCBLength;
	uint8_t CBWCB[16];
} __attribute__((packed));

struct CSW {
	uint32_t dCSWSignature;
	uint32_t dCSWTag;
	uint32_t dCSWDataResidue;
	uint8_t bCSWStatus;
} __attribute__((packed));

uint32_t nullbuf[16];
uint32_t capbuf[] = {
	0x00010000,
	0x00020000
};
uint32_t inquirybuf[] = {
	0x02068020,
	32,
	
	0x20202020, 0x20202020,
	
	0x20202020, 0x20202020, 0x20202020, 0x20202020,
	
	0x34333231,
};

uint32_t sensebuf[] = {
	0x00050070,
	0x0a000000,
	0,
	0x00000020,
	0
};

uint32_t modesensebuf[] = {
	0x00800004
};

//uint32_t sd_buf[512];

uint8_t sectors[][512] {
	{}, // NULL
	{ // MBR
		0xeb, 0x3c, 0x90, 0x6d, 0x6b, 0x64, 0x6f, 0x73, 0x66, 0x73, 0x00, 0x00, 0x02, 0x04, 0x01, 0x00,
		0x02, 0x00, 0x02, 0x00, 0x01, 0xf8, 0x01, 0x00, 0x20, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x29, 0xc9, 0x6a, 0x10, 0xc4, 0x20, 0x20, 0x20, 0x20, 0x20,
		0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x46, 0x41, 0x54, 0x31, 0x32, 0x20, 0x20, 0x20, 0x0e, 0x1f,
		0xbe, 0x5b, 0x7c, 0xac, 0x22, 0xc0, 0x74, 0x0b, 0x56, 0xb4, 0x0e, 0xbb, 0x07, 0x00, 0xcd, 0x10,
		0x5e, 0xeb, 0xf0, 0x32, 0xe4, 0xcd, 0x16, 0xcd, 0x19, 0xeb, 0xfe, 0x54, 0x68, 0x69, 0x73, 0x20,
		0x69, 0x73, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x61, 0x20, 0x62, 0x6f, 0x6f, 0x74, 0x61, 0x62, 0x6c,
		0x65, 0x20, 0x64, 0x69, 0x73, 0x6b, 0x2e, 0x20, 0x20, 0x50, 0x6c, 0x65, 0x61, 0x73, 0x65, 0x20,
		0x69, 0x6e, 0x73, 0x65, 0x72, 0x74, 0x20, 0x61, 0x20, 0x62, 0x6f, 0x6f, 0x74, 0x61, 0x62, 0x6c,
		0x65, 0x20, 0x66, 0x6c, 0x6f, 0x70, 0x70, 0x79, 0x20, 0x61, 0x6e, 0x64, 0x0d, 0x0a, 0x70, 0x72,
		0x65, 0x73, 0x73, 0x20, 0x61, 0x6e, 0x79, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x74, 0x6f, 0x20, 0x74,
		0x72, 0x79, 0x20, 0x61, 0x67, 0x61, 0x69, 0x6e, 0x20, 0x2e, 0x2e, 0x2e, 0x20, 0x0d, 0x0a, 0x00,
	},
	{ // FAT
		0xf8, 0xff, 0xff, 0x00, 0xf0, 0xff,
	},
	{ // File table
		0x41, 0x63, 0x00, 0x61, 0x00, 0x72, 0x00, 0x64, 0x00, 0x30, 0x00, 0x0f, 0x00, 0xa7, 0x2e, 0x00,
		0x74, 0x00, 0x78, 0x00, 0x74, 0x00, 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff,
		0x43, 0x41, 0x52, 0x44, 0x30, 0x20, 0x20, 0x20, 0x54, 0x58, 0x54, 0x20, 0x00, 0x64, 0x53, 0x95,
		0x03, 0x43, 0x03, 0x43, 0x00, 0x00, 0x53, 0x95, 0x03, 0x43, 0x03, 0x00, 0x10, 0x00, 0x00, 0x00,
	},
	"E004123456789AB",
};

uint8_t* get_sector(uint32_t sector) {
	switch(sector) {
		case 0:
			return sectors[1];
		
		case 1:
		case 2:
			return sectors[2];
		
		case 3:
			return sectors[3];
		
		case 39:
			return sectors[4];
		
		default:
			return sectors[0];
	}
}

class USB_MSC_BBB : public USB_class_driver {
	private:
		USB_generic& usb;
		
		uint32_t buf[16];
		
		uint32_t pending_data_in;
		
		bool pending_write;
		uint32_t pending_write_addr;
		uint32_t pending_write_num;
		
		CBW cbw;
		
		uint32_t pending_write_recv;
		
		CSW csw;
	
	public:
		USB_MSC_BBB(USB_generic& usbd) : usb(usbd) {
			usb.register_driver(this);
		}
	
	protected:
		virtual SetupStatus handle_setup(uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
			// Get max LUN
			if(bmRequestType == 0xa1 && bRequest == 0xfe) {
				uint32_t max_lun = 0;
				usb.write(0, &max_lun, 1);
				return SetupStatus::Ok;
			}
			
			// Mass Storage Reset
			if(bmRequestType == 0x21 && bRequest == 0xff) {
				usb.write(0, nullptr, 0);
				return SetupStatus::Ok;
			}
			
			return SetupStatus::Unhandled;
		}
		
		virtual void handle_set_configuration(uint8_t configuration) {
			if(configuration) {
				usb.register_out_handler(this, 1);
				usb.hw_conf_ep(0x01, EPType::Bulk, 64);
				usb.hw_conf_ep(0x81, EPType::Bulk, 64);
				
				pending_data_in = 0;
				pending_write = false;
			}
		}
		
		virtual void handle_out(uint8_t ep, uint32_t len) {
			if(ep == 0) {
				if(len) {
					usb.write(0, nullptr, 0);
				}
				
			} else if(ep == 1 && pending_data_in != 0) {
				if(pending_write) {
					usb_rblog.log("Handling write packet. (%d bytes)", len);
					
					//handle_write_block_packet();
					
				} else {
					usb_rblog.log("Ignoring %d bytes of received data.", len);
				}
				
				if(len > pending_data_in) {
					pending_data_in = 0;
				} else {
					pending_data_in -= len;
				}
				
				if(pending_data_in == 0) {
					usb.write(1, (uint32_t*)&csw, sizeof(CSW));
				}
				
			} else if(ep == 1) {
				
				// Read CBW.
				if(len != sizeof(CBW) || usb.read(ep, (uint32_t*)&cbw, len) != sizeof(CBW)) {
					usb_rblog.log("Invalid CBW size.");
					return; // FIXME: Indicate error.
				}
				
				if(cbw.dCBWSignature != 0x43425355) {
					usb_rblog.log("Invalid CBW signature.");
					return; // FIXME: Indicate error.
				}
				
				usb_rblog.log("Received CBW, tl=%d, flags=%#02x", cbw.dCBWDataTransferLength, cbw.bmCBWFlags);
				
				bool cmd_ok = handle_scsi_cmd(cbw.CBWCB, cbw.bCBWCBLength);
				
				if(!cmd_ok && cbw.dCBWDataTransferLength != 0 && cbw.bmCBWFlags & 0x80) {
					write_zero();
				}
				
				if(cmd_ok) {
					csw = {0x53425355, cbw.dCBWTag, 0, 0x00};
				} else {
					csw = {0x53425355, cbw.dCBWTag, cbw.dCBWDataTransferLength, 0x01};
				}
				
				if(cbw.dCBWDataTransferLength != 0 && !(cbw.bmCBWFlags & 0x80)) {
					pending_data_in = cbw.dCBWDataTransferLength;
					return;
				}
				
				usb.write(1, (uint32_t*)&csw, sizeof(CSW));
			}
		}
		
		void write_zero() {
			uint32_t len = cbw.dCBWDataTransferLength;
			while(len > 64) {
				usb.write(1, nullbuf, 64);
				while(!usb.ep_ready(1));
				len -= 64;
			}
			
			usb.write(1, nullbuf, len);
			while(!usb.ep_ready(1));
		}
		
		void handle_read_block() {
			uint32_t block = (cbw.CBWCB[2] << 24) | (cbw.CBWCB[3] << 16) | (cbw.CBWCB[4] << 8) | cbw.CBWCB[5];
			uint16_t num_blocks = (cbw.CBWCB[7] << 8) | cbw.CBWCB[8];
			
			while(num_blocks--) {
				uint32_t* bufp = (uint32_t*)get_sector(block++);
				
				for(uint32_t i = 0; i < 512; i += 64) {
					usb.write(1, bufp + i / 4, 64);
					while(!usb.ep_ready(1));
				}
			}
		}
		
		bool handle_scsi_cmd(uint8_t* cmd, uint32_t len) {
			pending_write = false;
			
			if(!len) {
				return false;
			}
			
			switch(cmd[0]) {
				// TEST UNIT READY
				case 0x00:
					return true;
				
				// REQUEST SENSE
				case 0x03:
					usb.write(1, sensebuf, cbw.dCBWDataTransferLength);
					while(!usb.ep_ready(1));
					return true;
				
				// INQUIRY
				case 0x12:
					usb.write(1, inquirybuf, cbw.dCBWDataTransferLength);
					while(!usb.ep_ready(1));
					return true;
				
				// MODE SENSE
				case 0x1a:
					usb.write(1, modesensebuf, cbw.dCBWDataTransferLength > 4 ? 4 : cbw.dCBWDataTransferLength);
					while(!usb.ep_ready(1));
					return true;
				
				// READ CAPACITY
				case 0x25:
					usb.write(1, capbuf, cbw.dCBWDataTransferLength);
					while(!usb.ep_ready(1));
					return true;
				
				// READ (10)
				case 0x28:
					handle_read_block();
					return true;
				
				default:
					return false;
			}
			
			return true;
		}
};

USB_MSC_BBB usb_msc_bbb(usb);

int main() {
	// Configure flash latency.
	FLASH.ACR = 1 << 0;
	
	// Enable HSI16.
	RCC.CR |= 1 << 0; // HSI16ON
	while(!(RCC.CR & (1 << 2))); // HSI16RDYF
	
	// Configure PLL.
	RCC.CFGR |= (1 << 22) | (1 << 18) | (0 << 16); // PLLDIV = /2, PLLMUL = 4x, PLLSRC = HSI16
	
	// Enable PLL.
	RCC.CR |= 1 << 24; // PLLON
	while(!(RCC.CR & (1 << 25))); // PLLRDY
	
	// Switch to PLL.
	RCC.CFGR |= 3 << 0; // SW = PLL
	while((RCC.CFGR & (3 << 2)) != (3 << 2)); // SWS = PLL
	
	// Initialize system timer.
	STK.LOAD = 32000000 / 8 / 1000; // 1000 Hz.
	STK.VAL = 0;
	STK.CTRL = 0x03;
	
	// Enable VREFINT for HSI48.
	RCC.enable(RCC.SYSCFG);
	SYSCFG.CFGR3 |= (1 << 13) | (1 << 0); // ENREF_HSI48, EN_VREFINT
	while(!(SYSCFG.CFGR3 & (1 << 26))); // REF_HSI48_RDYF
	
	// Enable HSI48.
	RCC.CRRCR |= 1 << 0; // HSI48ON
	while(!(RCC.CRRCR & (1 << 1))); // HSI48RDY
	
	// Select HSI48 for USB.
	RCC.CCIPR |= 1 << 26;
	
	RCC.enable(RCC.GPIOA);
	RCC.enable(RCC.GPIOB);
	RCC.enable(RCC.GPIOC);
	
	RCC.enable(RCC.USB);
	
	usb.init();
	
	Time::sleep(10);
	USB.reg.BCDR |= 1 << 15;
	//usb_disc.off();
	
	RCC.enable(RCC.SPI1);
	
	nfc_irq_out.set_mode(Pin::Input);
	nfc_irq_in.on();
	nfc_irq_in.set_mode(Pin::Output);
	
	nfc_ss.on();
	nfc_ss.set_mode(Pin::Output);
	nfc_sck.set_speed(Pin::High);
	nfc_sck.set_mode(Pin::AF);
	nfc_miso.set_mode(Pin::AF);
	nfc_mosi.set_mode(Pin::AF);
	
	SPI1.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (4 << 3) | (1 << 2) | (0 << 1) | (0 << 0); // SSM, SSI, SPE, 36/32MHz, MSTR, CPOL=0, CPHA=0
	SPI1.reg.CR2 = (1 << 12) | (7 << 8); // FRXTH, DS = 8bit
	
	Time::sleep(1);
	nfc_irq_in.off();
	Time::sleep(1);
	nfc_irq_in.on();
	Time::sleep(1);
	
	uint8_t buf[64];
	bool cmd_sent = false;
	uint32_t nfc_delay_until = 0;
	
	Time::sleep(1000);
	
	cr95hf.send_cmd(0x02, 2, (uint8_t*)"\x01\x05"); // Select ISO 15693
	cr95hf.get_response(64, buf);
	
	while(1) {
		if(Time::time() < nfc_delay_until) {
			// Just do nothing.
		} else if(!cmd_sent) {
			cr95hf.send_cmd(0x04, 3, (uint8_t*)"\x26\x01\x00"); // INVENTORY
			cmd_sent = true;
		} else if(!nfc_irq_out.get()) {
			cr95hf.get_response(64, buf);
			cmd_sent = false;
			
			if(buf[0] == 128 && buf[14] == 0) {
				//usb_disc.on();
				uint8_t* id_raw = &buf[11];
				uint8_t* id_ascii = sectors[4];
				for(uint32_t i = 0; i < 8; i++) {
					*id_ascii++ = "0123456789ABCDEF"[*id_raw >> 4];
					*id_ascii++ = "0123456789ABCDEF"[*id_raw-- & 0xf];
				}
				Time::sleep(10);
				//usb_disc.off();
				
				nfc_delay_until = Time::time() + 5000; // Valid for five seconds.
			} else {
				nfc_delay_until = Time::time() + 100; // Retry in 100ms.
			}
		}
		
		usb.process();
	}
}