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

#define JTx A3
#define JTy A2
#define JTs 8
#define JMx A1
#define JMy A0
#define JMs 7
#define UStrigPin 11
#define USechoPin 10
#define Buzzer 12
#define Button 4

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

// ESP32 use 4096 as high while nano use 1024
int JSAH=1024; // Joystick analog input high
int JSAM=512; // Joystick analog input medium
int JSAU=700; // Joystick analog input upper limit
int JSAL=300; // Joystick analog input lower limit
int JSAA=10; // Joystick analog input amplifier
int JSMA=9; // Joystick map ascend and accelerate

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(9);
servoFR.attach(5);
servoBL.attach(6);
servoBR.attach(3);
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();
}
read_joystick_throttle_X();
read_joystick_throttle_Y();
read_joystick_motion_X();
read_joystick_motion_Y();
read_joystick_throttle_S();
read_joystick_motion_S();
}

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

void read_joystick_throttle_X() {
deltaW=analogRead(JTx);
if (deltaW>JSAU) {
Serial.println("Spin Right");
W=W+(deltaW-JSAM)/JSAM;
SFR=SFR+JSAA*W;
SBL=SBL+JSAA*W;
servoFR.write(SFR);
servoBL.write(SBL);
delay(1000); // Sensitivity of right spin
SFR=SFR-JSAA*W;
SBL=SBL-JSAA*W;
servoFR.write(SFR);
servoBL.write(SBL);
}
if (deltaW<JSAL) {
Serial.println("Spin Left");
W=W+(JSAM-deltaW)/JSAM;
SFL=SFL+JSAA*W;
SBR=SBR+JSAA*W;
servoFL.write(SFL);
servoBR.write(SBR);
delay(1000); // Sensitivity of left spin
SFL=SFL-JSAA*W;
SBR=SBR-JSAA*W;
servoFL.write(SFL);
servoBR.write(SBR);
}
}

void read_joystick_throttle_Y() {
deltaZ=analogRead(JTy);
if (deltaZ>JSAU) {
Serial.println("Ascend");
Z=Z+(deltaZ+1-JSAM)/JSAM;
SFL=map(Z,0,JSMA,0,180);
SFR=map(Z,0,JSMA,0,180);
SBL=map(Z,0,JSMA,0,180);
SBR=map(Z,0,JSMA,0,180);
servoFL.write(SFL);
servoFR.write(SFR);
servoBL.write(SBL);
servoBR.write(SBR);
}
if (deltaZ<JSAL) {
Serial.println("Descend");
Z=Z-(JSAM-deltaZ)/JSAM;
SFL=map(Z,0,JSMA,0,180);
SFR=map(Z,0,JSMA,0,180);
SBL=map(Z,0,JSMA,0,180);
SBR=map(Z,0,JSMA,0,180);
servoFL.write(SFL);
servoFR.write(SFR);
servoBL.write(SBL);
servoBR.write(SBR);
}
}

void read_joystick_motion_X() {
deltaX=analogRead(JMx);
if (deltaX>JSAU) {
Serial.println("Right");
X=X+(deltaX-JSAM)/JSAM;
SFL=SFL+JSAA*X;
SBL=SBL+JSAA*X;
servoFL.write(SFL);
servoBL.write(SBL);
delay(1000); // Sensitivity of right turn
SFL=SFL-JSAA*X;
SBL=SBL-JSAA*X;
servoFL.write(SFL);
servoBL.write(SBL);
}
if (deltaX<JSAL) {
Serial.println("Left");
X=X+(JSAM-deltaX)/JSAM;
SFR=SFR+JSAA*X;
SBR=SBR+JSAA*X;
servoFR.write(SFR);
servoBR.write(SBR);
delay(1000); // Sensitivity of left turn
SFR=SFR-JSAA*X;
SBR=SBR-JSAA*X;
servoFR.write(SFR);
servoBR.write(SBR);
}
}

void read_joystick_motion_Y() {
deltaY=analogRead(JMy);
if (deltaY>JSAU) {
Serial.println("Accelerate");
Y=Y+(deltaY+1-JSAM)/JSAM;
SBL=map(Y,0,JSMA,0,180);
SBR=map(Y,0,JSMA,0,180);
servoBL.write(SBL);
servoBR.write(SBR);
}
if (deltaY<JSAL) {
Serial.println("Decelerate");
Y=Y-(JSAM-deltaY)/JSAM;
SBL=map(Y,0,JSMA,0,180);
SBR=map(Y,0,JSMA,0,180);
servoBL.write(SBL);
servoBR.write(SBR);
}
}

void read_joystick_throttle_S() {
if (digitalRead(JTs) == LOW) {
Serial.println("Landing sequence initiated");
W=b;
Z=b;
SFL=JSAA*b;
servoFL.write(SFL);
SFR=JSAA*b;
servoFR.write(SFR);
SBL=JSAA*b;
servoBL.write(SBL);
SBR=JSAA*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");
}
}
}

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