// TESTE JOYSTICK 7.0 - ESP32

#include <Wire.h>
#include <ESP32Servo.h>
#include <MPU6050_light.h>
#include "readChannel.h"
#include <DistanceSensor.h>


// Definição das conexões de entrada
#define Ch1 26
#define Ch2 25
#define Ch3 33
#define Ch4 32
#define Ch5 00 //A4
#define Ch6 00 //A5

// Definição das conexões dos motores
#define Motor1 13 // frontal direito
#define Motor2 12 // frontal esquerdo
#define Motor3 14 // traseiro direito
#define Motor4 27 // traseiro esquerdo

// Definição das conexões das luzes
#define light 00 // Lanterna
#define ledSig 13 // Led de Sinalização
#define StR 00 // Led RGB - Red
#define StG 2 // Led RGB - Green
#define StB 00 // Led RGB - Blue

// Definição das conexões dos sensores de distância
#define trigPin 00 // For the TRIG pins of the Ultrassonic Sensor.
DistanceSensor sens1(trigPin, 00); // (trig, echo);
DistanceSensor sens2(trigPin, 00); // (trig, echo);
DistanceSensor sens3(trigPin, 00); // (trig, echo);
DistanceSensor sens4(trigPin, 00); // (trig, echo);
DistanceSensor sens5(trigPin, 00); // (trig, echo);
DistanceSensor sens6(trigPin, 00); // (trig, echo);
int d1, d2, d3, d4, d5, d6;
int Hset = 0; // When 0 restarts measurement
int H0; // Initial Height 
int Hlimit = 0; // Limit Height

// Definição das conexões do MPU6050
MPU6050 mpu(Wire);

// Engines
Servo Engine1;
Servo Engine2;
Servo Engine3;
Servo Engine4;
float Mtr1Out, Mtr2Out, Mtr3Out, Mtr4Out;
int M1, M2, M3, M4;
int maxSpeed;



// Tolerances
#define tol1 100   //ReadTolerance +
#define tol2 -100  //ReadTolerance -
#define tol3 15   //AngleTolerance +
#define tol4 -15  //AngleTolerance -
#define tol5 8   //Tolerance +
#define tol6 -8  //Tolerance -
int angX, angY;
int XR, XL, YF, YB, rotH, rotAH;

// Time
long ledSigTime, ActTime, UltrsTime, ReadTime, PrintTime;

 
void setup(){
  // Configurar a comunicação serial
  Serial.begin(115200);
  Serial.println("Starting ...");
  Wire.begin();

  byte status = mpu.begin();
  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while(status!=0){ } // stop everything if could not connect to MPU6050
  
  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  // mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
  mpu.calcOffsets(); // gyro and accelero
  Serial.println("Done!\n");
  
  
  // Configurar os pinos 
  pinMode(Ch1, INPUT);
  pinMode(Ch2, INPUT);
  pinMode(Ch3, INPUT);
  pinMode(Ch4, INPUT);
  pinMode(Ch5, INPUT);
  pinMode(Ch6, INPUT);
  pinMode(StR, OUTPUT);
  pinMode(StG, OUTPUT);
  pinMode(StB, OUTPUT);
  pinMode(light, OUTPUT);
  pinMode(ledSig, OUTPUT);

  // Configurar motores
  Engine1.attach(Motor1);
  Engine2.attach(Motor2);
  Engine3.attach(Motor3);
  Engine4.attach(Motor4);

  digitalWrite(StG,1);
  delay(700);
  digitalWrite(StG,0);
  delay(700);
  digitalWrite(StG,1);
  delay(700);
  digitalWrite(StG,0);
  delay(700);
  digitalWrite(StG,1);


  Serial.println(". . .");
  Serial.println("Initialization done");
  Serial.println("The Painting Drone is Ready!");
  Serial.println(". . .");
}
 
