#include "stm32.h"
#include "interrupt.h"
#include "thread.h"
#include "time.h"

#include "ppmsum.h"
#include "i2c.h"

#include "ahrs.h"
#include "telemetry.h"

#include "usart.h"
#include "xbee.h"
#include "gps.h"

#include "pin.h"

template<class T>
inline void saturate(T& var, T absmax) {
	if(var > absmax) {
		var = absmax;
	} else if(var < -absmax) {
		var = -absmax;
	}
}

template<class T>
inline T limit(T var, T min, T max) {
	if(var < min) {
		return min;
	} else if(var > max) {
		return max;
	} else {
		return var;
	}
}

class PID {
	private:
		uint16_t Kp, Ki, Kd;
		
		int16_t last;
		int32_t accum;
	
	public:
		PID(uint16_t p, uint16_t i, uint16_t d) : Kp(p), Ki(i), Kd(d), last(0), accum(0) {}
		
		int16_t update(int16_t error) {
			// P
			int32_t corr_p = Kp * error;
			
			// I
			accum += Ki * error;
			int32_t corr_i = accum;
			
			// D
			int32_t corr_d = Kd * (error - last);
			last = error;
			
			return (corr_p + corr_i + corr_d) >> 16;
		}
};

I2C i2c;
AHRS ahrs(i2c);

volatile uint16_t motors[4];

GPS gps;

void gps_thread_main() {
	while(1) {
		P<GPSMsg> msg = gps.read();
		
		if(msg->n < 128) {
			xbee_send(2, msg->n, msg->buf);
		}
	}
}

uint32_t gps_stack[256];

Thread gps_thread(gps_stack, sizeof(gps_stack), gps_thread_main);

static Pin& led_green = PD12;
static Pin& led_yellow = PD13;
static Pin& led_red = PD14;
static Pin& led_blue = PD15;

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.GPIOD);
	
	led_green.set_mode(Pin::Output);
	led_yellow.set_mode(Pin::Output);
	led_red.set_mode(Pin::Output);
	led_blue.set_mode(Pin::Output);
	
	
	RCC.enable(RCC.DMA1);
	RCC.enable(RCC.ADC1);
	PB6.set_af(7);
	PB7.set_af(7);
	PB6.set_mode(Pin::Output);
	PB7.set_mode(Pin::Output);
	
	// Give all hardware enough time to initialize.
	Time::sleep(200);
	
	i2c.enable();
	ahrs.init();
	
	PPMSum ppmsum;
	ppmsum.enable();
	
	RCC.enable(RCC.TIM2);
	TIM2.PSC = 72;
	TIM2.ARR = 5000;
	TIM2.CCER = 0x1111;
	TIM2.CCMR1 = 0x6868;
	TIM2.CCMR2 = 0x6868;
	
	TIM2.CR1 = 0x05;
	
	PID pid_pitch(6000, 0, 0);
	PID pid_roll(6000, 0, 0);
	PID pid_yaw(6000, 0, 0);
	
	usart_enable();
	gps.enable();
	
	telemetry_thread.start();
	//gps_thread.start();
	
	while(1) {
		// Wait for a new update.
		while(!(TIM2.SR & 0x01)) {
			Thread::yield();
		}
		TIM2.SR = 0;
		
		// Update AHRS.
		ahrs.update();
		
		// Update filter.
		int16_t throttle = ppmsum.channels[2] - 1000;
		int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 1 - ahrs.gyro.x);
		int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 1 - ahrs.gyro.y);
		int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * -1 - ahrs.gyro.z);
		
		int16_t max = throttle > 250 ? 250 : throttle;
		saturate(pitch, max);
		saturate(roll, max);
		saturate(yaw, max);
		
		// Generate motor mix.
		motors[0] = limit(throttle + pitch - roll + yaw, 0, 1000);
		motors[1] = limit(throttle - pitch - roll - yaw, 0, 1000);
		motors[2] = limit(throttle - pitch + roll + yaw, 0, 1000);
		motors[3] = limit(throttle + pitch + roll - yaw, 0, 1000);
		
		TIM2.CCR1 = 1000 + motors[0];
		TIM2.CCR2 = 1000 + motors[1];
		TIM2.CCR3 = 1000 + motors[2];
		TIM2.CCR4 = 1000 + motors[3];
	}
}