#include <ESP32Servo.h>
#include <ESP32AnalogRead.h>
#include <Wire.h>

#define JTx 36
#define JTy 39
#define JTs 32
#define JMx 34
#define JMy 35
#define JMs 33
#define UStrigPin 4
#define USechoPin 2
#define Buzzer 15
#define Button 25

Servo servoFL;
Servo servoFR;
Servo servoBL;
Servo servoBR;

int SFL=0;
int SFR=0;
int SBL=0;
int SBR=0;
int deltaW=0;
int deltaZ=0;
int W=0;
int Z=0;
int deltaX=0;
int deltaY=0;
int X=0;
int Y=0;
int a=3; // Setting to hover
int b=2; // Setting to hover
float duration, distance;
int pitch=0;
int gonoff = 0;
const int i2c_addr = 0x3F;
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
float gyro_x_val, gyro_y_val, gyro_z_val;
long acc_x, acc_y, acc_z;
long acc_x_cal, acc_y_cal, acc_z_cal;
float acc_x_val, acc_y_val, acc_z_val;
int temp;
long temp_cal;
float temp_val;
long loop_timer;

void setup() {
Serial.begin(9600);
Serial.println("Welcome pilot");
pinMode(JTx, INPUT);
pinMode(JTy, INPUT);
pinMode(JTs, INPUT_PULLUP);
pinMode(JMx, INPUT);
pinMode(JMy, INPUT);
pinMode(JMs, INPUT_PULLUP);
servoFL.attach(13);
servoFR.attach(14);
servoBL.attach(12);
servoBR.attach(27);
servoFL.write(SFL);
servoFR.write(SFR);
servoBL.write(SBL);
servoBR.write(SBR);
pinMode(UStrigPin, OUTPUT);
pinMode(USechoPin, INPUT);
pinMode(Buzzer, OUTPUT);
pinMode(Button, INPUT);
calibrate_mpu6050();
Serial.println("...");
Serial.print("EDF drone is ready to fly");
}

