#include "mpu6050.hpp"

measurement mpu_offset;

void setup_mpu_6050_registers() {
	Wire.begin();
	//Activate the MPU-6050
	Wire.beginTransmission(0x68);        		//Start communicating with the MPU-6050
	Wire.write(0x6B);                     		//Send the requested starting register
	Wire.write(0x00);                      		//Set the requested starting register
	Wire.endTransmission();
	//Configure the accelerometer (+/-8g)
	Wire.beginTransmission(0x68);        		//Start communicating with the MPU-6050
	Wire.write(0x1C);                     		//Send the requested starting register
	Wire.write(0x10);                      		//Set the requested starting register
	Wire.endTransmission();
	//Configure the gyro (500dps full scale)
	Wire.beginTransmission(0x68);        		//Start communicating with the MPU-6050
	Wire.write(0x1B);                     		//Send the requested starting register
	//Wire.write(0x08);                      		//Set the requested starting register 500°
	Wire.write(0x10);                      		//Set the requested starting register 250°
	Wire.endTransmission();
	//TODO Configure digital low pass filter
	/*Wire.beginTransmission(0x68);
	Wire.write(0x1A);
	Wire.write(0x04);
	Wire.endTransmission();*/
}

void read_mpu_6050_data_with_offset_substraction(measurement &measurement) {
	read_mpu_6050_data(measurement);
	//TODO: check if necessary
	substract_gyroscope_offset(measurement);
	//substract_accelerometer_offset(measurement);
}

void read_mpu_6050_data(measurement &measurement) { 					//Subroutine for reading the raw gyro and accelerometer data
	Wire.beginTransmission(0x68);        		//Start communicating with the MPU-6050
	Wire.write(0x3B);                     		//Send the requested starting register
	Wire.endTransmission();                     //End the transmission
	Wire.requestFrom(0x68, 14);             	//Request 14 bytes from the MPU-6050
	while (Wire.available() < 14);              //Wait until all the bytes are received
	measurement.acceleration.x = Wire.read() << 8 | Wire.read();
	measurement.acceleration.y = Wire.read() << 8 | Wire.read();
	measurement.acceleration.z = Wire.read() << 8 | Wire.read();
	measurement.temperature = Wire.read() << 8 | Wire.read();
	measurement.gyroscope.x = Wire.read() << 8 | Wire.read();
	measurement.gyroscope.y = Wire.read() << 8 | Wire.read();
	measurement.gyroscope.z = Wire.read() << 8 | Wire.read();
}

void substract_gyroscope_offset(measurement &measurement) {
	measurement.gyroscope.x -= mpu_offset.gyroscope.x;
	measurement.gyroscope.y -= mpu_offset.gyroscope.y;
	measurement.gyroscope.z -= mpu_offset.gyroscope.z;
}

void substract_accelerometer_offset(measurement &measurement) {
	measurement.acceleration.x -= mpu_offset.acceleration.x;
	measurement.acceleration.y -= mpu_offset.acceleration.y;
	measurement.acceleration.z -= mpu_offset.acceleration.z;
}

void calculate_mpu_offsets_as_average() {
	mpu_offset.acceleration.x = 0;
	mpu_offset.acceleration.y = 0;
	mpu_offset.acceleration.z = 0;
	measurement measurement;
	for (int cal_int = 0; cal_int < 1000; cal_int++) { //Read the raw acc and gyro data from the MPU-6050 for 1000 times
		read_mpu_6050_data(measurement);
		mpu_offset.gyroscope.x += measurement.gyroscope.x; 					//Add the gyro x offset to the gyro_x_cal variable
		mpu_offset.gyroscope.y += measurement.gyroscope.y; 					//Add the gyro y offset to the gyro_y_cal variable
		mpu_offset.gyroscope.z += measurement.gyroscope.z; 					//Add the gyro z offset to the gyro_z_cal variable
		mpu_offset.acceleration.x += measurement.acceleration.x;
		mpu_offset.acceleration.y += measurement.acceleration.y;
		mpu_offset.acceleration.z += measurement.acceleration.z;
		delay(3);          													//Delay 3us to have 250Hz for-loop
	}

	// get average offset
	mpu_offset.gyroscope.x /= 1000;
	mpu_offset.gyroscope.y /= 1000;
	mpu_offset.gyroscope.z /= 1000;
	mpu_offset.acceleration.x /= 1000;
	mpu_offset.acceleration.y /= 1000;
	mpu_offset.acceleration.z /= 1000;
}

measurement get_mpu_offset() {
	return mpu_offset;
}

void mpu_measurement_with_precission(measurement_double_precission &measurement_dp, measurement const measurement) {
	measurement_dp.acceleration.x = measurement.acceleration.x * ACCELEROMETER_CONST;
	measurement_dp.acceleration.y = measurement.acceleration.y * ACCELEROMETER_CONST;
	measurement_dp.acceleration.z = measurement.acceleration.z * ACCELEROMETER_CONST;

	measurement_dp.gyroscope.x = measurement.gyroscope.x * GYRO_CONST;
	measurement_dp.gyroscope.y = measurement.gyroscope.y * GYRO_CONST;
	measurement_dp.gyroscope.z = measurement.gyroscope.z * GYRO_CONST;
}