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

#define Pin_JOYSTICK_X 26
#define Pin_JOYSTICK_Y 27
#define Pin_JOYSTICK_W 33
#define Pin_JOYSTICK_Z 25
#define Pin_KNOB_D 35
#define Pin_KNOB_H 32
#define Pin_HCSR04_Trig 19
#define Pin_HCSR04_Echo 34
#define Pin_BUZZER 23
#define Pin_BUTTON 5
#define Pin_SERVO_PWM1 4
#define Pin_SERVO_PWM2 15
#define Pin_SERVO_PWM3 2
#define Pin_SERVO_PWM4 18
#define Pin_LED_Red 13
#define Pin_LED_Green 12
#define Pin_LED_Blue 14

// Defination of several constants:
const float C[2][10] = {
{0, 36, 180, 3000, 1000, 8191, 4095, 2047, 1023, 255},
{0, 9.8067, 16383, 250, 32767, 37.2, 228, 85, 16480, 0.0343}};

int Bit[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int Speed[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Servo or motor IO
float Gyro[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

Servo SERVO1, SERVO2, SERVO3, SERVO4;

void setup() {
pinMode(Pin_HCSR04_Trig, OUTPUT);
pinMode(Pin_HCSR04_Echo, INPUT);
pinMode(Pin_BUZZER, OUTPUT);
pinMode(Pin_BUTTON, INPUT);
SERVO1.attach(Pin_SERVO_PWM1);
SERVO2.attach(Pin_SERVO_PWM2);
SERVO3.attach(Pin_SERVO_PWM3);
SERVO4.attach(Pin_SERVO_PWM4);
Wire.begin();
Serial.begin(9600);
Serial.println("Welcome pilot");
get_KNOB_D();
Serial.println("EDF drone is ready to fly");
Bit[0] = digitalRead(Pin_BUTTON);
if (Bit[0] == HIGH) {
Serial.println("Automated flight control initiated");
}}

void loop() {
Bit[0] = digitalRead(Pin_BUTTON);
if (Bit[0] == HIGH) {
read_MPU6050_data();
print_MPU6050_data();
}
get_KNOB_H();
get_JOYSTICK_Y();
get_JOYSTICK_X();
get_JOYSTICK_W();
get_JOYSTICK_Z();
delay(200);
}

int write_BUZZER_sound() {
tone(Pin_BUZZER, 500); // 500 Hz sound
delay(1000);
noTone(Pin_BUZZER);
}

void set_MPU6050_register() {
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);
Gyro[1][0] = Wire.read()<<8|Wire.read();
Gyro[1][1] = Wire.read()<<8|Wire.read();
Gyro[1][2] = Wire.read()<<8|Wire.read();
Gyro[2][0] = Wire.read()<<8|Wire.read();
Gyro[0][0] = Wire.read()<<8|Wire.read();
Gyro[0][1] = Wire.read()<<8|Wire.read();
Gyro[0][2] = Wire.read()<<8|Wire.read();
Gyro[1][0] *= C[1][1]/C[1][2];
Gyro[1][1] *= C[1][1]/C[1][2];
Gyro[1][2] *= C[1][1]/C[1][2];
Gyro[2][0] = (Gyro[2][0] - C[1][6])*(C[1][7] - C[1][5])/(C[1][8] - C[1][6]) + C[1][5];
Gyro[0][0] *= C[1][3]/C[1][4];
Gyro[0][1] *= C[1][3]/C[1][4];
Gyro[0][2] *= C[1][3]/C[1][4];
}

void print_MPU6050_data() {
Serial.print("Gyro_x (°/s)");
Serial.print("\t");
Serial.print("Gyro_y (°/s)");
Serial.print("\t");
Serial.print("Gyro_z (°/s)");
Serial.print("\t");
Serial.print("Acce_x (m/s/s)");
Serial.print("\t");
Serial.print("Acce_y (m/s/s)");
Serial.print("\t");
Serial.print("Acce_z (m/s/s)");
Serial.print("\t");
Serial.println("Temperature (°C)");
Serial.print(Gyro[0][0]);
Serial.print("\t\t");
Serial.print(Gyro[0][1]);
Serial.print("\t\t");
Serial.print(Gyro[0][2]);
Serial.print("\t\t");
Serial.print(Gyro[1][0]);
Serial.print("\t\t");
Serial.print(Gyro[1][1]);
Serial.print("\t\t");
Serial.print(Gyro[1][2]);
Serial.print("\t\t");
Serial.println(Gyro[2][0]);
delay(1000);
}

void get_KNOB_D() {
int dD = analogRead(Pin_KNOB_D);
}

void get_KNOB_H() {
int dH = analogRead(Pin_KNOB_H);
if (dH < C[0][4]) {read_HCSR04_data();}
Speed[1] = map(dH, 0, C[0][6], 0, C[0][2]);
Speed[2] = map(dH, 0, C[0][6], 0, C[0][2]);
Speed[9] = map(dH, 0, C[0][6], 0, C[0][2]);
Speed[10] = map(dH, 0, C[0][6], 0, C[0][2]);
}

void get_JOYSTICK_Y() {
int dY = analogRead(Pin_JOYSTICK_Y);
Speed[3] = Speed[9] + map(dY, 0, C[0][6], 0, C[0][2]);
Speed[4] = Speed[10] + map(dY, 0, C[0][6], 0, C[0][2]);
write_SERVO_data(Speed[1], Speed[2], Speed[3], Speed[4]);
}

void get_JOYSTICK_X() {
int dX = analogRead(Pin_JOYSTICK_X);
if (dX > C[0][3]) {set_LED_data(1);
Speed[5] = Speed[1] + C[0][1]; Speed[6] = Speed[2];
Speed[8] = Speed[4] + C[0][1]; Speed[7] = Speed[3];
write_SERVO_data(Speed[5], Speed[6], Speed[7], Speed[8]);
delay(1000);}
if (dX < C[0][4]) {set_LED_data(2);
Speed[6] = Speed[2] + C[0][1]; Speed[5] = Speed[1];
Speed[7] = Speed[3] + C[0][1]; Speed[8] = Speed[4];
write_SERVO_data(Speed[5], Speed[6], Speed[7], Speed[8]);
delay(1000);}
if (dX < C[0][3] & dX > C[0][4]) {set_LED_data(3);
write_SERVO_data(Speed[1], Speed[2], Speed[3], Speed[4]);
}}

void get_JOYSTICK_W() {
int dW = analogRead(Pin_JOYSTICK_W);
if (dW > C[0][3]) {set_LED_data(1);
Speed[5] = Speed[1] + C[0][1]; Speed[6] = Speed[2];
Speed[7] = Speed[3] + C[0][1]; Speed[8] = Speed[4];
write_SERVO_data(Speed[5], Speed[6], Speed[7], Speed[8]);
delay(1000);}
if (dW < C[0][4]) {set_LED_data(2);
Speed[6] = Speed[2] + C[0][1]; Speed[5] = Speed[1];
Speed[8] = Speed[4] + C[0][1]; Speed[7] = Speed[3];
write_SERVO_data(Speed[5], Speed[6], Speed[7], Speed[8]);
delay(1000);}
if (dW < C[0][3] & dW > C[0][4]) {set_LED_data(3);
write_SERVO_data(Speed[1], Speed[2], Speed[3], Speed[4]);
}}

void get_JOYSTICK_Z() {
int dZ = analogRead(Pin_JOYSTICK_Z);
if (dZ > C[0][3]) {set_LED_data(1);
Speed[5] = Speed[1] + C[0][1]; Speed[6] = Speed[2] + C[0][1];
Speed[7] = Speed[3] + C[0][1]; Speed[8] = Speed[4] + C[0][1];
write_SERVO_data(Speed[5], Speed[6], Speed[7], Speed[8]);}
if (dZ < C[0][4]) {set_LED_data(2);
Speed[5] = Speed[1] - C[0][1]; Speed[6] = Speed[2] - C[0][1];
Speed[7] = Speed[3] - C[0][1]; Speed[8] = Speed[4] - C[0][1];
write_SERVO_data(Speed[5], Speed[6], Speed[7], Speed[8]);}
if (dZ < C[0][3] & dZ > C[0][4]) {set_LED_data(3);
write_SERVO_data(Speed[1], Speed[2], Speed[3], Speed[4]);
}}

void write_SERVO_data(int S1, int S2, int S3, int S4) {
SERVO1.write(S1);
SERVO2.write(S2);
SERVO3.write(S3);
SERVO4.write(S4);
}

void set_LED_data(int j) {
switch (j) {
case 1: show_LED_data(0, 0, C[0][9]); break;
case 2: show_LED_data(0, 0, 128); break;
case 3: show_LED_data(C[0][9], 0, C[0][9]); break;
case 4: show_LED_data(128, 0, 128); break;
case 5: show_LED_data(0, C[0][9], C[0][9]); break;
case 6: show_LED_data(0, 128, 128); break;
case 7: show_LED_data(C[0][9], C[0][9], 0); break;
case 8: show_LED_data(128, 128, 0); break;
case 9: show_LED_data(0, C[0][9], 0); break;
case 10: show_LED_data(C[0][9], 0, 0); break;
case 11: show_LED_data(C[0][9], C[0][9], C[0][9]); break;
}}

void show_LED_data(int Red, int Green, int Blue) {
analogWrite(Pin_LED_Red, Red);
analogWrite(Pin_LED_Green, Green);
analogWrite(Pin_LED_Blue, Blue);
}

void read_HCSR04_data() {
float duration, distance;
Serial.println("Landing sequence initiated");
digitalWrite(Pin_HCSR04_Trig, LOW);
delayMicroseconds(2);
digitalWrite(Pin_HCSR04_Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Pin_HCSR04_Trig, LOW);
duration = pulseIn(Pin_HCSR04_Echo, HIGH);
distance = duration*C[1][9]/2;
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");
write_BUZZER_sound();
}
if (distance > 300 || distance < 2) {
Serial.println("Ultrasonic sensor is out of range");
}}