/*
 * imu.cpp
 *
 *  Created on: 09.01.2021
 *      Author: frank
 */

#include <Arduino.h>
#include "imu.hpp"

inertia inertia_offset;

void initialize_inertia(inertia &inertia, const measurement mpu_measurement) {

	inertia_offset.roll_vector_length = roll_vector_length(get_mpu_offset());
	inertia_offset.pitch_vector_length = pitch_vector_length(get_mpu_offset());
	inertia_offset.yaw_vector_length = yaw_vector_length(get_mpu_offset());

	calculate_inertia(inertia, mpu_measurement);

	//inertia_offset.roll_vector_length = inertia.roll_vector_length;
	//inertia_offset.pitch_vector_length = inertia.pitch_vector_length;
	//inertia_offset.yaw_vector_length = inertia.pitch_vector_length;

	inertia_offset.roll_acceleration = inertia.roll_acceleration;
	inertia_offset.pitch_acceleration = inertia.pitch_acceleration;

	inertia_offset.roll_gyroscope = inertia.roll_gyroscope;
	inertia_offset.pitch_gyroscope = inertia.pitch_gyroscope;

	inertia_offset.vector_length = inertia.vector_length;

	inertia.roll_gyroscope = inertia.roll_acceleration;
	inertia.pitch_gyroscope = inertia.pitch_acceleration;
}

void calculate_inertia_with_offset_substraction(inertia &inertia,
		const measurement mpu_measurement) {
	calculate_inertia(inertia, mpu_measurement);
	substract_acceleration_offset(inertia);
}

void calculate_inertia(inertia &inertia, const measurement mpu_measurement) {

	inertia.vector_length = vector_length_3d(mpu_measurement);

	/*inertia.roll_acceleration = asin(
	 (float) mpu_measurement.acceleration.y / inertia.vector_length)
	 * -RADIAN_CONST
	 ;
	 inertia.pitch_acceleration = asin(
	 (float) mpu_measurement.acceleration.x / inertia.vector_length)
	 * RADIAN_CONST
	 ;*/

	inertia.roll_acceleration = atan2(mpu_measurement.acceleration.y,
			mpu_measurement.acceleration.z) * RAD_TO_DEG;

	inertia.pitch_acceleration = atan2(-mpu_measurement.acceleration.x,
			vector_length_2d(mpu_measurement.acceleration.y,
					mpu_measurement.acceleration.z)) * RAD_TO_DEG;

	//inertia.roll_acceleration = 180 * (asin(mpu_measurement.acceleration.y / vector_length_3d(mpu_measurement)))/M_PI;
	//inertia.pitch_acceleration = 180 * (asin(mpu_measurement.acceleration.x / vector_length_3d(mpu_measurement)))/M_PI;

	/*inertia.roll_vector_length = roll_vector_length(mpu_measurement);
	 inertia.pitch_vector_length = pitch_vector_length(mpu_measurement);
	 inertia.yaw_vector_length = yaw_vector_length(mpu_measurement);

	 inertia.roll_acceleration = roll_by_acceleration(inertia, mpu_measurement);
	 inertia.pitch_acceleration = pitch_by_acceleration(inertia,
	 mpu_measurement);*/

	//TODO Evaluate correctness of dimensions (actually x -> roll, y -> pitch)
	inertia.roll_gyroscope += gyroscope_with_precission(
			mpu_measurement.gyroscope.x);
	inertia.pitch_gyroscope += gyroscope_with_precission(
			mpu_measurement.gyroscope.y);

	inertia.roll_gyroscope -= inertia.pitch_gyroscope
	 * sin(gyroscope_with_precission(mpu_measurement.gyroscope.z)) * RAD_TO_DEG; //If the IMU has yawed transfer the pitch angle to the roll angel
	 inertia.pitch_gyroscope += inertia.roll_gyroscope
	 * sin(gyroscope_with_precission(mpu_measurement.gyroscope.z)) * RAD_TO_DEG; //If the IMU has yawed transfer the roll angle to the pitch angel
}

void substract_acceleration_offset(inertia &inertia) {
	inertia.roll_acceleration -= inertia_offset.roll_acceleration;
	inertia.pitch_acceleration -= inertia_offset.pitch_acceleration;
}

void correct_inertia_drift(inertia &inertia) {
	inertia.roll_gyroscope = inertia.roll_gyroscope * DRIFT_CORRECTION_RATE_GYRO
	+ inertia.roll_acceleration * DRIFT_CORRECTION_RATE_ACCEL;

	inertia.pitch_gyroscope = inertia.pitch_gyroscope
			* DRIFT_CORRECTION_RATE_GYRO
	+ inertia.pitch_acceleration * DRIFT_CORRECTION_RATE_ACCEL;
}

long square(const long value) {
	return value * value;
}

double roll_vector_length(const measurement mpu_measurement) {
	return vector_length_2d(mpu_measurement.acceleration.y,
			mpu_measurement.acceleration.z);
}

double pitch_vector_length(const measurement mpu_measurement) {
	return vector_length_2d(mpu_measurement.acceleration.x,
			mpu_measurement.acceleration.z);
}

double yaw_vector_length(const measurement mpu_measurement) {
	return vector_length_2d(mpu_measurement.acceleration.x,
			mpu_measurement.acceleration.y);
}

double roll_by_acceleration(const inertia inertia,
		const measurement mpu_measurement) {

	long scalar_product = mpu_measurement.acceleration.x
			* get_mpu_offset().acceleration.x
			+ mpu_measurement.acceleration.z * get_mpu_offset().acceleration.z;

	double area = inertia.roll_vector_length
			* inertia_offset.roll_vector_length;
	/*Serial.print("sp=");
	 Serial.print(scalar_product);
	 Serial.print(",a=");
	 Serial.print(area);
	 Serial.print(",v=");
	 Serial.print(inertia_offset.roll_vector_length);
	 Serial.println();*/
	return acos(scalar_product / area) * RADIAN_CONST;
}

double pitch_by_acceleration(const inertia inertia,
		const measurement mpu_measurement) {
	long scalar_product = mpu_measurement.acceleration.y
			* get_mpu_offset().acceleration.y
			+ mpu_measurement.acceleration.z * get_mpu_offset().acceleration.z;

	double area = inertia.pitch_vector_length
			* inertia_offset.pitch_vector_length;

	return acos(scalar_product / area) * RADIAN_CONST;
}

double vector_length_2d(const long x, const long y) {
	return sqrt((double) square(x) + square(y));
}

double vector_length_3d(measurement const measurement) {
	return sqrt(
			(double) square(measurement.acceleration.x)
					+ square(measurement.acceleration.y)
					+ square(measurement.acceleration.z));
}

double acceleration_with_precission(const long value) {
	return value * ACCELEROMETER_CONST;
}

double gyroscope_with_precission(const long value) {
	return value * GYRO_CONST;
}

inertia get_inertia_offset() {
	return inertia_offset;
}