//Painting Drone 
//
//by: 
//Murilo Casa Grande Lopes;
//Pedro Luiz Carvalho de Oliveira Costa;
//Maria Gabriela Bianco;
//Welder Beltrão Paixão;
//Giovany Ribeiro;
//

#include <Wire.h>
#include <MPU6050_light.h>
#include <DistanceSensor.h>

#define NUM_MPUS 1
#define ULTRS 6

//Pins
#define Ch1 32
#define Ch2 33
#define Ch3 34
#define Ch4 35
#define Ch5 36
#define Ch6 39
#define LedW 2
#define LedGR 23
#define Mtr1 25
#define Mtr2 26
#define Mtr3 27;
#define Mtr4 14
#define StR 17
#define StG 0
int AD0pin[NUM_MPUS] = {16}; // For the AD0 pins of the MPU-6050.
int trigPin = 18; // For the TRIG pins of the Ultrassonic Sensor.
DistanceSensor sens1(trigPin, 5); // (trig, echo);
DistanceSensor sens2(trigPin, 12);
DistanceSensor sens3(trigPin, 4);
DistanceSensor sens4(trigPin, 13);
DistanceSensor sens5(trigPin, 15);
DistanceSensor sens6(trigPin, 19);

//CHs
int ch1Value;
int ch2Value;
int ch3Value;
int ch4Value;
int ch5Value;
int ch6Value;

//MPU6050 Setup
MPU6050 mpu[NUM_MPUS] = {MPU6050(Wire)};
const int mpuAddr = 0x68;  // I2C address
const int mpuAddr_not_used = 0x69; // not even used in the code.
unsigned long previousMillis;
const unsigned long interval = 1000;

//Motors
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

// Error
int Err;

//Função que lê o valor de um canal e a transforma num outro numa faixa definida
  // Se o valor do canal for menor que 100, isto é canal em estado OFF, retorna o valor default
  // O valor de um canal geralmente fica na faixa de 1000 a 2000
int readChannel(int channelInput, int minLimit, int maxLimit, int defaultValue){
  int ch = pulseIn(channelInput, HIGH, 30000);
  if (ch < 100) return defaultValue;
  return map(ch, 1000, 2000, minLimit, maxLimit);
}
  // Read the switch channel and return a boolean value
  bool readSwitch(byte channelInput, bool defaultValue){
  int intDefaultValue = (defaultValue)? 100: 0;
  int ch = readChannel(channelInput, 0, 100, intDefaultValue);
  return (ch > 50);
}

void setup() {
  Serial.begin(115200);
  Serial.println();

  //Pins
  pinMode(LedW, OUTPUT);
  pinMode(LedGR, OUTPUT);
  pinMode(Ch1, INPUT);
  pinMode(Ch2, INPUT);
  pinMode(Ch3, INPUT);
  pinMode(Ch4, INPUT);
  pinMode(Ch5, INPUT);
  pinMode(Ch6, INPUT);
  pinMode(StR, OUTPUT);
  pinMode(StG, OUTPUT);

  //Motors
  servo1.attach(25);
  servo2.attach(26);
  servo3.attach(27);
  servo4.attach(14);

  //MPU6050
  Wire.begin();  // default pins: SDA=21, SCL=22.
  for(int i=0; i<NUM_MPUS; i++){
    pinMode(AD0pin[i],OUTPUT);
    digitalWrite(AD0pin[i],HIGH); // default high for 0x69
  }

  // Initialize all the sensors.
  Serial.println(F("Initializing the sensors and calculating offsets. Do not move the sensors."));
  delay(500);
  for(int i=0; i<NUM_MPUS; i++){
    SelectMPU(i);
    byte status = mpu[i].begin();
    Serial.print(F("Initializing MPU-6050 "));
    Serial.print(i);
    Serial.print(F(", error = "));
    Serial.print(status);
    if(status == 0)
      Serial.print(F(" (no error)"));
    Serial.println();
    mpu[i].calcOffsets(true,true); // gyro and accelometer
  }
  previousMillis = millis();

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

  //
  Serial.println(". . .");
  Serial.println("Initialization done");
  Serial.println("The Painting Drone is Ready");
  Serial.println(". . .");
}


