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

volatile unsigned int cnt;

void i2c_write_reg(uint8_t addr, uint8_t reg, uint8_t data) {
	I2C2.CR1 |= 0x100;
	while(!(I2C2.SR1 & 0x01));
	
	I2C2.DR = (addr << 1) | 0;
	while (!(I2C2.SR1 & 0x02));
	I2C2.SR2;
	
	I2C2.DR = reg;
	while (!(I2C2.SR1 & 0x80));
	
	I2C2.DR = data;
	while (!(I2C2.SR1 & 0x04));
	
	I2C2.CR1 |= 0x200;
}

void i2c_read_reg(uint8_t addr, uint8_t reg, uint8_t len, uint8_t* buf) {
	I2C2.CR1 |= 0x100;
	while(!(I2C2.SR1 & 0x01));
	
	I2C2.DR = (addr << 1) | 0;
	while (!(I2C2.SR1 & 0x02));
	I2C2.SR2;
	
	I2C2.DR = reg;
	while (!(I2C2.SR1 & 0x80));
	
	I2C2.CR1 |= 0x100;
	while(!(I2C2.SR1 & 0x01));
	
	I2C2.DR = (addr << 1) | 1;
	while (!(I2C2.SR1 & 0x02));
	I2C2.SR2;
	
	I2C2.CR1 |= 0x400; // Set ACK.
	
	while(len) {
		if(len == 3) {
			while (!(I2C2.SR1 & 0x04)); // Wait for BTF
			
			I2C2.CR1 &= ~0x400; // Clear ACK.
			*buf++ = I2C2.DR;
			len--;
			
			I2C2.CR1 |= 0x200; // Set STOP.
			*buf++ = I2C2.DR;
			len--;
			
		} else {
			while (!(I2C2.SR1 & 0x40)); // Wait for RXNE
			
			*buf++ = I2C2.DR;
			len--;
		}
	}
}

template<>
void interrupt<Interrupt::USART1>() {
	USART1.DR;
	GPIOA.ODR ^= 1 << 5;
}

void usart_send(uint8_t data) {
	while(!(USART1.SR & 0x80)); // Wait for TXE.
	
	USART1.DR = data;
}

void xbee_send(int len, const uint8_t* buf) {
	// Start and length.
	usart_send(0x7e);
	usart_send(((len + 14) >> 8) & 0xff);
	usart_send((len + 14) & 0xff);
	
	// Frame type and ID.
	usart_send(0x10);
	usart_send(0x01);
	
	// Destination address.
	usart_send(0x00);
	usart_send(0x13);
	usart_send(0xa2);
	usart_send(0x00);
	usart_send(0x40);
	usart_send(0x6f);
	usart_send(0x19);
	usart_send(0xf1);
	
	usart_send(0x00);
	usart_send(0x00);
	usart_send(0x00);
	usart_send(0x00);
	
	uint8_t chsum = 0x80;
	
	// Payload
	for(int i = 0; i < len; i++) {
		usart_send(buf[i]);
		chsum -= buf[i];
	}
	
	usart_send(chsum);
}

class PPMSum {
	friend void interrupt<Interrupt::TIM4>();
	
	private:
		static PPMSum* self;
		
		uint8_t index;
		uint16_t channels[16];
		
		void irq() {
			int16_t sr = TIM4.SR;
			TIM4.SR = 0;
			
			if(sr & 0x06) {
				GPIOA.ODR = 1 << 5;
			} else {
				GPIOA.ODR = 0;
			}
			
			
			if(sr & 0x01) {
				// Timeout.
				
				// TODO: Indicate failsafe.
				
			} else if(sr & 0x02) {
				// Period.
				
				if(TIM4.CCR1 > 5000) {
					index = 0;
				} else {
					index++;
				}
				
			} else if(sr & 0x04) {
				// Pulsewidth.
				
				channels[index] = TIM4.CCR2;
			}
		}
	
	public:
		PPMSum() {
			self = this;
		}
		
		void start() {
			RCC.enable(RCC.TIM4);
			TIM4.PSC = 36;
			TIM4.CCMR1 = 0x0201;
			TIM4.SMCR = 0x54;
			TIM4.CCER = 0x31;
			TIM4.DIER = 0x07;
			
			Interrupt::enable(Interrupt::TIM4);
			
			TIM4.CR1 = 0x05;
		}
};

PPMSum* PPMSum::self = 0;

template<>
void interrupt<Interrupt::TIM4>() {
	PPMSum::self->irq();
}

int main() {
	RCC.enable(RCC.AFIO);
	RCC.enable(RCC.IOPA);
	RCC.enable(RCC.IOPB);
	
	GPIOA.CRL = 0x44344444;
	GPIOA.CRH = 0x444444b4;
	GPIOA.ODR = 1 << 5;
	
	GPIOB.CRH = 0x4444ff44;
	
	cnt = 0;
	while(cnt++ < (1 << 20));
	
	// 100 kHz.
	RCC.enable(RCC.I2C2);
	while(cnt++ < (1 << 20));
	
	I2C2.CR1 = 0x8000;
	I2C2.CR1 = 0;
	
	I2C2.CR2 = 36;
	I2C2.TRISE = 37;
	I2C2.CCR = 180;
	I2C2.CR1 = 1;
	
	i2c_write_reg(0x68, 0x3e, 0x03);
	i2c_write_reg(0x68, 0x16, 0x18 | 0x02);
	
	uint8_t buf[6];
	
	RCC.enable(RCC.USART1);
	USART1.BRR = 7500; // 9600 baud
	USART1.CR1 = 0x202c;
	
	Interrupt::enable(Interrupt::USART1);
	
	PPMSum ppmsum;
	ppmsum.start();
	
	while(1) {
		cnt++;
		if(cnt & (1 << 20)) {
			//GPIOA.ODR = 1 << 5;
		} else {
			//GPIOA.ODR = 0;
		}
		
		if(!(cnt & ((1 << 20) - 1))) {
			i2c_read_reg(0x68, 0x1d, 6, buf);
			//xbee_send(6, buf);
		}
	}
}