#include "thread.h"
#include "usbserial.h"
#include "itg3200.h"
#include "bma150.h"
#include "ak8975.h"

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

#include "IMU.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;

#include "foo.h"
#include <cmath>

uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55};
uint8_t buf[64];
int16_t* sensordata = (int16_t*)buf;

class I2CThread : public BaseThread<I2CThread, 256> {
	public:
		ITG3200 gyro;
		BMA150 acc;
		AK8975 magn;
		
		int16_t x, y, z;
		
		noreturn_t thread_main() {
			I2CSensor::enable_bus();
			
			gyro.init();
			acc.init();
			magn.init();
			
			systime_t nexttime = chTimeNow();
			while (1) {
				nexttime += MS2ST(100);
				gyro.update();
				acc.update();
				magn.update();
				x = gyro.x;
				y = gyro.y;
				z = gyro.z;
				
				IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z);
				
				//float pitch = asinf(2*(q0*q2 - q3*q1));
				int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
				int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
				int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767;
				
				sensordata[0] = gyro.x;
				sensordata[1] = gyro.y;
				sensordata[2] = gyro.z;
				sensordata[3] = acc.x;
				sensordata[4] = acc.y;
				sensordata[5] = acc.z;
				sensordata[6] = magn.x;
				sensordata[7] = magn.y;
				sensordata[8] = magn.z;
				sensordata[9] = pitch;
				sensordata[10] = roll;
				sensordata[11] = yaw;
				
				
				
				usbs.write(syncword, sizeof(syncword));
				usbs.write(buf, sizeof(buf));
				
				/*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n",
					gyro.x, gyro.y, gyro.z,
					acc.x, acc.y, acc.z,
					magn.x, magn.y, magn.z,
					int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),
					int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/
				
				//chThdSleepUntil(nexttime);
			}
		}
};

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);
	}
}