#include <Wire.h>
#include <Adafruit_SSD1306.h>

#define Pin_RC1_Read 33
#define Pin_RC2_Read 32
#define Pin_RC3_Read 26
#define Pin_RC4_Read 25
#define Pin_RC5_Read 34
#define Pin_RC6_Read 35
#define Pin_SERVO1_PWM 15
#define Pin_SERVO2_PWM 4
#define Pin_SERVO3_PWM 2
#define Pin_SERVO4_PWM 18
#define Pin_HCSR04_Trig 23
#define Pin_HCSR04_Echo 19
#define Pin_BUZZER 27
#define Pin_BUTTON 5
#define Pin_LED_Red 13
#define Pin_LED_Green 12
#define Pin_LED_Blue 14

// Speed of ultrasonic wave: Set[0][2]
// MPU6050: Set[1][1] to Set[1][14]
// Servo control: Set[2][2] to Set[2][3]
// HX711: Set[3][1] to Set[3][4]
// LDR: Set[3][5] to Set[3][7]
// VOLT: Set[3][8] to Set[3][11]
// AMP: Set[3][12] to Set[3][15]
int Bit[8] = {0, 0, 0, 0, 0, 0, 0, 0};
float Set[5][16] = {
{0, 0, 343, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 15.27, 15.27, 15.27, 2.394, 2.394, 2.394, 0, 0},
{0, 0, 0.3, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 2100, 0, 5000, 0, 0, 0, 0, 4095, 1000, 2000, 0, 4095, 1000, 2000},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
float Data[5][16] = {
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};

Adafruit_SSD1306 OLED(128, 64, &Wire, -1);

void setup() {
Wire.begin();
Serial.begin(9600);
PIN_set();
LED_set(1);
OLED_start();
I2C_start();
I2C_set();
LEDC_set();
MODE_set();
LED_set(3);
OLED_print(5, "EDF drone is ready to fly", 0, 2, 0, 0);}

void loop() {
RC_read();
DATA_process();
SERVO_write();}

void PIN_set() {
pinMode(Pin_RC1_Read, INPUT);
pinMode(Pin_RC2_Read, INPUT);
pinMode(Pin_RC3_Read, INPUT);
pinMode(Pin_RC4_Read, INPUT);
pinMode(Pin_RC5_Read, INPUT);
pinMode(Pin_RC6_Read, INPUT);
pinMode(Pin_SERVO1_PWM, OUTPUT);
pinMode(Pin_SERVO2_PWM, OUTPUT);
pinMode(Pin_SERVO3_PWM, OUTPUT);
pinMode(Pin_SERVO4_PWM, OUTPUT);
pinMode(Pin_HCSR04_Trig, OUTPUT);
pinMode(Pin_HCSR04_Echo, INPUT);
pinMode(Pin_BUZZER, OUTPUT);
pinMode(Pin_BUTTON, INPUT);}

void MODE_set() {
LED_set(1);
for (int j = 0; j <= 3; j++) {
OLED_print(5, "Press button in ..", 0, 2, 0, 0);
OLED_print(1, "", 3 - j, 2, 75, 50);
delay(1000);}
Bit[0] = digitalRead(Pin_BUTTON);
OLED_print(6, "", 0, 0, 0, 0);
if (Bit[0] == HIGH) {
LED_set(2);
while (Bit[0] == HIGH) {
MPU6050_read();
Bit[0] = digitalRead(Pin_BUTTON);
delay(1000);}
OLED_print(5, "Calibration completed", 0, 2, 0, 0);
delay(1000);}
OLED_print(5, "Push max. throttle for auto mode", 0, 2, 0, 0);
delay(3000);
// Data[2][3] = pulseIn(Pin_RC3_Read, HIGH);
Data[2][9] = analogRead(Pin_RC3_Read);
Data[2][3] = map(Data[2][9], 0, 4095, 1000, 2000);
Set[2][1] = Data[2][3];
if (Set[2][1] > 1800) {
LED_set(3);
OLED_print(5, "Automated control initiated", 0, 2, 0, 0);
delay(1500);}
else {
LED_set(4);
OLED_print(5, "Manual control mode", 0, 2, 0, 0);
delay(1500);}}

void OLED_start() {
OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C);
OLED_print(5, "Welcome pilot", 0, 2, 0, 0);
delay(2000);
OLED_print(6, "", 0, 0, 0, 0);}

