#include <rcc/rcc.h>
#include <gpio/gpio.h>
#include <interrupt/interrupt.h>
#include <os/time.h>
#include <usb/usb.h>
#include <usb/descriptor.h>
#include <extmem/fsmc.h>

Pin fpga_prog_b = GPIOG[7];
Pin fpga_csi_b  = GPIOG[9];
Pin fpga_done   = GPIOG[1];

Pin mco2 = GPIOC[9];

Pin usb_vbus = GPIOA[9];
Pin usb_dm   = GPIOA[11];
Pin usb_dp   = GPIOA[12];

auto dev_desc = device_desc(0x200, 0, 0, 0, 64, 0x1234, 0x5678, 0, 0, 0, 0, 1);
auto conf_desc = configuration_desc(0, 1, 0, 0xc0, 0);

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

USB_otg usb(OTG_FS, dev_desc_p, conf_desc_p);

class USB_FPGA : public USB_class_driver {
	private:
		USB_generic& usb;
		
		uint32_t pending_data;
		
		volatile uint8_t* fpga_load_addr = (uint8_t*)0x64000000;
		
		bool fpga_load(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
			if(!wIndex) {
				//mco2.set_mode(Pin::Input);
				
				fpga_prog_b.off();
				Time::sleep(2);
				fpga_prog_b.on();
				Time::sleep(3);
				fpga_csi_b.off();
			}
			
			pending_data = wLength;
			
			if(!pending_data) {
				fpga_csi_b.on();
				//mco2.set_mode(Pin::AF);
				
				usb.write(0, nullptr, 0);
			}
			
			return true;
		}
		
	public:
		USB_FPGA(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 == 0xff) {
				return fpga_load(wValue, wIndex, wLength) ? SetupStatus::Ok : SetupStatus::Stall;
			}
			
			return SetupStatus::Unhandled;
		}
		
		virtual void handle_out(uint8_t ep, uint32_t len) {
			if(ep != 0) {
				return;
			}
			
			uint8_t buf[64];
			usb.read(ep, (uint32_t*)buf, len);
			
			for(uint32_t i = 0; i < len; i++) {
				*fpga_load_addr = buf[i];
			}
			
			pending_data -= len;
			
			if(!pending_data) {
				usb.write(0, nullptr, 0);
			}
		}
};

USB_FPGA usb_fpga(usb);

Pin fsmc_pins[] = {
	// D0-15
	GPIOD[14], GPIOD[15], GPIOD[ 0], GPIOD[ 1], GPIOE[ 7], GPIOE[ 8], GPIOE[ 9], GPIOE[10],
	GPIOE[11], GPIOE[12], GPIOE[13], GPIOE[14], GPIOE[15], GPIOD[ 8], GPIOD[ 9], GPIOD[10],
	// A0-10
	GPIOF[ 0], GPIOF[ 1], GPIOF[ 2], GPIOF[ 3], GPIOF[ 4], GPIOF[ 5], GPIOF[12], GPIOF[13],
	GPIOF[14], GPIOF[15], GPIOG[ 0],
	// NBL0-1
	GPIOE[ 0], GPIOE[ 1],
	// NE3
	GPIOG[10],
	// NOE, NWE, NWAIT
	GPIOD[ 4], GPIOD[ 5], GPIOD[ 6],
};

int main() {
	rcc_init();
	
	// Initialize system timer.
	STK.LOAD = 168000000 / 8 / 1000; // 1000 Hz.
	STK.CTRL = 0x03;
	
	RCC.enable(RCC.GPIOA);
	RCC.enable(RCC.GPIOC);
	RCC.enable(RCC.GPIOD);
	RCC.enable(RCC.GPIOE);
	RCC.enable(RCC.GPIOF);
	RCC.enable(RCC.GPIOG);
	RCC.enable(RCC.FSMC);
	
	// Configure FPGA control pins.
	fpga_prog_b.on();
	fpga_prog_b.set_type(Pin::OpenDrain);
	fpga_prog_b.set_mode(Pin::Output);
	
	fpga_csi_b.off();
	fpga_csi_b.set_mode(Pin::Output);
	
	// Configure MCO2 to output SYSCLK/4
	RCC.CFGR |= (0 << 30) | (6 << 27);
	
	mco2.set_speed(Pin::High);
	mco2.set_af(0);
	mco2.set_mode(Pin::AF);
	
	// Configure FSMC
	for(Pin& p : fsmc_pins) {
		p.set_speed(Pin::High);
		p.set_af(12);
		p.set_mode(Pin::AF);
	}
	
	// Initialize bank 2 for configuration.
	FSMC.reg.BCR2 = (1 << 12) | (1 << 7) | (1 << 0);
	
	// Initialize bank 3 for communication.
	FSMC.reg.BCR3 = (1 << 12) | (1 << 7) | (1 << 4) | (1 << 0);
	
	// Initialize USB.
	usb_vbus.set_mode(Pin::Input);
	usb_dm.set_mode(Pin::AF);
	usb_dm.set_af(10);
	usb_dp.set_mode(Pin::AF);
	usb_dp.set_af(10);
	
	RCC.enable(RCC.OTGFS);
	
	usb.init();
	
	while(1) {
		usb.process();
	}
}