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

#define Pin_Control_LR 32
#define Pin_Control_FB 33
#define Pin_Control_FBLR 27
#define Pin_Control_SP 34
#define Pin_Control_UD 35
#define Pin_Control_UDSP 26
#define Pin_US_Trig 23
#define Pin_US_Echo 19
#define Pin_Buzzer 4
#define Pin_Button 13
#define Pin_Servo1 15
#define Pin_Servo2 5
#define Pin_Servo3 18
#define Pin_Servo4 2

int S1 = 0; // Servo1 or motor1 IO  
int S2 = 0; // Servo2 or motor2 IO
int S3 = 0; // Servo3 or motor3 IO
int S4 = 0; // Servo4 or motor4 IO
int dW = 0; // Joystick ascend control
int dX = 0; // Joystick turn control
int dY = 0; // Joystick accelerate control
int dZ = 0; // Joystick spin control

int C1 = 4096; // Joystick IO high (ESP32 4096, nano 1024)
int C2 = 2048; // Joystick IO medium
int C3 = 3000; // Joystick IO upper limit
int C4 = 1000; // Joystick IO lower limit
int C5 = 40; // Motor maximum speed setting
int C6 = 36; // Motor maximum speed setting
int C7 = 3; // Setting to hover
int C8 = 2; // Setting to land
int C9 = 180; // Motor maximum signal

float duration, distance;
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;

Servo Servo1;
Servo Servo2;
Servo Servo3;
Servo Servo4;

void IRAM_ATTR PulseW() {
read_joystick_W();
}

void IRAM_ATTR PulseX() {
read_joystick_X();
}

void IRAM_ATTR PulseY() {
read_joystick_Y();
}

void IRAM_ATTR PulseZ() {
read_joystick_Z();
}

void setup() {
Serial.begin(9600);
Serial.println("Welcome pilot");
pinMode(Pin_Control_LR, INPUT_PULLUP);
pinMode(Pin_Control_FB, INPUT_PULLUP);
pinMode(Pin_Control_FBLR, INPUT_PULLUP);
pinMode(Pin_Control_SP, INPUT_PULLUP);
pinMode(Pin_Control_UD, INPUT_PULLUP);
pinMode(Pin_Control_UDSP, INPUT_PULLUP);
pinMode(Pin_US_Trig, OUTPUT);
pinMode(Pin_US_Echo, INPUT);
pinMode(Pin_Buzzer, OUTPUT);
pinMode(Pin_Button, INPUT);
attachInterrupt(digitalPinToInterrupt(Pin_Control_LR),PulseX,CHANGE);
attachInterrupt(digitalPinToInterrupt(Pin_Control_FB),PulseY,CHANGE);
attachInterrupt(digitalPinToInterrupt(Pin_Control_SP),PulseW,CHANGE);
attachInterrupt(digitalPinToInterrupt(Pin_Control_UD),PulseZ,CHANGE);
Servo1.attach(Pin_Servo1);
Servo2.attach(Pin_Servo2);
Servo3.attach(Pin_Servo3);
Servo4.attach(Pin_Servo4);
write_servo_value();
calibrate_mpu6050();
Serial.println("...");
Serial.println("EDF drone is ready to fly");
}

void loop() {
gonoff = digitalRead(Pin_Button);
if (gonoff == HIGH) {
Serial.println("Automated flight control initiated");
display_mpu6050_data();
}
read_joystick_T();
read_joystick_P();
delay(200);
}

int Sound () {
tone(Pin_Buzzer, 500); // 500 Hz sound
delay(1000);
noTone(Pin_Buzzer);
}

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_W() {
dW = analogRead(Pin_Control_SP);
if (dW > C3) {
Serial.println("Spin Right");
S1 = S1 + C5*(dW - C2)/C2;
S3 = S3 + C5*(dW - C2)/C2;
write_servo_value();
delay(1000); // Sensitivity of right spin
S1 = S1 - C5*(dW - C2)/C2;
S3 = S3 - C5*(dW - C2)/C2;
write_servo_value();
}
if (dW < C4) {
Serial.println("Spin Left");
S2 = S2 + C5*(C2 - dW)/C2;
S4 = S4 + C5*(C2 - dW)/C2;
write_servo_value();
delay(1000); // Sensitivity of left spin
S2 = S2 - C5*(C2 - dW)/C2;
S4 = S4 - C5*(C2 - dW)/C2;
write_servo_value();
}
}

void read_joystick_Z() {
dZ = analogRead(Pin_Control_UD);
if (dZ > C3) {
Serial.println("Ascend");
S1 = S1 + C9/C6*(dZ + 1 - C2)/C2;
S2 = S2 + C9/C6*(dZ + 1 - C2)/C2;
S3 = S3 + C9/C6*(dZ + 1 - C2)/C2;
S4 = S4 + C9/C6*(dZ + 1 - C2)/C2;
write_servo_value();
}
if (dZ < C4) {
Serial.println("Descend");
S1 = S1 - C9/C6*(C2 - dZ)/C2;
S2 = S2 - C9/C6*(C2 - dZ)/C2;
S3 = S3 - C9/C6*(C2 - dZ)/C2;
S4 = S4 - C9/C6*(C2 - dZ)/C2;
write_servo_value();
}
}

void read_joystick_X() {
dX = analogRead(Pin_Control_LR);
if (dX > C3) {
Serial.println("Right");
S2 = S2 + C5*(dX - C2)/C2;
S3 = S3 + C5*(dX - C2)/C2;
write_servo_value();
delay(1000); // Sensitivity of right turn
S2 = S2 - C5*(dX - C2)/C2;
S3 = S3 - C5*(dX - C2)/C2;
write_servo_value();
}
if (dX < C4) {
Serial.println("Left");
S1 = S1 + C5*(C2 - dX)/C2;
S4 = S4 + C5*(C2 - dX)/C2;
write_servo_value();
delay(1000); // Sensitivity of left turn
S1 = S1 - C5*(C2 - dX)/C2;
S4 = S4 - C5*(C2 - dX)/C2;
write_servo_value();
}
}

void read_joystick_Y() {
dY = analogRead(Pin_Control_FB);
if (dY > C3) {
Serial.println("Accelerate");
S3 = S3 + C9/C6*(dY + 1 - C2)/C2;
S4 = S4 + C9/C6*(dY + 1 - C2)/C2;
write_servo_value();
}
if (dY < C4) {
Serial.println("Decelerate");
S3 = S3 - C9/C6*(C2-dY)/C2;
S4 = S4 - C9/C6*(C2-dY)/C2;
write_servo_value();
}
}

void read_joystick_T() {
if (digitalRead(Pin_Control_UDSP) == LOW) {
Serial.println("Landing sequence initiated");
S2 = C5*C8;
S1 = C5*C8;
S3 = C5*C8;
S4 = C5*C8;
write_servo_value();
digitalWrite(Pin_US_Trig, LOW);
delayMicroseconds(2);
digitalWrite(Pin_US_Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Pin_US_Trig, LOW);
duration = pulseIn(Pin_US_Echo, 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");
Sound ();
}
if (distance > 300 || distance < 2) {
Serial.println("Ultrasonic sensor is out of range");
}
}
}

void read_joystick_P() {
if (digitalRead(Pin_Control_FBLR) == LOW) {
Serial.println("Hovering");
S2 = C5*C7;
S1 = C5*C7;
S3 = C5*C7;
S4 = C5*C7;
write_servo_value();
}
}

void write_servo_value() {
Servo1.write(S1);
Servo2.write(S2);
Servo3.write(S3);
Servo4.write(S4);
}