#include "stm32.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--;
		}
	}
}

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

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 = 0x2008;
	
	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);
		}
	}
}

/*
#include "thread.h"
#include "usbserial.h"
#include "itg3200.h"
#include "bma150.h"
#include "ak8975.h"
#include "ppmsum.h"
#include "motormixer.h"

#include <ch.h>
#include <hal.h>

#include "IMU.h"

class LEDThread : public BaseThread<LEDThread, 128> {
	public:
		noreturn_t thread_main() {
			systime_t time = chTimeNow();     // T0
			while (TRUE) {
				time += MS2ST(1000);            // Next deadline
				palClearPad(GPIOA, 5);
				chThdSleepUntil(time);
				time += MS2ST(1000);            // Next deadline
				palSetPad(GPIOA, 5);
				chThdSleepUntil(time);
			}
		}
};

LEDThread led_thread;

PPMSum ppmsum;
MotorMixer motors;

class USBThread : public BaseThread<USBThread, 256> {
	private:
		typedef enum {W_S, W_N, W_V} w_s_t;
	
	public:
		USBSerial* usbs;
		
		uint8_t data[9];
		
		noreturn_t thread_main() {
			for(int i = 0; i < 9; i++) {
				data[i] = 0;
			}
			
			w_s_t w_s = W_S;
			uint8_t w_n = 0;
			
			while(1) {
				size_t buffer = usbs->getc();
				if(buffer >= 0 && buffer < 256) {
					if(w_s == W_S && buffer == 'S') {
						w_s = W_N;
					} else if(w_s == W_N && buffer >= '1' && buffer <= '9') {
						w_s = W_V;
						w_n = buffer - '1';
					} else if(w_s == W_V) {
						w_s = W_S;
						data[w_n] = buffer;
					} else {
						w_s = W_S;
					}
				}
			}
		}
};

USBThread usb_thread;
USBSerial usbs;

#include "foo.h"
#include <cmath>

uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55};
uint8_t buf[64];
int16_t* sensordata = (int16_t*)buf;

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

class I2CThread : public BaseThread<I2CThread, 256> {
	public:
		ITG3200 gyro;
		BMA150 acc;
		AK8975 magn;
		
		int16_t x, y, z;
		
		noreturn_t thread_main() {
			I2CSensor::enable_bus();
			
			gyro.init();
			acc.init();
			magn.init();
			
			systime_t time = chTimeNow();
			
			int32_t pitch_angle_accum = 0;
			int32_t roll_angle_accum = 0;
			
			while (1) {
				gyro.update();
				acc.update();
				magn.update();
				x = gyro.x;
				y = gyro.y;
				z = gyro.z;
				
				IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z);
				
				//float pitch = asinf(2*(q0*q2 - q3*q1));
				//int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
				//int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
				//int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767;
				
				float norm_x = 2*(q0*q2 - q1*q3);
				float norm_y = 2*(q0*q1 + q2*q3);
				float norm_z = (1 - 2*(q1*q1 + q2*q2));
				
				float elev = acosf(norm_z);
				float azim = atan2f(norm_y, norm_x);
				
				int16_t pitch = elev * sinf(azim) / M_PI * 32767;
				int16_t roll = elev * cosf(azim) / M_PI * 32767;
				int16_t yaw = 0;
				
				sensordata[0] = gyro.x;
				sensordata[1] = gyro.y;
				sensordata[2] = gyro.z;
				sensordata[3] = acc.x;
				sensordata[4] = acc.y;
				sensordata[5] = acc.z;
				sensordata[6] = magn.x;
				sensordata[7] = magn.y;
				sensordata[8] = magn.z;
				sensordata[9] = pitch;
				sensordata[10] = roll;
				sensordata[11] = yaw;
				
				usbs.write(syncword, sizeof(syncword));
				usbs.write(buf, sizeof(buf));
				
				/*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n",
					gyro.x, gyro.y, gyro.z,
					acc.x, acc.y, acc.z,
					magn.x, magn.y, magn.z,
					int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),
					int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*//*
				
				int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8;
				int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8;
				
				int16_t pitch_angle_error = pitch_angle_target - pitch;
				int16_t roll_angle_error = roll_angle_target - roll;
				
				// 25 deg max error.
				saturate(pitch_angle_error, 4551);
				saturate(roll_angle_error, 4551);
				
				pitch_angle_accum += pitch_angle_error;
				roll_angle_accum += roll_angle_error;
				
				// 20 deg s max error.
				saturate(pitch_angle_accum, 364088);
				saturate(roll_angle_accum, 364088);
				
				int32_t pitch_rate_target = (pitch_angle_error * 2 * 65536 + pitch_angle_accum * 98) >> 16;
				int32_t roll_rate_target = (roll_angle_error * 2 * 65536 + roll_angle_accum * 98) >> 16;
				
				int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16;
				int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16;
				
				saturate(pitch_rate_comp, 250);
				saturate(roll_rate_comp, 250);
				
				motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0);
				
				time += MS2ST(10);
				if(time > chTimeNow()) {
					chThdSleepUntil(time);
				}
			}
		}
};

I2CThread i2c_thread;

static const ADCConversionGroup adcgrpcfg = {
	FALSE,
	2,
	0,
	0,
	0,
	0,
	0,
	ADC_SQR1_NUM_CH(2),
	0,
	ADC_SQR3_SQ2_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN15)
};

class ADCThread : public BaseThread<ADCThread, 128> {
	private:
		adcsample_t adc_samples[2];
		
	public:
		noreturn_t thread_main() {
			adcStart(&ADCD1, NULL);
			
			systime_t time = chTimeNow();
			while (TRUE) {
				adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, 1);
				sensordata[12] = adc_samples[0] * 1265 / 1000;
				sensordata[13] = adc_samples[1] * 2201 / 1000;
				
				time += MS2ST(1000);
				chThdSleepUntil(time);
			}
		}
};

ADCThread adc_thread;

int main(void) {
	halInit();
	chSysInit();
	
	led_thread.start();
	
	ppmsum.start();
	
	usbs.init();
	
	i2c_thread.start();
	
	adc_thread.start();
	
	motors.start();
	
	usb_thread.usbs = &usbs;
	usb_thread.start();
	
	while (1) {
		chThdSleepMilliseconds(1000);
	}
}
*/