void loop() {
  
  Read();

  if(millis() >= ledSigTime+700){
    ledSigTime = millis();
    digitalWrite(ledSig, !digitalRead(ledSig));
  }
  
  
  //ManualBasics();
  ManualController1();
  //ManualController2();

  Engine1.write(M1);
  Engine2.write(M2);
  Engine3.write(M3);
  Engine4.write(M4);
  

  if(millis() >= PrintTime+500){
    PrintTime = millis();
    Print();
  }
  
}

void Print(){
  Serial.print("Ch1: ");
  Serial.print(ch1Value);
  Serial.print(" | Ch2: ");
  Serial.print(ch2Value);
  Serial.print(" | Ch3: ");
  Serial.print(ch3Value);
  Serial.print(" | Ch4: ");
  Serial.print(ch4Value);
  Serial.print(" | Ch5: ");
  Serial.print(ch5Value);
  Serial.print(" | Ch6: ");
  Serial.println(ch6Value);
  
  Serial.print("M1 ");
  Serial.print(M1);
  Serial.print(" - M2 ");
  Serial.print(M2);
  Serial.print(" - M3 ");
  Serial.print(M3);
  Serial.print(" - M4 ");
  Serial.println(M4);
  
  Serial.print("Mtr1 ");
  Serial.print(Mtr1Out);
  Serial.print(" - Mtr2 ");
  Serial.print(Mtr2Out);
  Serial.print(" - Mtr3 ");
  Serial.print(Mtr3Out);
  Serial.print(" - Mtr4 ");
  Serial.println(Mtr4Out);
  /*
  Serial.print("Distance: ");
  Serial.print(d1);
  Serial.print("mm - ");
  Serial.print(d2);
  Serial.print("mm - ");
  Serial.print(d3);
  Serial.print("mm - ");
  Serial.print(d4);
  Serial.print("mm - ");
  Serial.print(d5);
  Serial.print("mm - ");
  Serial.print(d6);
  Serial.println("mm");
  */

  Serial.print(F("TEMPERATURE: "));Serial.println(mpu.getTemp());

  Serial.print(F("ACCELERO  X: "));Serial.print(mpu.getAccX());
  Serial.print("\tY: ");Serial.print(mpu.getAccY());
  Serial.print("\tZ: ");Serial.println(mpu.getAccZ());
  
  Serial.print(F("GYRO      X: "));Serial.print(mpu.getGyroX());
  Serial.print("\tY: ");Serial.print(mpu.getGyroY());
  Serial.print("\tZ: ");Serial.println(mpu.getGyroZ());
  
  Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX());
  Serial.print("\tY: ");Serial.println(mpu.getAccAngleY());
    
  Serial.print(F("ANGLE     X: "));Serial.print(mpu.getAngleX());
  Serial.print("\tY: ");Serial.print(mpu.getAngleY());
  Serial.print("\tZ: ");Serial.println(mpu.getAngleZ());

  Serial.println(". . .");//////////////////////////////////////////
  Serial.println();

  Serial.print("(x):");
  Serial.print(mpu.getAngleX());
  Serial.print("    (y):");
  Serial.print(mpu.getAngleY());
  Serial.print("    (z):");
  Serial.println(mpu.getAngleZ());

  Serial.print("angX:");
  Serial.print(angX);
  Serial.print("    angY:");
  Serial.println(angY);

  Serial.print("maxSpeed ");
  Serial.print(maxSpeed);
  Serial.print(" - XR ");
  Serial.print(XR);
  Serial.print(" - XL ");
  Serial.print(XL);
  Serial.print(" - YB ");
  Serial.print(YB);
  Serial.print(" - YF ");
  Serial.print(YF);
  Serial.print(" - rotH ");
  Serial.print(rotH);
  Serial.print(" - rotAH ");
  Serial.println(rotAH);
}

