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

#define DENOISE_PREVIOUS_INERTIA_RATE 0.9999
#define DENOISE_CURRENT_INERTIA_RATE 0.0001

#define DENOISE_PREVIOUS_MPU_RATE 0.9999
#define DENOISE_CURRENT_MPU_RATE 0.0001

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;

//Switch
const int switch_programm = A3;
int state_switch_programm = 0;

//header
void move_protesis_with_sensor_data();
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() {
	Serial.begin(115200);
  
  Serial.println("booting");
  
  Serial.println("setupr registers");
  setup_mpu_6050_registers();
  Serial.println("calc mpu offsets");
	calculate_mpu_offsets_as_average();
  Serial.println("read mpu data");
	read_mpu_6050_data(current_mpu_measurement);
  Serial.println("init inertia");
	initialize_inertia(current_mpu_inertia, current_mpu_measurement);

  Serial.println("init servos");
	servos_init_with_fixed_position();
	//servos_init_with_current_position();
  Serial.println("wait servos");
	servos_in_position_wait();
	pinMode(switch_programm, INPUT);
	
	current_timestamp_us = micros();

  Serial.println("booted");
  
}

void loop() {
  Serial.print("state_switch_programm=");
  Serial.println(state_switch_programm);


  int state_switch_programm = digitalRead(switch_programm);
  //Serial.println(digitalRead(switch_programm));
	if(state_switch_programm == 1) {
    slither();
	} else {
    move_protesis_with_sensor_data();
   
	}

	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 move_protesis_with_sensor_data() {
	read_mpu_6050_data_with_offset_substraction(current_mpu_measurement);
	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();

	//servos_collect_current_torque();
	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);

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

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