#include <ax12.h>
#include "mpu6050.hpp"
#include "imu.hpp"
#include "servo-control.hpp"
#include "movement.hpp"
#include "noise.hpp"
#include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise

#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 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 readMPU();


void setup() {
	Serial.begin(57600);
  
  Serial.println("booting");
  
  Serial.println("setup 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_PULLUP);
	



  matrix_weights_update(); //initialize weights matrix
  Serial.println("booted");
  
}

void loop() {
  unsigned long loopmillis=millis();
  static unsigned long last_loop_movement_millis;
  static unsigned long last_loop_readpos_millis;

  static unsigned long last_servo_send_millis;

  

  if (loopmillis-last_loop_movement_millis>50) {
    last_loop_movement_millis=loopmillis;
    int state_switch_programm = digitalRead(switch_programm);  //currently not used
    //Serial.println(digitalRead(switch_programm));
    
  
    //move_protesis_with_sensor_data(); //old movement function
    //slither();

    readMPU();
    update_mood();
    map_servos_by_weights_with_initial_position_and_move(current_mpu_inertia.roll_acceleration,current_mpu_inertia.pitch_acceleration);
    last_servo_send_millis=millis();

    //Serial.println("moved servos");
  }

  if (loopmillis-last_loop_readpos_millis>100 && loopmillis-last_servo_send_millis>=4) { //limit frequency and minimum delay after send positions
    last_loop_readpos_millis=loopmillis;
    read_servos_positions(); //takes around 8000us for all servos. read and send over serial.
  }
  

}

void update_mood() {  
  #define GYROSCOPE_FILTER_SHAKINESS 0.8
  float gyro=constrain(sqrt(pow(current_mpu_measurement.gyroscope.x,2)+pow(current_mpu_measurement.gyroscope.y,2)) / 10000.0   ,0.0, 1.0);
  mood.shakiness = constrain(mood.shakiness*GYROSCOPE_FILTER_SHAKINESS + (1-GYROSCOPE_FILTER_SHAKINESS)*gyro, 0,1.);
  #define GYROSCOPE_FILTER_WAKEFULNESS 0.99
  mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.);
  
  double roll=current_mpu_inertia.roll_acceleration;
  double pitch=current_mpu_inertia.pitch_acceleration;
  double rollmapped=constrain( mapfloat(abs(roll),5,10,0,1),0,1);
  double pitchmapped=constrain( mapfloat(abs(pitch),5,10,0,1),0,1);
  
  #define LONELINESS_FILTER 0.99
  mood.loneliness= constrain( mood.loneliness*LONELINESS_FILTER + (1-(rollmapped+pitchmapped)/2)*(1-LONELINESS_FILTER) ,0.0,1.0);

  /*
  Serial.print(gyro);
  Serial.print(", ");
  Serial.print(mood.shakiness);
  Serial.print(", ");
  Serial.print(mood.wakefulness);  
  Serial.print(", ");
  Serial.println(mood.loneliness);  
  */
  

  matrix_weights_update();
}

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

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