void Read(){
//Controller
  ch1Value = readChannel(Ch1, tol2, tol1, 0);
  ch2Value = readChannel(Ch2, tol2, tol1, 0);
  ch3Value = readChannel(Ch3, tol2, tol1, 0);
  ch4Value = readChannel(Ch4, tol2, tol1, 0);
  ch5Value = readChannel(Ch5, tol2, tol1, 0);
  ch6Value = readSwitch(Ch6, false);
  // ch6Value = readSwitch(CH6, false);

//Distance
  /*
  if(Hset = 0){
    H0 = (sens6.getCM())*10;
    delay(60);
    Hset = 1;
  }

  //Reading Ultrassonic Sensors
  if(millis() >= UltrsTime+60){
    d1 = (sens1.getCM())*10;
  }
  if(millis() >= UltrsTime+120){
    d2 = (sens2.getCM())*10;
  }
  if(millis() >= UltrsTime+180){
    d3 = (sens3.getCM())*10;
  }
  if(millis() >= UltrsTime+240){
    d4 = (sens4.getCM())*10;
  }
  if(millis() >= UltrsTime+300){
    d5 = (sens5.getCM())*10;
  }
  if(millis() >= UltrsTime+360){
    UltrsTime = millis();
    d6 = (sens6.getCM())*10 - H0;
  }

  if (d6 > 320){
    Hlimit = 300;
  }
  else if(d6 > 310 && d6 < 320){
    Hlimit = 200;
  }
  else if(d6 > 300 && d6 < 310){
    Hlimit = 100;
  }
  else{
    Hlimit = 0;
  }
  */

//Gyroscope
  mpu.update();
}

void ManualBasics(){
  
  //Direita & Esquerda
  int XL = ch1Value;
  if(ch1Value < tol6){
    XL = map(XL, tol2, tol6, 100, 0);
  }
  else{XL = 0;}
  int XR = ch1Value;
  if(ch1Value > tol5){
    XR = map(XR, tol5, tol1, 0, 100);
  }
  else{XR = 0;}

  //Frente e Tras
  int YF = ch2Value;
  if(ch2Value < tol6){
    YF = map(YF, tol2, tol6, 100, 0);
  }
  else{YF = 0;}
  int YB = ch2Value;
  if(ch2Value > tol5){
    YB = map(YB, tol5, tol1, 0, 100);
  }
  else{YB = 0;}
  
  //Rotação
  int rotAH = ch4Value;
  if(ch4Value < tol6){
    rotAH = map(rotAH, tol2, tol6, 100, 0);
  }
  else{rotAH = 0;}
  int rotH = ch4Value;
  if(ch4Value > tol5){
    rotH = map(rotH, tol5, tol1, 0, 100);
  }
  else{rotH = 0;}

  maxSpeed = map(ch3Value, -100, 100, 0, 300);

  Mtr1Out = maxSpeed - XL - YB - rotAH;
  Mtr2Out = maxSpeed - XR - YB - rotH;
  Mtr3Out = maxSpeed - XR - YF - rotAH;
  Mtr4Out = maxSpeed - XL - YF - rotH;

  M1 = Mtr1Out;
  M1 = map(M1, 0, 300, 0, 180);
  M2 = Mtr2Out;
  M2 = map(M2, 0, 300, 0, 180);
  M3 = Mtr3Out;
  M3 = map(M3, 0, 300, 0, 180);
  M4 = Mtr4Out;
  M4 = map(M4, 0, 300, 0, 180);
}

