#include <Wire.h>                   // Biblioteca da comunicação I2C
#include <Adafruit_MPU6050.h>       // Biblioteca do MPU
#include <Adafruit_Sensor.h>
#include <Servo.h>                  // Bliblioteca do Servo motor

const int MPU_Endereco = 0x68;    // Endereço da comunicação I2C do MPU-6050
Adafruit_MPU6050 mpu;             // Objeto para o MPU6050
Adafruit_Sensor *mpu_temp, *mpu_accel, *mpu_gyro;
Servo myservo01, myservo02, myservo03, myservo04;   // Objeto para o Servo

//SDA no A4
//SCL no A5
//VCC 3.3V

float angl01, angl02, angl03, angl04;
float aux_01, aux_02, aux_03, aux_04;
int incremento = 5;

/* Controles:
*   Para controlar a base da garra, deve-se girar a placa para direita / Esquerda
*   Para controlar a altura da garra, movimente a placa para cima / baixo
*   Para controlar a distância da garra, movimente a placa para frente / trás
*   O controle da garra é feito por meio de dois botões
*/

void setup()
{
  pinMode(4, INPUT);
  pinMode(7, INPUT);

  Serial.begin(115200);
  myservo01.attach(5);
  myservo02.attach(6);
  myservo03.attach(9);
  myservo04.attach(10);

  myservo04.write(0);


  /****************************** MPU6050 CONFIG ******************************/
  /*
  Wire.begin();     // Inicia a comunicação I2C Bus
  Wire.beginTransmission(MPU_Endereco);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();
  */

  if (!mpu.begin())
  {
    Serial.println("MPU6050 não encontrado");
    while(1){delay(10);}
  }
  Serial.println("MPU6050 Conectado");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) 
  {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) 
  {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+- 250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+- 500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+- 1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+- 2000 deg/s");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) 
  {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
  }
  delay(5000);
  
  /****************************************************************************/
}

void loop() 
{
  // Aquisição de dados utilizando o endereço de memória do MPU6050:
  /*
    Wire.beginTransmission(MPU_Endereco);
    Wire.write(0x3B);
    Wire.endTransmission(false);

    int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;  // Variaveis de 16 bytes
    Wire.requestFrom( MPU_Endereco, 14);   // request a total of 14 bytes
    AcX= Wire.read()<<8 | Wire.read();   // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    AcY = Wire.read()<<8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AcZ = Wire.read()<<8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

    Tmp = Wire.read()<<8 | Wire.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
    GyX = Wire.read()<<8 | Wire.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
    GyY = Wire.read()<<8 | Wire.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
    GyZ = Wire.read()<<8 | Wire.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)
    

    Serial.print("1 - AcX: ");Serial.println(AcX);
    Serial.print("2 - Acy: ");Serial.println(AcY);
    Serial.print("3 - Acz: ");Serial.println(AcZ);
    for(int i=0;i<3;i++){Serial.println();}
    delay(1000);
  */

  //  Aquisição de dados utilizando a biblioteca do MPU6050:
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /****************************************************************************/
  /**********************  Controle analógico do MPU6050 **********************/

  int erro_gyro = 41.24;    // Valor de correção para o giroscópio
  int Y_acce = 9.81;     // Valor de correção de gravidade para o acelerometro
  int erro_acce;
  int i, j;
  //calcular a relação de escala de acelerometro

  angl01 = (g.gyro.y) * erro_gyro/2 + 90;         // Servo da base (Esquerda / Direita)
  angl02 = (a.acceleration.z) - Y_acce + 90;   // Servo da esqueda (Sobe / Desce)
  angl03 = (a.acceleration.y) + 90;               // Servo da direita (Frente / Atrás)

  myservo01.write(angl01);
  myservo02.write(angl02);
  myservo03.write(angl03);

 

  /****************************************************************************/
  /************************ Controle digital do MPU6050 ***********************/

  if (digitalRead(7) == HIGH && digitalRead(4) == LOW)
    {
      angl04 = angl04 + incremento;
      myservo04.write(angl04);
    }
    
  if (digitalRead(7) == LOW && digitalRead(4) == HIGH)
    {
      angl04 = angl04 - incremento;
      myservo04.write(angl04);
    }

  while (digitalRead(7) == HIGH && digitalRead(4) == HIGH) // Mantém o estado atual
    {
      for (i=0; i<1; i++)
      {
        aux_01 = angl01; aux_02 = angl02; aux_03 = angl03; aux_04 = angl04;
      }
      
      if (aux_01 != angl01 || aux_02 != angl02 || aux_03 != angl03 || aux_04 != angl04)
      {
        for (j=0; j<1; j++)
        myservo01.write(aux_01);
        myservo02.write(aux_02);
        myservo03.write(aux_03);
        myservo04.write(aux_04);
      }
      debugger();
    }
  i=0; j=0;
  debugger();
}


void debugger()
{
  for(int i=0 ; i<5 ; i++){Serial.println();}
  /*
  Serial.print("Servo 'BASE' ...: "); Serial.println(angl01);
  Serial.print("Servo 'ESQUERDO' ...: "); Serial.println(angl02);
  Serial.print("Servo 'DIREITO' ...: "); Serial.println(angl03);
  Serial.print("Servo 'GARRA' ...: "); Serial.println(angl04);
  */
  
  Serial.print("PIN7 ............: "); Serial.println(digitalRead(7));
  Serial.print("PIN4 ............: "); Serial.println(digitalRead(4));
  /*
  Serial.print("VAR AUX_01.......: "); Serial.println(aux_01);
  Serial.print("VAR AUX_02.......: "); Serial.println(aux_02);
  Serial.print("VAR AUX_03.......: "); Serial.println(aux_03);
  Serial.print("VAR AUX_04.......: "); Serial.println(aux_04);
  */
  delay(50);
}

void Reset_Positons()
{

}