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

Adafruit_MPU6050 mpu;
Servo Rservo;
Servo Lservo;

int serval_R;
int serval_L;
float GyroX;
float GyroY;
float GyroZ;
int horzval;
int vertval;

int duration;
int initial_state;
int SensPeriod;
unsigned long CurrMill;
unsigned long LastMill;

int countP;
float Pitch;
int countR;
float Roll;
int countY;
float Yaw;

#define SERPIN 2
#define TRIGPIN 18
#define ECHOPIN 5
#define HORZPIN 34
#define VERTPIN 35

void setup() {
  Serial.begin(115200);
  pinMode(TRIGPIN, OUTPUT);
  pinMode(ECHOPIN, INPUT);

  mpu.begin();

  Rservo.setPeriodHertz(50);
  Rservo.attach(SERPIN, 544, 2400);
  Lservo.setPeriodHertz(50);
  Lservo.attach(SERPIN, 544, 2400);
  Rservo.write(90);
  Lservo.write(90);

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);

  initial_state = 0;
  SensPeriod = 100;
  LastMill = 0;
  Pitch = 0;
  Roll = 0;
  Yaw = 0;
}

void loop() {
  //getDistance();
  getPitch();
  //getRoll();
  //getYaw();
  //getJoyHorz();
  //getJoyVert();

  //delay(1000);
}

int getDistance() {
  digitalWrite(TRIGPIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGPIN, LOW);

  duration = pulseIn(ECHOPIN, HIGH);
  Serial.print("Ultrasonic Distance: ");
  Serial.println(duration / 58);
  return duration;
}

int getPitch() {
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  for(countP = 0; countP < 10; countP++) {
    CurrMill = millis();
    if(CurrMill - LastMill >= SensPeriod) {
      GyroX += gyro.gyro.x;
      LastMill = CurrMill;
    }
    Serial.println(countP);

    if(countP == 9) {
      GyroX /= 10;
      GyroX *= 57.2958;
      Pitch += GyroX;

      if(Pitch > 359) {
        Pitch -= 360;
      }
      else if(Pitch < 0) {
        Pitch += 360;
      }

      Serial.print("GyroX: ");
      Serial.print(GyroX);
      Serial.print("\tPitch: ");
      Serial.println(Pitch);
      GyroX = 0;
    }
  }
  
  return Pitch;
}

int getYaw() {
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  for(countY = 0; countY < 10; countY++) {
    GyroY += gyro.gyro.y;
    delay(100);

    if(countY == 9) {
      GyroY /= 10;
      GyroY *= 57.2958;
      Yaw += GyroY;

      if(Yaw > 359) {
        Yaw -= 360;
      }
      else if(Yaw < 0) {
        Yaw += 360;
      }

      Serial.print("GyroY: ");
      Serial.print(GyroY);
      Serial.print("\tYaw: ");
      Serial.println(Yaw);
      GyroY = 0;
    }
  }
  
  return Yaw;
}

int getRoll() {
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  for(countR = 0; countR < 10; countR++) {
    GyroZ += gyro.gyro.z;
    delay(100);

    if(countR == 9) {
      GyroZ /= 10;
      GyroZ *= 57.2958;
      Roll += GyroZ;

      if(Roll > 359) {
        Roll -= 360;
      }
      else if(Roll < 0) {
        Roll += 360;
      }

      Serial.print("GyroZ: ");
      Serial.print(GyroZ);
      Serial.print("\tRoll: ");
      Serial.println(Roll);
      GyroZ = 0;
    }
  }
  
  return Roll;
}

int getJoyHorz() {
  horzval = analogRead(HORZPIN);
  Serial.print("JoyHorz: ");
  Serial.println(horzval);
  return horzval;
}

int getJoyVert() {
  vertval = analogRead(VERTPIN);
  Serial.print("JoyVert: ");
  Serial.println(vertval);
  return vertval;
}