void ManualController1(){

  angX = 0;
  if(ch1Value > tol5 || ch1Value < tol6){
  angX = map(ch1Value, tol2, 100, tol4, tol3);
  }
  else{angX = 0;}
  
  angY = 0;
  if(ch2Value > tol5 || ch2Value < tol6){
  angY = map(ch2Value, tol2, 100, tol4, tol3);
  }
  else{angY = 0;}


  //Direita & Esquerda
  int XL = 0;
  if(mpu.getAngleX() < angX){
    XL = 100;
  }
  else{
    XL = 0;
  }
  int XR = 0;
  if(mpu.getAngleX() > angX){
    XR = 100;
  }
  else{
    XR = 0;
  }

  //Frente e Tras
  int YF = 0;
  if(mpu.getAngleY() < angY){
    YF = 100;
  }
  else{
    YF = 0;
  }
  int YB = 0;
  if(mpu.getAngleY() > angY){
    YB = 100;
  }
  else{
    YB = 0;
  }
  
  //Rotação
  int rotAH = ch4Value;
  if(ch4Value < tol6){
    rotAH = map(rotAH, tol2, tol6, 100, 0);
  }
  else{rotAH = 0;}
  int rotH = ch4Value;
  if(ch4Value > tol5){
    rotH = map(rotH, tol5, tol1, 0, 100);
  }
  else{rotH = 0;}
  
  maxSpeed = map(ch3Value, -80, 100, 0, 300);
  
  if(maxSpeed <= 0){
    Mtr1Out = 0;
    Mtr2Out = 0;
    Mtr3Out = 0;
    Mtr4Out = 0;
  }
  else{
    Mtr1Out = maxSpeed - XL - YB - rotAH - Hlimit;
    Mtr2Out = maxSpeed - XR - YB - rotH - Hlimit;
    Mtr3Out = maxSpeed - XR - YF - rotAH - Hlimit;
    Mtr4Out = maxSpeed - XL - YF - rotH - Hlimit;
  }
  

  M1 = Mtr1Out;
  M1 = map(M1, 0, 300, 0, 180);
  M2 = Mtr2Out;
  M2 = map(M2, 0, 300, 0, 180);
  M3 = Mtr3Out;
  M3 = map(M3, 0, 300, 0, 180);
  M4 = Mtr4Out;
  M4 = map(M4, 0, 300, 0, 180);
}

void ManualController2(){

  angX = 0;
  if(ch1Value > tol5 || ch1Value < tol6){
  angX = map(ch1Value, tol2, 100, tol4, tol3);
  }
  else{angX = 0;}
  
  angY = 0;
  if(ch2Value > tol5 || ch2Value < tol6){
  angY = map(ch2Value, tol2, 100, tol4, tol3);
  }
  else{angY = 0;}

  int tolX1 = angX + tol3;
  int tolX2 = angX - tol3;
  int tolY1 = angY + tol3;
  int tolY2 = angY - tol3;


  //Direita & Esquerda
  int XL = 0;
  if(mpu.getAngleX() > angX && mpu.getAngleX() < tolX1){
    XL = map(XL, angX, tolX1, 0, 100);
  }
  else if(mpu.getAngleX() > tolX1){
    XL = 120;
  }
  else{
    XL = 0;
  }
  int XR = 0;
  if(mpu.getAngleX() < angX && mpu.getAngleX() > tolX2){
    XR = map(XR, tolX2, angX, 100, 0);
  }
  else if(mpu.getAngleX() < tolX2){
    XR = 120;
  }
  else{
    XR = 0;
  }

  //Frente e Tras
  int YF = 0;
  if(mpu.getAngleY() > angY && mpu.getAngleY() < tolY1){
    YF = map(YF, angY, tolY1, 0, 100);
  }
  else if(mpu.getAngleY() > tolY1){
    XL = 120;
  }
  else{
    YF = 0;
  }
  int YB = 0;
  if(mpu.getAngleY() < angY && mpu.getAngleY() > tolY2){
    YB = map(YB, tolY2, angY, 100, 0);
  }
  else if(mpu.getAngleY() < tolY2){
    XR = 120;
  }
  else{
    YB = 0;
  }
  
  //Rotação
  int rotAH = ch4Value;
  if(ch4Value < tol6){
    rotAH = map(rotAH, tol2, tol6, 100, 0);
  }
  else{rotAH = 0;}
  int rotH = ch4Value;
  if(ch4Value > tol5){
    rotH = map(rotH, tol5, tol1, 0, 100);
  }
  else{rotH = 0;}
  
  maxSpeed = map(ch3Value, -80, 100, 0, 300);
  
  if(maxSpeed <= 0){
    Mtr1Out = 0;
    Mtr2Out = 0;
    Mtr3Out = 0;
    Mtr4Out = 0;
  }
  else{
    Mtr1Out = maxSpeed - XL - YB - rotAH - Hlimit;
    Mtr2Out = maxSpeed - XR - YB - rotH - Hlimit;
    Mtr3Out = maxSpeed - XR - YF - rotAH - Hlimit;
    Mtr4Out = maxSpeed - XL - YF - rotH - Hlimit;
  }
  

  M1 = Mtr1Out;
  M1 = map(M1, 0, 300, 0, 180);
  M2 = Mtr2Out;
  M2 = map(M2, 0, 300, 0, 180);
  M3 = Mtr3Out;
  M3 = map(M3, 0, 300, 0, 180);
  M4 = Mtr4Out;
  M4 = map(M4, 0, 300, 0, 180);  
}

