#include <Servo.h>
#include <PID_v1.h>
#include <Adafruit_MPU6050.h>
#include <Wire.h>

const int p_sonar_trig                      = 7;
const int p_sonar_echo                      = 8;
const int p_wing_front_right_control_signal = 9;
const int p_wing_front_left_control_signal  = 10;

Servo             wing_front_right; 
Servo             wing_front_left; 
Adafruit_MPU6050  mpu;

double distance_in_cm;
double setpoint, input, output;
double control_signal;
double Kp = 15, Ki = 10, Kd = 0;

PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

const int filter_samples_amount = 20;
int filter_samples[filter_samples_amount];     
int sample_index  = 0;             
int total         = 0;             
int average       = 0;

// Simulation
double slider;
double speed;
double lift;
double downforce;
double acceleration;

void setup(void) {
  setpoint = 13;
  for (int thisReading = 0; thisReading < filter_samples_amount; thisReading++) {
    filter_samples[thisReading] = 0;
  }

  pinMode(p_sonar_trig, OUTPUT);
  pinMode(p_sonar_echo, INPUT);

  wing_front_right.attach(p_wing_front_right_control_signal);
  wing_front_left.attach(p_wing_front_left_control_signal);
  myPID.SetMode(AUTOMATIC);

  Serial.begin(115200);

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("");

  delay(100);
}

void loop() {
  control_signal = map(output, 0, 255, -50, 50);
  wing_front_right.write(map(control_signal, -50, 50, 40, 140)); 
  wing_front_left.write(map(control_signal, -50, 50, 140, 40)); 
  input = filter_sonar_distance(get_sonar_distance());
  myPID.Compute();

  // Debug
  debug_show_values();
  print_gyro_values();

  delay(20);
}

long convert_ms_to_cm(long microseconds) {
  return microseconds / 29 / 2;
}

long get_sonar_distance() {
  digitalWrite(p_sonar_trig, LOW);
  delayMicroseconds(2); 
  digitalWrite(p_sonar_trig, HIGH);
  delayMicroseconds(10); 
  digitalWrite(p_sonar_trig, LOW);
  long pulse_duration_ms, distance_in_cm;

  pulse_duration_ms = pulseIn(p_sonar_echo, HIGH);
  distance_in_cm    = convert_ms_to_cm(pulse_duration_ms);

  return distance_in_cm;
}

bool debug_show_values() {
  Serial.print(slider * 2 > speed);
  Serial.print(",");  
  Serial.print(control_signal);
  Serial.print(",");
  Serial.print(filter_sonar_distance(get_sonar_distance()));
  Serial.println((""));
}

double filter_sonar_distance(int distance) {
  total = total - filter_samples[sample_index];
  filter_samples[sample_index] = get_sonar_distance();
  total = total + filter_samples[sample_index];
  sample_index = sample_index + 1;

  if (sample_index >= filter_samples_amount) {
    sample_index = 0;
  }

  return total / filter_samples_amount;
}

bool print_gyro_values() {
  /* Get new sensor events with the filter_samples */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  Serial.print(a.acceleration.x);
  Serial.print(",");
  Serial.print(a.acceleration.y);
  Serial.print(",");
  Serial.print(a.acceleration.z);
  Serial.print(", ");
  Serial.print(g.gyro.x);
  Serial.print(",");
  Serial.print(g.gyro.y);
  Serial.print(",");
  Serial.print(g.gyro.z);
  Serial.println("");
}