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

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

// 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, 0, 0, 0, 2);}

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

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

void OLED_print(int type, String text, int value, int time, int col, int row, int size) {
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();
delay(time);}

void I2C_start() {
OLED_print(5, "I2C scanning ...", 0, 2000, 0, 0, 2);
for (byte j = 8; j < 120; j++) {
Wire.beginTransmission (j);
if (Wire.endTransmission () == 0) {
OLED_print(5, "Addr:", 0, 0, 0, 0, 2);
OLED_print(1, "", j, 0, 60, 0, 2);
OLED_print(4, "0x", 0, 0, 0, 25, 2);
OLED_print(2, "", j, 2000, 25, 25, 2);}}
OLED_print(6, "", 0, 0, 0, 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[#], freq[Hz], res[bit], channel[#])
/*
ledcAttach(Pin_SERVO1_PWM, 50, 13);
ledcAttach(Pin_SERVO2_PWM, 50, 13);
ledcAttach(Pin_SERVO3_PWM, 50, 13);
ledcAttach(Pin_SERVO4_PWM, 50, 13);
*/
ledcAttachChannel(Pin_SERVO1_PWM, 50, 13, 3);
ledcAttachChannel(Pin_SERVO2_PWM, 50, 13, 4);
ledcAttachChannel(Pin_SERVO3_PWM, 50, 13, 5);
ledcAttachChannel(Pin_SERVO4_PWM, 50, 13, 6);}

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); // remove
Data[2][8] = analogRead(Pin_RC2_Read); // remove
Data[2][9] = analogRead(Pin_RC3_Read); // remove
Data[2][10] = analogRead(Pin_RC4_Read); // remove
Data[2][11] = analogRead(Pin_RC5_Read); // remove
Data[2][12] = analogRead(Pin_RC6_Read); // remove
Data[2][1] = map(Data[2][7], 0, 4095, 1000, 2000); // remove
Data[2][2] = map(Data[2][8], 0, 4095, 1000, 2000); // remove
Data[2][3] = map(Data[2][9], 0, 4095, 1000, 2000); // remove
Data[2][4] = map(Data[2][10], 0, 4095, 1000, 2000); // remove
Data[2][5] = map(Data[2][11], 0, 4095, 1000, 2000); // remove
Data[2][6] = map(Data[2][12], 0, 4095, 1000, 2000);} // remove

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

void BUZZER_write(int j) {
// ledcAttachChannel(Pin_BUZZER, j, 13, 7);
// ledcWriteChannel(6, 4095);
tone(Pin_BUZZER, j, 2000); // j Hz sound
noTone(Pin_BUZZER);
delay(500);}

void HCSR04_read() {
float duration, distance;
OLED_print(5, "Landing sequence initiated", 0, 1000, 0, 0, 2);
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, 0, 0, 0, 2);
OLED_print(1, "", distance, 0, 0, 25, 2);
OLED_print(4, "cm", 0, 1000, 50, 25, 2);
OLED_print(6, "", 0, 0, 0, 0, 0);}
if (distance < 10 && distance >= 2) {
LED_set(6);
OLED_print(5, "Safe to land", 0, 1000, 0, 0, 2);
BUZZER_write(250);}
if (distance > 300 || distance < 2) {
LED_set(7);
OLED_print(5, "Ultrasonic sensor is out of range", 0, 1000, 0, 0, 2);
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() {
LED_set(2);
OLED_print(3, "", Data[1][11], 0, 0, 0, 2);
OLED_print(1, "", Data[1][12], 0, 0, 25, 2);
OLED_print(1, "", Data[1][13], 0, 0, 50, 2);
OLED_print(1, "", Data[1][8], 0, 60, 0, 2);
OLED_print(1, "", Data[1][9], 0, 60, 25, 2);
OLED_print(1, "", Data[1][10], 100, 60, 50, 2);}