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

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

#include "itg3200.h"

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;
	
	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 = 0;
		int16_t yaw = gyro.z >> 7;
		int16_t roll = 0;
		
		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;
	}
}