#include "Delay.hpp"
#include "stm32f10x.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_rcc.h"
#include "ztMpu6050.hpp"
#include "ztOled.hpp"
#include <cstdint>
#include <stdio.h>


int main(){
	// zt::Led leda0(RCC_APB2Periph_GPIOA,GPIO_Pin_0,GPIOA);
	// zt::Led ledc13(RCC_APB2Periph_GPIOC,GPIO_Pin_13,GPIOC);

	zt::Oled oled(RCC_APB2Periph_GPIOB,GPIOB,GPIO_Pin_8,GPIO_Pin_9);
	oled.ShowString(1, 1, "Hello,world!")
	.ShowString(2, 1, "by Zengtudor")
	.ShowString(3, 1, "From BDFS EECRPD")
	.ShowString(4, 1, "time:");

	delay_ms(500);

	zt::Mpu6050 mpu(RCC_APB1Periph_I2C2,I2C2,RCC_APB2Periph_GPIOB,GPIOB,GPIO_Pin_10,GPIO_Pin_11);
	int16_t arr[6],first[6];
	oled.Clear();
	const float change = 32767.0f/4.0f;
	char c[3][50];
	mpu.MPU6050_GetData(arr, arr+1, arr+2, arr+3, arr+4, arr+5);
	first[0]=arr[0],first[1]=arr[1],first[2]=arr[2];
	while(true){
		mpu.MPU6050_GetData(arr, arr+1, arr+2, arr+3, arr+4, arr+5);
		sprintf(c[0], "%.2f",float(arr[0]-first[0])/change);
		sprintf(c[1], "%.2f",float(arr[1]-first[1])/change);
		sprintf(c[2], "%.2f",float(arr[2]-first[2])/change);
		oled.ShowString(1, 1, "id:")
			.ShowUNum(1,4,mpu.MPU6050_GetID())
			.ShowString(2,1,c[0])
			.ShowString(3,1,c[1])
			.ShowString(4,1,c[2]);
	}
}