#include "stm32.h"
#include "interrupt.h"

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

#include "itg3200.h"

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

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

int main() {
	RCC.enable(RCC.AFIO);
	RCC.enable(RCC.IOPA);
	RCC.enable(RCC.IOPB);
	
	GPIOA.CRL = 0x4434bbbb;
	GPIOA.CRH = 0x444444b4;
	GPIOA.ODR = 1 << 5;
	
	GPIOB.CRH = 0x4444ff44;
	
	I2C i2c;
	i2c.enable();
	
	ITG3200 gyro(i2c);
	gyro.init();
	
	PPMSum ppmsum;
	ppmsum.enable();
	
	RCC.enable(RCC.TIM2);
	TIM2.PSC = 72;
	TIM2.ARR = 20000;
	TIM2.CCER = 0x1111;
	TIM2.CCMR1 = 0x6868;
	TIM2.CCMR2 = 0x6868;
	
	TIM2.CR1 = 0x05;
	
	PID pid_pitch(2000, 0, 0);
	PID pid_roll(2000, 0, 0);
	PID pid_yaw(2000, 0, 0);
	
	while(1) {
		// Wait for a new update.
		while(!(TIM2.SR & 0x01));
		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) - gyro.x);
		int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) - gyro.y);
		int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) - gyro.z);
		
		int16_t max = throttle > 250 ? 250 : throttle;
		saturate(pitch, max);
		saturate(roll, max);
		saturate(yaw, max);
		
		TIM2.CCR1 = 1000 + throttle + pitch + roll + yaw;
		TIM2.CCR2 = 1000 + throttle + pitch - roll - yaw;
		TIM2.CCR3 = 1000 + throttle - pitch + roll - yaw;
		TIM2.CCR4 = 1000 + throttle - pitch - roll + yaw;
	}
}