void OLED_print(int type, String text, int value, int size, int col, int row) {
OLED.setTextColor(WHITE);
OLED.setTextSize(size);
OLED.setCursor(col, row);
switch (type) {
case 1: OLED.println(value, DEC); break;
case 2: OLED.println(value, HEX); break;
case 3: OLED.clearDisplay(); OLED.println(value); break;
case 4: OLED.println(text); break;
case 5: OLED.clearDisplay(); OLED.println(text); break;
case 6: OLED.clearDisplay(); break;}
OLED.display();}

void I2C_start() {
OLED_print(5, "I2C scanning ...", 0, 2, 0, 0);
delay(2000);
for (byte j = 8; j < 120; j++) {
Wire.beginTransmission (j);
if (Wire.endTransmission () == 0) {
OLED_print(5, "Addr:", 0, 2, 0, 0);
OLED_print(1, "", j, 2, 60, 0);
OLED_print(4, "0x", 0, 2, 0, 25);
OLED_print(2, "", j, 2, 25, 25);
delay(2000);}}
OLED_print(6, "", 0, 1, 0, 0);}

void I2C_set() {
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 LEDC_set() {
// ledcAttach(pin[#], freq[Hz], res[bit])
ledcAttach(Pin_SERVO1_PWM, 50, 13);
ledcAttach(Pin_SERVO2_PWM, 50, 13);
ledcAttach(Pin_SERVO3_PWM, 50, 13);
ledcAttach(Pin_SERVO4_PWM, 50, 13);}

void RC_read() {
/*
Data[2][1] = pulseIn(Pin_RC1_Read, HIGH);
Data[2][2] = pulseIn(Pin_RC2_Read, HIGH);
Data[2][3] = pulseIn(Pin_RC3_Read, HIGH);
Data[2][4] = pulseIn(Pin_RC4_Read, HIGH);
Data[2][5] = pulseIn(Pin_RC5_Read, HIGH);
Data[2][6] = pulseIn(Pin_RC6_Read, HIGH);
*/
Data[2][7] = analogRead(Pin_RC1_Read);
Data[2][8] = analogRead(Pin_RC2_Read);
Data[2][9] = analogRead(Pin_RC3_Read);
Data[2][10] = analogRead(Pin_RC4_Read);
Data[2][11] = analogRead(Pin_RC5_Read);
Data[2][12] = analogRead(Pin_RC6_Read);
Data[2][1] = map(Data[2][7], 0, 4095, 1000, 2000);
Data[2][2] = map(Data[2][8], 0, 4095, 1000, 2000);
Data[2][3] = map(Data[2][9], 0, 4095, 1000, 2000);
Data[2][4] = map(Data[2][10], 0, 4095, 1000, 2000);
Data[2][5] = map(Data[2][11], 0, 4095, 1000, 2000);
Data[2][6] = map(Data[2][12], 0, 4095, 1000, 2000);}

void DATA_process() {
if (Data[2][3] < 1300) {HCSR04_read();}
if (Set[2][1] > 1800) {LED_set(3);}
else {
LED_set(4);
Data[4][5] = Data[2][3] + Set[2][2]*(1500 - Data[2][1]) + Set[2][3]*(1500 - Data[2][2]);
Data[4][6] = Data[2][3] + Set[2][2]*(1500 - Data[2][1]) + Set[2][3]*(Data[2][2] - 1500);
Data[4][7] = Data[2][3] + Set[2][2]*(Data[2][1] - 1500) + Set[2][3]*(1500 - Data[2][2]);
Data[4][8] = Data[2][3] + Set[2][2]*(Data[2][1] - 1500) + Set[2][3]*(Data[2][2] - 1500);}}

void SERVO_write() {
Data[4][1] = map(Data[4][5], 0, 20000, 0, 8191);
Data[4][2] = map(Data[4][6], 0, 20000, 0, 8191);
Data[4][3] = map(Data[4][7], 0, 20000, 0, 8191);
Data[4][4] = map(Data[4][8], 0, 20000, 0, 8191);
ledcWrite(Pin_SERVO1_PWM, Data[4][1]);
ledcWrite(Pin_SERVO2_PWM, Data[4][2]);
ledcWrite(Pin_SERVO3_PWM, Data[4][3]);
ledcWrite(Pin_SERVO4_PWM, Data[4][4]);}

void LED_set(int j) {
switch (j) {
case 1: LED_show(0, 255, 255); break;
case 2: LED_show(0, 0, 255); break;
case 3: LED_show(255, 0, 255); break;
case 4: LED_show(255, 255, 0); break;
case 5: LED_show(0, 255, 0); break;
case 6: LED_show(255, 0, 0); break;
case 7: LED_show(128, 255, 0); break;
case 8: LED_show(0, 255, 128); break;
case 9: LED_show(255, 128, 0); break;
case 10: LED_show(255, 0, 128); break;
case 11: LED_show(0, 128, 255); break;
case 12: LED_show(128, 0, 255); break;}}

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

int BUZZER_write(int j) {
tone(Pin_BUZZER, j, 3000); // j Hz sound for 3 seconds
noTone(Pin_BUZZER);}

void HCSR04_read() {
float duration, distance;
OLED_print(5, "Landing sequence initiated", 0, 2, 0, 0);
delay(1000);
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 = 100*(duration/1000000)*Set[0][2]/2;
if (distance <= 300 && distance >= 10) {
LED_set(5);
OLED_print(5, "Distance =", 0, 2, 0, 0);
OLED_print(1, "", distance, 2, 0, 25);
OLED_print(4, "cm", 0, 2, 50, 25);
delay(1000);
OLED_print(6, "", 0, 1, 0, 0);}
if (distance < 10 && distance >= 2) {
LED_set(6);
OLED_print(5, "Safe to land", 0, 2, 0, 0);
BUZZER_write(250);}
if (distance > 300 || distance < 2) {
LED_set(7);
OLED_print(5, "Ultrasonic sensor is out of range", 0, 2, 0, 0);
BUZZER_write(1000);}}

void MPU6050_read() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14);
while(Wire.available() < 14);
Data[1][4] = Wire.read()<<8|Wire.read();
Data[1][5] = Wire.read()<<8|Wire.read();
Data[1][6] = Wire.read()<<8|Wire.read();
Data[1][7] = Wire.read()<<8|Wire.read();
Data[1][1] = Wire.read()<<8|Wire.read();
Data[1][2] = Wire.read()<<8|Wire.read();
Data[1][3] = Wire.read()<<8|Wire.read();
Data[1][8] = (Data[1][1] - Set[1][1]) * Set[1][8]/1000;
Data[1][9] = (Data[1][2] - Set[1][2]) * Set[1][9]/1000;
Data[1][10] = (Data[1][3] - Set[1][3]) * Set[1][10]/1000;
Data[1][11] = (Data[1][4] - Set[1][4]) * Set[1][11]/1000;
Data[1][12] = (Data[1][5] - Set[1][5]) * Set[1][12]/1000;
Data[1][13] = (Data[1][6] - Set[1][6]) * Set[1][13]/1000;
Data[1][14] = (Data[1][7] - Set[1][7]) * Set[1][14]/1000;
MPU6050_show();}

void MPU6050_show() {
OLED_print(3, "", Data[1][11], 2, 0, 0);
OLED_print(1, "", Data[1][12], 2, 0, 25);
OLED_print(1, "", Data[1][13], 2, 0, 50);
OLED_print(1, "", Data[1][8], 2, 60, 0);
OLED_print(1, "", Data[1][9], 2, 60, 25);
OLED_print(1, "", Data[1][10], 2, 60, 50);
delay(100);}