/*
void ManualController2(){

  angX = 0;
  if(ch1Value > tol5 || ch1Value < tol6){
  angX = map(ch1Value, tol2, 100, tol4, tol3);
  }
  else{angX = 0;}
  
  angY = 0;
  if(ch2Value > tol5 || ch2Value < tol6){
  angY = map(ch2Value, tol2, 100, tol4, tol3);
  }
  else{angY = 0;}

  int tolX1 = angX + tol3;
  int tolX2 = angX - tol3;
  int tolY1 = angY + tol3;
  int tolY2 = angY - tol3;


  //Direita & Esquerda
  int XL = 0;
  if(mpu.getAngleX() > angX && mpu.getAngleX() < tolX1){
    XL = map(XL, angX, tolX1, 0, 100);
  }
  else{
    XL = 0;
  }
  int XR = 0;
  if(mpu.getAngleX() < angX && mpu.getAngleX() > tolX2){
    XR = map(XR, tolX2, angX, 100, 0);
  }
  else{
    XR = 0;
  }

  //Frente e Tras
  int YF = 0;
  if(mpu.getAngleY() > angY && mpu.getAngleY() < tolY1){
    YF = map(YF, angY, tolY1, 0, 100);
  }
  else{
    YF = 0;
  }
  int YB = 0;
  if(mpu.getAngleY() < angY && mpu.getAngleY() > tolY2){
    YB = map(YB, tolY2, angY, 100, 0);
  }
  else{
    YB = 0;
  }
  
  //Rotação
  int rotAH = ch4Value;
  if(ch4Value < tol6){
    rotAH = map(rotAH, tol2, tol6, 100, 0);
  }
  else{rotAH = 0;}
  int rotH = ch4Value;
  if(ch4Value > tol5){
    rotH = map(rotH, tol5, tol1, 0, 100);
  }
  else{rotH = 0;}
  
  maxSpeed = map(ch3Value, -80, 100, 0, 300);
  
  if(maxSpeed <= 0){
    Mtr1Out = 0;
    Mtr2Out = 0;
    Mtr3Out = 0;
    Mtr4Out = 0;
  }
  else{
    Mtr1Out = maxSpeed - XL - YB - rotAH - Hlimit;
    Mtr2Out = maxSpeed - XR - YB - rotH - Hlimit;
    Mtr3Out = maxSpeed - XR - YF - rotAH - Hlimit;
    Mtr4Out = maxSpeed - XL - YF - rotH - Hlimit;
  }
  

  M1 = Mtr1Out;
  M1 = map(M1, 0, 300, 0, 180);
  M2 = Mtr2Out;
  M2 = map(M2, 0, 300, 0, 180);
  M3 = Mtr3Out;
  M3 = map(M3, 0, 300, 0, 180);
  M4 = Mtr4Out;
  M4 = map(M4, 0, 300, 0, 180);  
}
*/