void loop() {

gonoff = digitalRead(Button);
if (gonoff == HIGH) {
Serial.println("Automated flight control initiated");
display_mpu6050_data();
}

deltaW=analogRead(JTx);
if (deltaW>700) {
Serial.println("Spin Right");
W=W+(deltaW-511)/512;
SFR=SFR+10*W;
SBL=SBL+10*W;
servoFR.write(SFR);
servoBL.write(SBL);
delay(1000); // Sensitivity of right spin
SFR=SFR-10*W;
SBL=SBL-10*W;
servoFR.write(SFR);
servoBL.write(SBL);
}
if (deltaW<300) {
Serial.println("Spin Left");
W=W+(512-deltaW)/512;
SFL=SFL+10*W;
SBR=SBR+10*W;
servoFL.write(SFL);
servoBR.write(SBR);
delay(1000); // Sensitivity of left spin
SFL=SFL-10*W;
SBR=SBR-10*W;
servoFL.write(SFL);
servoBR.write(SBR);
}

deltaZ=analogRead(JTy);
if (deltaZ>700) {
Serial.println("Ascend");
Z=Z+(deltaZ-511)/512;
SFL=map(Z,0,18,0,180);
SFR=map(Z,0,18,0,180);
SBL=map(Z,0,18,0,180);
SBR=map(Z,0,18,0,180);
servoFL.write(SFL);
servoFR.write(SFR);
servoBL.write(SBL);
servoBR.write(SBR);
}
if (deltaZ<300) {
Serial.println("Descend");
Z=Z-(512-deltaZ)/512;
SFL=map(Z,0,18,0,180);
SFR=map(Z,0,18,0,180);
SBL=map(Z,0,18,0,180);
SBR=map(Z,0,18,0,180);
servoFL.write(SFL);
servoFR.write(SFR);
servoBL.write(SBL);
servoBR.write(SBR);
}

deltaX=analogRead(JMx);
if (deltaX>700) {
Serial.println("Right");
X=X+(deltaX-511)/512;
SFL=SFL+10*X;
SBL=SBL+10*X;
servoFL.write(SFL);
servoBL.write(SBL);
delay(1000); // Sensitivity of right turn
SFL=SFL-10*X;
SBL=SBL-10*X;
servoFL.write(SFL);
servoBL.write(SBL);
}
if (deltaX<300) {
Serial.println("Left");
X=X+(512-deltaX)/512;
SFR=SFR+10*X;
SBR=SBR+10*X;
servoFR.write(SFR);
servoBR.write(SBR);
delay(1000); // Sensitivity of left turn
SFR=SFR-10*X;
SBR=SBR-10*X;
servoFR.write(SFR);
servoBR.write(SBR);
}

deltaY=analogRead(JMy);
if (deltaY>700) {
Serial.println("Accelerate");
Y=Y+(deltaY-511)/512;
SBL=map(Y,0,18,0,180);
SBR=map(Y,0,18,0,180);
servoBL.write(SBL);
servoBR.write(SBR);
}
if (deltaY<300) {
Serial.println("Decelerate");
Y=Y-(512-deltaY)/512;
SBL=map(Y,0,18,0,180);
SBR=map(Y,0,18,0,180);
servoBL.write(SBL);
servoBR.write(SBR);
}

if (digitalRead(JTs) == LOW) {
Serial.println("Landing sequence initiated");
W=b;
Z=b;
SFL=10*b;
servoFL.write(SFL);
SFR=10*b;
servoFR.write(SFR);
SBL=10*b;
servoBL.write(SBL);
SBR=10*b;
servoBR.write(SBR);
digitalWrite(UStrigPin, LOW);
delayMicroseconds(2);
digitalWrite(UStrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(UStrigPin, LOW);
duration = pulseIn(USechoPin, HIGH);
distance = (duration / 2) * 0.0343;
if (distance <= 300 && distance >= 10) {
Serial.print("Landing distance = ");
Serial.print(distance);
Serial.println(" cm");
delay(500);
}
if (distance < 10 && distance >= 2) {
Serial.println("Safe to land");
pitch = 1000; // 1000 Hz sound
Sound ();
}
if (distance > 300 || distance < 2) {
Serial.println("Ultrasonic sensor is out of range");
}
}

if (digitalRead(JMs) == LOW) {
Serial.println("Hovering");
X=a;
Y=a;
SFL=10*a;
servoFL.write(SFL);
SFR=10*a;
servoFR.write(SFR);
SBL=10*a;
servoBL.write(SBL);
SBR=10*a;
servoBR.write(SBR);
pitch = 500; // 500 Hz sound
Sound ();
}
delay(100);
}

int Sound () {
tone(Buzzer, pitch);
delay(500);
noTone(Buzzer);
delay(500);
}

void calibrate_mpu6050() {
Wire.begin();
Serial.print("The MPU6050 unit is calibrating, please wait .....");
setup_mpu6050_registers();
for (int cal_int = 0; cal_int < 1000 ; cal_int ++) {
read_mpu6050_data();
gyro_x_cal += gyro_x;
gyro_y_cal += gyro_y;
gyro_z_cal += gyro_z;
temp_cal += temp;
acc_x_cal += acc_x;
acc_y_cal += acc_y;
acc_z_cal += acc_z;
delay(3);
}
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
temp_cal /= 1000;
acc_x_cal /= 1000;
acc_y_cal /= 1000;
acc_z_cal /= 1000;
}

void setup_mpu6050_registers() {
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
}

void read_mpu6050_data() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14);
while(Wire.available() < 14);
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temp = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}

void display_mpu6050_data() {
read_mpu6050_data();
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
temp -= temp_cal;
acc_x -= acc_x_cal;
acc_y -= acc_y_cal;
acc_z -= acc_z_cal;
// Conversion of unit
gyro_x_val = gyro_x * 0.01526717557;
gyro_y_val = gyro_y * 0.01526717557;
gyro_z_val = gyro_z * 0.01526717557;
temp_val = temp * 0.002941176471;
acc_x_val = acc_x * 0.00239501953125;
acc_y_val = acc_y * 0.00239501953125;
acc_z_val = (acc_z - 4096) * 0.00239501953125;
Serial.print("Gyro_x (degree/s) = ");
Serial.println(gyro_x_val);
Serial.print("Gyro_y (degree/s) = ");
Serial.println(gyro_y_val);
Serial.print("Gyro_z (degree/s) = ");
Serial.println(gyro_z_val);
Serial.print("Temperature variation (C) = ");
Serial.println(temp_val);
Serial.print("Acceleration_x (m/s^2) = ");
Serial.println(acc_x_val);
Serial.print("Acceleration_y (m/s^2) = ");
Serial.println(acc_y_val);
Serial.print("Acceleration_z (m/s^2) = ");
Serial.println(acc_z_val);
delay(1000);
}