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

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

#include "itg3200.h"

#include "usart.h"
#include "xbee.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;
		}
};

void threadmain() {
	while(1) {
		GPIOB.ODR ^= 1 << 1;
		
		xbee_send(3, (uint8_t*)"hei");
	}
}

uint32_t thstack[1024];

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

int main() {
	// Initialize system timer.
	Time::init();
	
	RCC.enable(RCC.AFIO);
	RCC.enable(RCC.IOPA);
	RCC.enable(RCC.IOPB);
	
	GPIOA.CRL = 0x4444bbbb;
	GPIOA.CRH = 0x444444b4;
	
	GPIOB.CRL = 0xff444434;
	GPIOB.CRH = 0x44444444;
	GPIOB.ODR = 1 << 1;
	
	I2C i2c;
	i2c.enable();
	
	ITG3200 gyro(i2c);
	gyro.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();
	
	thread.start();
	
	while(1) {
		// Wait for a new update.
		while(!(TIM2.SR & 0x01)) {
			Thread::yield();
		}
		TIM2.SR = 0;
		
		// Read sensors.
		gyro.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);
	}
}