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

#include <ch.h>
#include <hal.h>
#include <pwm.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;
		
		int8_t thrust, pitch, yaw, roll;
		
		noreturn_t thread_main() {
			thrust = pitch = roll = yaw = 0;
			
			w_s_t w_s = W_S;
			uint8_t w_n = 0;
			
			while (TRUE) {
				palSetPad(GPIOA, 5);
				
				size_t buffer = usbs->getc();
				if(buffer >= 0 && buffer < 256) {
					usbs->putc(buffer);
					
					if(w_s == W_S && buffer == 'S') {
						w_s = W_N;
					} else if(w_s == W_N && buffer >= '1' && buffer <= '4') {
						w_s = W_V;
						w_n = buffer - '1';
					} else if(w_s == W_V) {
						w_s = W_S;
						switch(w_n) {
							case 0:
								thrust = buffer;
								break;
							case 1:
								pitch = buffer - 64;
								break;
							case 2:
								roll = buffer - 64;
								break;
							case 3:
								yaw = buffer - 64;
								break;
						}
					} else {
						w_s = W_S;
					}
				}
			}
		}
};

USBThread usb_thread;

void foo(PWMDriver*) {
	pwmEnableChannel(&PWMD2, 0, 1000 + usb_thread.thrust * 10 + usb_thread.pitch * 3 + usb_thread.roll * 3 - usb_thread.yaw * 3);
	pwmEnableChannel(&PWMD2, 1, 1000 + usb_thread.thrust * 10 + usb_thread.pitch * 3 - usb_thread.roll * 3 + usb_thread.yaw * 3);
	pwmEnableChannel(&PWMD2, 2, 1000 + usb_thread.thrust * 10 - usb_thread.pitch * 3 + usb_thread.roll * 3 + usb_thread.yaw * 3);
	pwmEnableChannel(&PWMD2, 3, 1000 + usb_thread.thrust * 10 - usb_thread.pitch * 3 - usb_thread.roll * 3 - usb_thread.yaw * 3);
}

static PWMConfig pwmcfg = {
  foo,
  {
   {PWM_OUTPUT_ACTIVE_HIGH, NULL},
   {PWM_OUTPUT_ACTIVE_HIGH, NULL},
   {PWM_OUTPUT_ACTIVE_HIGH, NULL},
   {PWM_OUTPUT_ACTIVE_HIGH, NULL}
  },

  PWM_COMPUTE_PSC(STM32_TIMCLK1, 1000000),
  20000,
  0
};

USBSerial usbs;

int main(void) {
	halInit();
	chSysInit();
	
	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);
	
	led_thread.start();
	
	usbs.init();
	usb_thread.usbs = &usbs;
	usb_thread.start();
	
	while (1) {
		chThdSleepMilliseconds(1000);
	}
}