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

#include <gpio/pin.h>

#include <usb/usb.h>
#include <i2c/i2c.h>

#include "lsm303dlm.h"
#include "l3gd20.h"

#include "ppmsum.h"

static Pin& led_status = PA4;
static Pin& led_error = PC4;

static Pin& usb_vbus = PB13;
static Pin& usb_dm = PB14;
static Pin& usb_dp = PB15;

static Pin& jtag_tdo = PC3;
static Pin& jtag_tms = PC13;
static Pin& jtag_tck = PC14;
static Pin& jtag_tdi = PC15;

static Pin& i2c_scl = PB10;
static Pin& i2c_sda = PB11;

static Pin& cs_pressure = PB4;
static Pin& cs_gyro = PB5;

USB_otg usb(OTG_HS);

bool i2c_read(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
	uint8_t buf[wLength];
	I2C2.read_reg(wValue, wIndex, wLength, buf);
	
	usb.write(0, (uint32_t*)buf, wLength);
	return true;
}

bool jtag_tick(bool tdi, bool tms) {
	bool tdo = jtag_tdo.get();
	jtag_tdi.set(tdi);
	jtag_tms.set(tms);
	jtag_tck.on();
	for(uint32_t i = 0; i < 1000; i++) {
		asm volatile("nop");
	}
	jtag_tck.off();
	for(uint32_t i = 0; i < 1000; i++) {
		asm volatile("nop");
	}
	return tdo;
}

bool jtag_shift(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
	if(wLength > 16) {
		return false;
	}
	
	uint32_t tdo = 0;
	
	for(int16_t i = 0; i < wLength; i++) {
		tdo |= jtag_tick(wValue & 1, wIndex & 1) ? 1 << i : 0;
		wValue >>= 1;
		wIndex >>= 1;
	}
	
	usb.write(0, &tdo, (wLength + 7) >> 3);
	return true;
}

template<>
void interrupt<(Interrupt::IRQ)77>() {
	usb.process();
}

void usb_main() {
	usb_vbus.set_mode(Pin::Input);
	usb_dm.set_mode(Pin::AF);
	usb_dm.set_pull(Pin::PullNone);
	usb_dm.set_af(12);
	usb_dp.set_mode(Pin::AF);
	usb_dp.set_pull(Pin::PullNone);
	usb_dp.set_af(12);
	
	RCC.enable(RCC.OTGHS);
	//Interrupt::enable((Interrupt::IRQ)77);
	
	usb.init();
	
	while(1) {
		usb.process();
		Thread::yield();
	}
}

uint32_t usb_stack[1024];

Thread usb_thread(usb_stack, sizeof(usb_stack), usb_main);

L3GD20 gyro(cs_gyro, SPI1);
LSM303DLM_A accel(I2C2);
LSM303DLM_M magn(I2C2);

PPMSum ppmsum;

int main() {
	// Initialize system timer.
	STK.LOAD = 168000000 / 8 / 1000; // 1000 Hz.
	STK.CTRL = 0x03;
	
	RCC.enable(RCC.GPIOA);
	RCC.enable(RCC.GPIOB);
	RCC.enable(RCC.GPIOC);
	RCC.enable(RCC.GPIOD);
	
	led_status.set_mode(Pin::Output);
	led_status.off();
	led_error.set_mode(Pin::Output);
	led_error.off();
	
	jtag_tdi.set_mode(Pin::Output);
	jtag_tms.set_mode(Pin::Output);
	jtag_tck.set_mode(Pin::Output);
	jtag_tdo.set_mode(Pin::Input);
	
	RCC.enable(RCC.I2C2);
	I2C2.enable(i2c_scl, i2c_sda);
	
	usb_thread.start();
	
	RCC.enable(RCC.SPI1);
	
	PA5.set_mode(Pin::AF);
	PA5.set_af(5);
	PA6.set_mode(Pin::AF);
	PA6.set_af(5);
	PA7.set_mode(Pin::AF);
	PA7.set_af(5);
	
	cs_gyro.on();
	cs_gyro.set_mode(Pin::Output);
	cs_pressure.on();
	cs_pressure.set_mode(Pin::Output);
	
	SPI1.reg.CR1 = 0x37f;
	
	Time::sleep(1000);
	
	gyro.init();
	accel.init();
	magn.init();
	
	PA0.set_mode(Pin::AF);
	PA0.set_af(1);
	ppmsum.enable();
	
	while(1) {
		led_error.toggle();
		Time::sleep(10);
		
		gyro.update();
		accel.update();
		magn.update();
		
		int16_t buf[] = {
			gyro.x,
			gyro.y,
			gyro.z,
			accel.x,
			accel.y,
			accel.z,
			magn.x,
			magn.y,
			magn.z,
		};
		
		if(usb.ep_ready(1)) {
			usb.write(1, (uint32_t*)buf, sizeof(buf));
		}
	}
}