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

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

#include "itg3200.h"
#include "bma150.h"

#include "usart.h"
#include "xbee.h"
#include "gps.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;
ITG3200 gyro(i2c);
BMA150 accel(i2c);

volatile uint16_t dmabuf[2];
void threadmain() {
	ADC1.CR2 = 0x9;
	while(ADC1.CR2 & 0x8);
	ADC1.CR2 = 0x5;
	while(ADC1.CR2 & 0x4);
	
	DMA1.CH[0].CMAR = (uint32_t)&dmabuf;
	DMA1.CH[0].CPAR = (uint32_t)&ADC1.DR;
	DMA1.CH[0].CNDTR = 2;
	DMA1.CH[0].CCR = 0x05a1;
	
	ADC1.SMPR2 = 0x003f000;
	ADC1.SQR1 = 0x100000;
	ADC1.SQR3 = 5 | (4 << 5);
	ADC1.CR1 = 0x100;
	ADC1.CR2 = 0x103;
	ADC1.CR2 = 0x103;
	
	while(1) {
		uint16_t buf[] = {
			gyro.x,
			gyro.y,
			gyro.z,
			accel.x,
			accel.y,
			accel.z,
			dmabuf[0],
			dmabuf[1],
		};
		
		xbee_send(1, sizeof(buf), (uint8_t*)buf);
		Time::sleep(100);
	}
}

uint32_t thstack[1024];

Thread thread(thstack, sizeof(thstack), threadmain);

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

int main() {
	// Initialize system timer.
	Time::init();
	
	RCC.enable(RCC.AFIO);
	RCC.enable(RCC.IOPA);
	RCC.enable(RCC.IOPB);
	
	GPIOA.CRL = 0x4400bbbb;
	GPIOA.CRH = 0x444444b4;
	
	GPIOB.CRL = 0xff444434;
	GPIOB.CRH = 0x44444444;
	GPIOB.ODR = 1 << 1;
	
	RCC.enable(RCC.DMA1);
	RCC.enable(RCC.ADC1);
	
	// Give all hardware enough time to initialize.
	Time::sleep(100);
	
	//I2C i2c;
	i2c.enable();
	
	//ITG3200 gyro(i2c);
	gyro.init();
	accel.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();
	
	thread.start();
	//gps_thread.start();
	
	while(1) {
		// Wait for a new update.
		while(!(TIM2.SR & 0x01)) {
			Thread::yield();
		}
		TIM2.SR = 0;
		
		// Read sensors.
		gyro.update();
		accel.update();
		
		// Update filter.
		
		// Generate motor mix.
		int16_t throttle = ppmsum.channels[2] - 1000;
		int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 1 - gyro.x);
		int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 1 - gyro.y);
		int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * -1 - gyro.z);
		
		int16_t max = throttle > 250 ? 250 : throttle;
		saturate(pitch, max);
		saturate(roll, max);
		saturate(yaw, max);
		
		int cmds[] = {
			1000 + throttle + pitch - roll + yaw,
			1000 + throttle - pitch - roll - yaw,
			1000 + throttle - pitch + roll + yaw,
			1000 + throttle + pitch + roll - yaw,
		};
		
		TIM2.CCR1 = limit(cmds[0], 1000, 2000);
		TIM2.CCR2 = limit(cmds[1], 1000, 2000);
		TIM2.CCR3 = limit(cmds[2], 1000, 2000);
		TIM2.CCR4 = limit(cmds[3], 1000, 2000);
	}
}