#include <rcc/rcc.h>
#include <os/time.h>
#include <interrupt/interrupt.h>
#include <timer/timer.h>

#include <gpio/gpio.h>
#include <usb/usb.h>
#include <usb/descriptor.h>

#include "hmc5983.h"
#include "mpu6000.h"

Pin usb_vbus = GPIOB[5];
Pin usb_dm = GPIOA[11];
Pin usb_dp = GPIOA[12];

Pin spi2_sck = GPIOB[13];
Pin spi2_miso = GPIOB[14];
Pin spi2_mosi = GPIOB[15];
Pin bar_cs = GPIOC[13];
Pin mag_cs = GPIOC[14];
Pin mpu_cs = GPIOC[15];
Pin flash_cs = GPIOB[2];

Pin pwm1 = GPIOA[0];
Pin pwm2 = GPIOA[1];
Pin pwm3 = GPIOA[2];
Pin pwm4 = GPIOA[3];

Pin s1 = GPIOA[0];
Pin s2 = GPIOA[1];

auto dev_desc = device_desc(0x200, 0, 0, 0, 64, 0x1234, 0x5678, 0, 0, 0, 0, 1);
auto conf_desc = configuration_desc(1, 1, 0, 0xc0, 0,
	interface_desc(1, 0, 1, 0xff, 0x00, 0x00, 0,
		endpoint_desc(0x81, 0x02, 64, 0) // IN
	)
);

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

USB_f1 usb(USB, dev_desc_p, conf_desc_p);

class USB_TM : public USB_class_driver {
	private:
		USB_generic& usb;
		uint32_t buf[16];
		
	public:
		USB_TM(USB_generic& usbd) : usb(usbd) {
			usb.register_driver(this);
		}
		
	protected:
		virtual void handle_set_configuration(uint8_t configuration) {
			if (configuration) {
				usb.hw_conf_ep(0x81, EPType::Bulk, 64);
				usb.register_out_handler(this, 1);
			}
		}
};

USB_TM usb_tm(usb);

HMC5983 magn(mag_cs, SPI2);
MPU6000 mpu(mpu_cs, SPI2);

int main() {
	rcc_init();
	
	// Initialize system timer.
	STK.LOAD = 72000000 / 8 / 1000; // 1000 Hz
	STK.CTRL = 0x03;
	
	RCC.enable(RCC.GPIOA);
	RCC.enable(RCC.GPIOB);
	RCC.enable(RCC.GPIOC);
	
	usb_dm.set_mode(Pin::AF);
	usb_dm.set_af(14);
	usb_dp.set_mode(Pin::AF);
	usb_dp.set_af(14);
	
	RCC.enable(RCC.USB);
	usb.init();
	
	RCC.enable(RCC.SPI2);
	
	spi2_sck.set_mode(Pin::AF);
	spi2_sck.set_af(5);
	spi2_mosi.set_mode(Pin::AF);
	spi2_mosi.set_af(5);
	spi2_miso.set_mode(Pin::AF);
	spi2_miso.set_af(5);
	mag_cs.set_mode(Pin::Output);
	mag_cs.on();
	bar_cs.set_mode(Pin::Output);
	bar_cs.on();
	mpu_cs.set_mode(Pin::Output);
	mpu_cs.on();
	flash_cs.set_mode(Pin::Output);
	flash_cs.on();
	
	pwm1.set_mode(Pin::AF);
	pwm1.set_af(1);
	pwm2.set_mode(Pin::AF);
	pwm2.set_af(1);
	pwm3.set_mode(Pin::AF);
	pwm3.set_af(1);
	pwm4.set_mode(Pin::AF);
	pwm4.set_af(1);
	
	RCC.enable(RCC.TIM2);
	
	TIM2.PSC = 71;
	TIM2.ARR = 2500;
	TIM2.CCER = (1 << 12) | (1 << 8) | (1 << 4) | (1 << 0);
	TIM2.CCMR1 = (6 << 12) | (1 << 11) | (6 << 4) | (1 << 3);
	TIM2.CCMR2 = (6 << 12) | (1 << 11) | (6 << 4) | (1 << 3);
	TIM2.CCR1 = 1000;
	TIM2.CCR2 = 1000;
	TIM2.CCR3 = 1000;
	TIM2.CCR4 = 1000;
	TIM2.CR1 = (1 << 0);
	
	magn.init();
	mpu.init();
	float buf[9] = {0,0,0,0,0,0,0,0,0};
	
	
	while(1) {
		usb.process();
		uint8_t magn_status = magn.update();
		mpu.update();
		
		buf[0] = magn.x;
		buf[1] = magn.y;
		buf[2] = magn.z;
		buf[3] = mpu.gyro[0];
		buf[4] = mpu.gyro[1];
		buf[5] = mpu.gyro[2];
		buf[6] = mpu.accel[0];
		buf[7] = mpu.accel[1];
		buf[8] = mpu.accel[2];
		if(usb.ep_ready(1)) {
			usb.write(1, (uint32_t*)buf, sizeof(buf));
			//usb.write(1, (uint32_t*)mpu.gyro, sizeof(mpu.gyro));
			//usb.write(1, (uint32_t*)&magn_status, sizeof(magn_status));
		}
	}
}