#include "thread.h"
#include "usbserial.h"

#include <ch.h>
#include <hal.h>
#include <pwm.h>
#include <i2c.h>

class LEDThread : public BaseThread<LEDThread, 128> {
	public:
		noreturn_t thread_main() {
			while (1) {
				palClearPad(GPIOA, 5);
				chThdSleepMilliseconds(500);
				palSetPad(GPIOA, 5);
				chThdSleepMilliseconds(500);
			}
		}
};

LEDThread led_thread;

class USBThread : public BaseThread<USBThread, 256> {
	private:
		typedef enum {W_S, W_N, W_V} w_s_t;
	
	public:
		USBSerial* usbs;
		
		uint8_t data[9];
		
		noreturn_t thread_main() {
			for(int i = 0; i < 9; i++) {
				data[i] = 0;
			}
			
			w_s_t w_s = W_S;
			uint8_t w_n = 0;
			
			while(1) {
				size_t buffer = usbs->getc();
				if(buffer >= 0 && buffer < 256) {
					if(w_s == W_S && buffer == 'S') {
						w_s = W_N;
					} else if(w_s == W_N && buffer >= '1' && buffer <= '9') {
						w_s = W_V;
						w_n = buffer - '1';
					} else if(w_s == W_V) {
						w_s = W_S;
						data[w_n] = buffer;
					} else {
						w_s = W_S;
					}
				}
			}
		}
};

USBThread usb_thread;
USBSerial usbs;

I2CConfig i2cconfig = {
	opmodeI2C,
	100000,       /*!< Specifies the clock frequency. Must be set to a value lower than 400kHz */
	stdDutyCycle, /*!< Specifies the I2C fast mode duty cycle */
	0,      /*!< Specifies the first device 7-bit own address. */
	0     /*!< Specifies the second part of device own address in 10-bit mode. Set to NULL if not used. */
};

void cba(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) {
	if(i2cp->id_slave_config->restart) {
		i2cp->id_slave_config->restart = FALSE;
		i2cMasterReceive(i2cp, i2cscfg);
		//palSetPad(GPIOA, 5);
	} else {
		i2cMasterStop(i2cp);
	}
}

void cbe(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) {
	//palClearPad(GPIOA, 5);
	//while(1);
}

i2cblock_t rxdata[6];
i2cblock_t txdata[2] = {0x1d};

I2CSlaveConfig i2c_gyro = {
	cba,
	
	cbe,
	
	rxdata,
	6,
	6,
	0,
	
	txdata,
	2,
	1,
	0,
	
	0x68,
	FALSE
};

uint8_t strbuf[64];

#include "foo.h"

class I2CThread : public BaseThread<I2CThread, 256> {
	public:
		int16_t x, y, z;
		
		noreturn_t thread_main() {
			i2cStart(&I2CD2, &i2cconfig);
			while(I2CD2.id_state != I2C_READY) {
				chThdSleepMilliseconds(1);
			}
			
			palSetPadMode(GPIOB, 10, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
			palSetPadMode(GPIOB, 11, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
			
			i2c_gyro.txbytes = 2;
			i2c_gyro.txbuf[0] = 0x3e;
			i2c_gyro.txbuf[1] = 0x03;
			i2cMasterTransmit(&I2CD2, &i2c_gyro);
			chThdSleepMilliseconds(2);
			
			i2c_gyro.txbytes = 2;
			i2c_gyro.txbuf[0] = 0x16;
			i2c_gyro.txbuf[1] = 0x18;
			i2cMasterTransmit(&I2CD2, &i2c_gyro);
			chThdSleepMilliseconds(2);
			
			while (1) {
				I2CD2.id_state = I2C_READY;
				i2c_gyro.txbufhead = 0;
				i2c_gyro.txbuf[0] = 0x1d;
				i2c_gyro.txbytes = 1;
				i2c_gyro.rxbufhead = 0;
				i2c_gyro.rxbytes = 6;
				i2c_gyro.restart = TRUE;
				
				i2cMasterTransmit(&I2CD2, &i2c_gyro);
				chThdSleepMilliseconds(2);
				
				x = rxdata[0] << 8 | rxdata[1];
				y = rxdata[2] << 8 | rxdata[3];
				z = rxdata[4] << 8 | rxdata[5];
				
				usbprintf(usbs, "%6d, %6d, %6d\r\n", x, y, z);
				
				chThdSleepMilliseconds(100);
			}
		}
};

I2CThread i2c_thread;

void foo(PWMDriver*) {
	
	int16_t thrust, pitch, roll, yaw;
	
	pitch = roll = 0;
	
	uint8_t shift = 8 - (usb_thread.data[8] >> 3);
	
	thrust = usb_thread.data[0] * 10;
	
	yaw = i2c_thread.z >> shift;
	
	
	pwmEnableChannel(&PWMD2, 0, 1000 + thrust + pitch + roll - yaw);
	pwmEnableChannel(&PWMD2, 1, 1000 + thrust + pitch - roll + yaw);
	pwmEnableChannel(&PWMD2, 2, 1000 + thrust - pitch + roll + yaw);
	pwmEnableChannel(&PWMD2, 3, 1000 + thrust - pitch - roll - yaw);
}

static PWMConfig pwmcfg = {
	1000000,
	1000000 / 50,
	foo,
	{
		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
		{PWM_OUTPUT_ACTIVE_HIGH, NULL}
	},
	0
};

int main(void) {
	halInit();
	chSysInit();
	
	led_thread.start();
	
	usbs.init();
	
	i2c_thread.start();
	
	pwmStart(&PWMD2, &pwmcfg);
	palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	palSetPadMode(GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	palSetPadMode(GPIOA, 3, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	
	pwmEnableChannel(&PWMD2, 0, 1000);
	pwmEnableChannel(&PWMD2, 1, 1000);
	pwmEnableChannel(&PWMD2, 2, 1000);
	pwmEnableChannel(&PWMD2, 3, 1000);
	
	usb_thread.usbs = &usbs;
	usb_thread.start();
	
	while (1) {
		chThdSleepMilliseconds(1000);
	}
}