#include <Arduino.h>
#include <ax12.h>
#include "mpu6050.hpp"
#include "imu.hpp"
#include "servo-control.hpp"

#define DENOISE_PREVIOUS_INERTIA_RATE 0.999
#define DENOISE_CURRENT_INERTIA_RATE 0.001

#define DENOISE_PREVIOUS_MPU_RATE 0.999
#define DENOISE_CURRENT_MPU_RATE 0.001

measurement current_mpu_measurement;
inertia current_mpu_inertia;
inertia previous_mpu_inertia;

measurement_double_precission previous_mpu_measurement_dp;
measurement_double_precission current_mpu_measurement_dp;

unsigned long current_timestamp_us;
unsigned long loop_counter = 0;
int servo_id;
int roll = 125;
int pitch;
int toggle = 50;

//header
void denoise_inertia_with_complementary_filter();
void denoise_mpu_measurement_with_complementary_filter();
double denoise_complementary(const double previous, const double current);
void print_inertia_gyroscope_values();
void print_inertia_acceleration_values();
void print_mpu_acceleration_data();
void print_mpu_gyroscope_data();
void print_mpu_offset();
void print_loop_time_micros();
void print_servo_torques();


void setup() {
	setup_mpu_6050_registers();
	calculate_mpu_offsets_as_average();
	read_mpu_6050_data(current_mpu_measurement);
	initialize_inertia(current_mpu_inertia, current_mpu_measurement);

	//servos_init_with_fixed_position();
	servos_init_with_current_position();
	servos_in_position_wait();
	Serial.begin(115200);
	current_timestamp_us = micros();
}

void loop() {
<<<<<<< HEAD
	read_mpu_6050_data(current_mpu_measurement);
=======
	read_mpu_6050_data_with_offset_substraction(current_mpu_measurement);
>>>>>>> vector-based-inertia
	calculate_inertia(current_mpu_inertia, current_mpu_measurement);
	correct_inertia_drift(current_mpu_inertia);
	denoise_inertia_with_complementary_filter();
	//substract_gyroscope_offset(current_mpu_measurement);
	//mpu_measurement_with_precission(current_mpu_measurement_dp, current_mpu_measurement);
	//denoise_mpu_measurement_with_complementary_filter();


	// PRINT DATA TO TERMINAL
	//print_mpu_acceleration_data();
	//print_mpu_gyroscope_data();
	//print_inertia_acceleration_values();
	//print_inertia_gyroscope_values();
	//print_mpu_offset();

	//servos_collect_current_torque();
	//print_servo_torques();

	map_roll_to_servos_with_initial_position_and_move(current_mpu_inertia.roll_acceleration);
	map_pitch_to_servos_with_initial_position_and_move(current_mpu_inertia.pitch_acceleration);

	while (micros() - current_timestamp_us < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
	//print_loop_time_micros();
	current_timestamp_us = micros();                      //Reset the loop timer

}

void denoise_inertia_with_complementary_filter() {
	current_mpu_inertia.roll_gyroscope = previous_mpu_inertia.roll_gyroscope
			* DENOISE_PREVIOUS_INERTIA_RATE
			+ current_mpu_inertia.roll_gyroscope * DENOISE_CURRENT_INERTIA_RATE;

	current_mpu_inertia.pitch_gyroscope =
			previous_mpu_inertia.pitch_gyroscope * DENOISE_PREVIOUS_INERTIA_RATE
					+ current_mpu_inertia.pitch_gyroscope
							* DENOISE_CURRENT_INERTIA_RATE;

	previous_mpu_inertia = current_mpu_inertia;
}

void denoise_mpu_measurement_with_complementary_filter() {
	current_mpu_measurement_dp.acceleration.x = denoise_complementary(
			previous_mpu_measurement_dp.acceleration.x,
			current_mpu_measurement_dp.acceleration.x);
	current_mpu_measurement_dp.acceleration.y = denoise_complementary(
			previous_mpu_measurement_dp.acceleration.y,
			current_mpu_measurement_dp.acceleration.y);
	current_mpu_measurement_dp.acceleration.z = denoise_complementary(
			previous_mpu_measurement_dp.acceleration.z,
			current_mpu_measurement_dp.acceleration.z);
	current_mpu_measurement_dp.gyroscope.x = denoise_complementary(
			previous_mpu_measurement_dp.gyroscope.x,
			current_mpu_measurement_dp.gyroscope.x);
	current_mpu_measurement_dp.gyroscope.y = denoise_complementary(
			previous_mpu_measurement_dp.gyroscope.y,
			current_mpu_measurement_dp.gyroscope.y);
	current_mpu_measurement_dp.gyroscope.z = denoise_complementary(
			previous_mpu_measurement_dp.gyroscope.y,
			current_mpu_measurement_dp.gyroscope.z);

	previous_mpu_measurement_dp = current_mpu_measurement_dp;
}

double denoise_complementary(const double previous, const double current) {
	return previous * DENOISE_PREVIOUS_MPU_RATE
			+ current * DENOISE_CURRENT_MPU_RATE;
}

void print_inertia_acceleration_values() {
	Serial.print("acc_rp=[");
	Serial.print(current_mpu_inertia.roll_acceleration);
	Serial.print(";");
	Serial.print(current_mpu_inertia.pitch_acceleration);
	Serial.println("]");
}

void print_inertia_gyroscope_values() {
	Serial.print("gyr_rp=[");
	Serial.print(current_mpu_inertia.roll_gyroscope);
	Serial.print(";");
	Serial.print(current_mpu_inertia.pitch_gyroscope);
	Serial.println("]");
}

void print_mpu_acceleration_data() {
	Serial.print("acc=[");
	Serial.print(
			acceleration_with_precission(
					current_mpu_measurement.acceleration.x));
	//Serial.print(current_mpu_measurement.acceleration.x);
	Serial.print(";");
	Serial.print(
			acceleration_with_precission(
					current_mpu_measurement.acceleration.y));
	//Serial.print(current_mpu_measurement.acceleration.y);
	Serial.print(";");
	Serial.print(
			acceleration_with_precission(
					current_mpu_measurement.acceleration.z));
	//Serial.print(current_mpu_measurement.acceleration.z);
	Serial.println("]");
}

void print_mpu_gyroscope_data() {
	Serial.print("gyro=[");
	Serial.print((int) current_mpu_measurement_dp.gyroscope.x);
	//Serial.print(
			//gyroscope_with_precission(current_mpu_measurement.gyroscope.x));
	//Serial.print(current_mpu_measurement.gyroscope.x);
	Serial.print(";");
	Serial.print((int)
				current_mpu_measurement_dp.gyroscope.y);
	//Serial.print(
			//gyroscope_with_precission(current_mpu_measurement.gyroscope.y));
	//Serial.print(current_mpu_measurement.gyroscope.y);
	Serial.print(";");
	Serial.print((int)current_mpu_measurement_dp.gyroscope.z);
	//Serial.print(
			//gyroscope_with_precission(current_mpu_measurement.gyroscope.z));
	//Serial.print(current_mpu_measurement.gyroscope.z);
	Serial.println("]");
}

void print_mpu_offset() {
	Serial.print("offset=[");
	Serial.print(get_mpu_offset().acceleration.x);
	Serial.print(";");
	Serial.print(get_mpu_offset().acceleration.y);
	Serial.print(";");
	Serial.print(get_mpu_offset().acceleration.z);
	Serial.println("]");
}

void print_loop_time_micros() {
	Serial.print(loop_counter++);
	Serial.print(" diff ms: ");
	Serial.println(micros()-current_timestamp_us);
}

void print_servo_torques() {
	servos_print_current_torque();
}