void loop() {
//Base
  int lig = 1;
  if(lig == 1){
  digitalWrite(LedGR, 1);
  delay(700);
  digitalWrite(LedGR, 0);
  delay(700);
  }

  if(analogRead(Ch5)){ //Front Light
  digitalWrite(LedW, 1);
  }
  else{
  digitalWrite(LedW, 0);
  }

//Reading MPU6050 Sensor
  unsigned long currentMillis = millis();
  for(int i=0; i<NUM_MPUS; i++){
    SelectMPU(i);
    mpu[i].update();
  }

  if(currentMillis - previousMillis >= interval){
    previousMillis = currentMillis;

    for(int i=0; i<NUM_MPUS; i++){
      SelectMPU(i);
      Serial.print("MPU6050: ");
      Serial.print(F("Temp:")); //Tempereture °C
      Serial.print(mpu[i].getTemp(),0);
      Serial.print("°C");
      Serial.print(F("/Acc:")); //Accel X,Y,Z axis
      Serial.print(mpu[i].getAccX());
      Serial.print(",");
      Serial.print(mpu[i].getAccY());
      Serial.print(",");
      Serial.print(mpu[i].getAccZ());
      Serial.print(F("/Gyro:")); //Gyro 
      Serial.print(mpu[i].getGyroX(),0);
      Serial.print(",");
      Serial.print(mpu[i].getGyroY(),0);
      Serial.print(",");
      Serial.print(mpu[i].getGyroZ(),0);
      Serial.print(F("/AccAngle:"));  //Accel angle
      Serial.print(mpu[i].getAccAngleX(),0);
      Serial.print(",");
      Serial.print(mpu[i].getAccAngleY(),0);
      Serial.print(F("/Angle:")); //Accel X,Y,Z axis
      Serial.print(mpu[i].getAngleX(),0);
      Serial.print(",");
      Serial.print(mpu[i].getAngleY(),0);
      Serial.print(",");
      Serial.print(mpu[i].getAngleZ(),0);
      Serial.println();
    }
  }

//Redaing Ultrassonic Sensors
  int d1 = (sens1.getCM())*10;
  delay(60);
  int d2 = (sens2.getCM())*10;
  delay(60);
  int d3 = (sens3.getCM())*10;
  delay(60);
  int d4 = (sens4.getCM())*10;
  delay(60);
  int d5 = (sens5.getCM())*10;
  delay(60);
  int d6 = (sens6.getCM())*10;
  delay(60);

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

//Reading Channels
  // Obtenção dos valores dos canais dentro da faixa de -100 a 100
  ch1Value = readChannel(Ch1, -100, 100, 0);
  ch2Value = readChannel(Ch2, -100, 100, 0);
  ch3Value = readChannel(Ch3, -100, 100, 0);
  ch4Value = readChannel(Ch4, -100, 100, 0);
  ch5Value = readChannel(Ch5, -100, 100, 0);
  ch6Value = readChannel(Ch6, -100, 100, 0);
  
  // Enviar a informação dos valores dos canais através da comunicação serial
  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);

//Errors
  byte status = mpu[0].begin();
  if(analogRead(Ch1) == 0 && analogRead(Ch2) == 0 && analogRead(Ch3) == 0 && analogRead(Ch4) == 0 && analogRead(Ch5) == 0 && analogRead(Ch6) == 0){ //Class 3 Error
    digitalWrite(StR,1);
    digitalWrite(StG,0);
    Serial.println("**Class 3 Error: Check the connection to the controller**");
    Err = 3;
  }
  else if((status != 0) || mpu[0].getTemp() >= 80){ //Class 2 Error
    digitalWrite(StR,1);
    digitalWrite(StG,1);
    Serial.println("**Class 2 Error: Check the MPU6050 sensor**");
    Err = 2;
  }
  else if(d1 == 0 || d2 == 0 || d3 == 0 || d4 == 0 || d5 == 0 || d6 == 0){ //Class 1 Error
    digitalWrite(StR,1);
    digitalWrite(StG,1);
    Serial.println("**Class 1 Error: Check the Ultrassonic sensors**");
     Err = 1;
  }
  else { //Class 0 Error
    digitalWrite(StR,0);
    digitalWrite(StG,1);
    Err = 0;
  }

 Serial.println(F("---- ----\n"));
}

// Choose a MPU, parameter starts at zero for the first sensor.
// AD0 = high, I2C address = 0x69, not selected
// AD0 = low,  I2C address = 0x68, selected
void SelectMPU(int selection){
  for(int i=0; i<NUM_MPUS; i++)
  {
    if(i == selection)
      digitalWrite(AD0pin[i],LOW);  // selected, 0x68
    else
      digitalWrite(AD0pin[i],HIGH); // not selected, 0x69
  }
}