// EVERYTHING IS IN METERS!!!
// drafted and designed by Otis Campbell
// functionExample
// variable_example

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

Adafruit_MPU6050 mpu;
Servo myservo; //rudder servo

#define PI 3.1415926535897932384626433832795

float current_dir = 90; // direction stored in degrees
const int waypoints = 3;
float waypoint_list[2*waypoints] = {35,30,20,30,5,5}; // waypoints stored as list. x1, y1, x2, y2....

int servo_pin = 3;

float pond_length = 55;
float x_pos = 35; //update with starting x position
float y_pos = 2; //update with starting y position
unsigned long prev_time; //previous time
unsigned long current_time = 0; //current time
float prev_accel_x = 0; //previous acceleration in x
float prev_accel_y = 0; //previous acceleration in y
float gyroList[3] = {0,0,0};
float x_velo_init = 0;
float y_velo_init = 0;


void setup() {
  Serial.begin(9600);

  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G); //possible values of 2,4,8,16 G
  Serial.println("Accelerometer range set to: 8 G");

  mpu.setGyroRange(MPU6050_RANGE_500_DEG); //possible values of 250,500,1000,2000 G
  Serial.println("Gyro range set to: 500 degrees");

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); //possible values of 260,184,94,44,21,10,5 G
  Serial.println("Filter bandwidth set to: 21 Hz");

  myservo.attach(servo_pin);

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  gyroReport(a,g);

  prev_accel_x = gyroList[0];
  prev_accel_y = gyroList[1];

  delay(1000);
  Serial.print("Est. Setup time: ");
  prev_time = millis();
  Serial.print(prev_time /1000.0);
  Serial.println(" s");
  Serial.println("");
}

//system general functions--------------------------------------------------------------------------

void killCode() {
  Serial.print("Process Terminated. Runtime: ");
  Serial.print(millis()/1000.0);
  Serial.print(" s");
  delay(1000);
  exit(0);
}

void posPrint(float x_pos, float y_pos) {
  Serial.print(x_pos);
  Serial.print(", ");
  Serial.print(y_pos);
  Serial.print("\n");
}

//Position Checker function--------------------------------------------------------------------------

int checkPos(float accel_x_in, float accel_y_in, float rot_in) {
  rot_in = rot_in + 90; // optional angle adjustment for boat orientation purposes
  Serial.println(rot_in);
  float ave_accel_x = 0;
  float ave_accel_y = 0;
  float change_x = 0;
  float change_y = 0;

  double elapsed_time;
  float resultX;
  float x_min = 0;
  float x_max = pond_length;
  float center_check = (sq(x_pos-27.5) + sq(y_pos-17));

  if (x_pos >= x_min && x_pos <= x_max) {
    if (y_pos >= (sqrt(sq(27.5)-sq(x_pos-27.5))+9) || y_pos <= 0) {
      Serial.print("Out of y coord bounds\n");
      posPrint(x_pos,y_pos);
      killCode();
    }

    if (center_check <= 25) {
      Serial.print("Contacting or inside pond center\n");
      posPrint(x_pos,y_pos);
      killCode();
    }

    Serial.print("Boundary Check Passed!\n");

    elapsed_time = (millis() - prev_time) / 1000.0;
    prev_time = millis();

    Serial.print("Delta Time: ");
    Serial.print(elapsed_time);
    Serial.print(" s, Time Stamp: ");
    Serial.print(prev_time/1000.0);
    Serial.println(" s");
    
    ave_accel_x = (accel_x_in + prev_accel_x) / 2;
    ave_accel_y = (accel_y_in + prev_accel_y) / 2;
    prev_accel_x = accel_x_in;
    prev_accel_y = accel_y_in;

    Serial.print(ave_accel_x);
    Serial.println(" m/s^2");
    Serial.print(ave_accel_y);
    Serial.println(" m/s^2");

    x_velo_init = x_velo_init + (ave_accel_x * elapsed_time);
    change_x = (x_velo_init*elapsed_time)+(0.5*ave_accel_x*elapsed_time*elapsed_time);
    
    Serial.print(x_velo_init);
    Serial.println(" m/s");

    y_velo_init = y_velo_init + (ave_accel_y * elapsed_time);
    change_y = (y_velo_init*elapsed_time)+(0.5*ave_accel_y*elapsed_time*elapsed_time);
    
    x_pos = x_pos + change_x;
    y_pos = y_pos + change_y;
    
    posPrint(x_pos,y_pos);
    Serial.print("Change in x:");
    Serial.print(change_x);
    Serial.print(", Change in y:");
    Serial.print(change_y);
    Serial.print("\n");

    

    resultX = 1;
  }

  else {
    Serial.print("Out of x coord bounds\n");
    killCode();
  }

  return resultX;
}

//Servo Control Function--------------------------------------------------------------------------

void steerControl(float ship_angle, float tar_x, float tar_y, float x_pos, float y_pos, int motor_impulse) {
  // range from 0 to 140 (technically, 150, 145 safe)
  //(tan(current_dir[1]/current_dir[0]) * (180/pi()))
  float target_vector[2] = {(tar_x - x_pos),(tar_y - y_pos)};
  float target_angle = (atan(target_vector[1]/target_vector[0]) * (180/PI)) +180;
  float servo_angle = (ship_angle - target_angle + 90) * (140.0/180);

  Serial.print("Target Angle: ");
  Serial.print(target_angle);
  Serial.println("");
  Serial.print("Angle Adjustment: ");
  Serial.print(ship_angle - target_angle);
  Serial.println("");
  Serial.print("Set angle: ");
  Serial.println(servo_angle);
  myservo.write(servo_angle);
}

// Gyro Control Function--------------------------------------------------------------------------
float gyroReport(sensors_event_t a, sensors_event_t g) {
  float x_accel = a.acceleration.x;
  float y_accel = a.acceleration.y;
  float rotation = g.gyro.z;
  float reportList[3] = {x_accel, y_accel, rotation};
  
  Serial.println(reportList[0]);
  Serial.println(reportList[1]);
  Serial.println(reportList[2]);
  gyroList[0] = reportList[0];
  gyroList[1] = reportList[1];
  gyroList[2] = reportList[2];
  
}
//Runtime Code--------------------------------------------------------------------------

void loop() {
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  gyroReport(a,g);
  checkPos(gyroList[0], gyroList[1], gyroList[2]);
  //Serial.println(millis() /1000.0);

  Serial.println("---servo---");
  steerControl(current_dir, 30, 30, x_pos, y_pos, 5);
  Serial.println("");
}