/****************************************Libreias**************************************************/

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_HTU21DF.h>
#include <RTClib.h>
#include <Fonts/FreeMonoBoldOblique12pt7b.h>
#include <Fonts/FreeSerif12pt7b.h>
#include <SD.h>
//...................................>>>>>>PARA PRUEBA<<<<<<........................................//
#include <DHT.h>

//#include <Adafruit_MLX90614.h>
RTC_DS1307 rtc;
//---------- MPU6050 Measurement & Filtering Range ----------------------------------------------
#define AFS_SEL 2  // Accelerometer Configuration Settings   AFS_SEL=2, Full Scale Range = +/- 8 [g]
#define DLPF_SEL  0  // DLPF Configuration Settings  Accel BW 260Hz, Delay 0ms / Gyro BW 256Hz, Delay 0.98ms, Fs 8KHz 
//---------- Variables for gravity --------------------------------------------------------------
const int MPU_ADDR=0x69;  // I2C address of the MPU-6050
int AcX,AcY,AcZ;  // Accelerometer values
long Cal_AcX, Cal_AcY, Cal_AcZ; // Calibration values
float GAcX, GAcY, GAcZ; // Convert accelerometer to gravity value
float Min_GAcX=0, Max_GAcX=0, PtoP_GAcX, Min_GAcY=0, Max_GAcY=0, PtoP_GAcY, Min_GAcZ=0, Max_GAcZ=0, PtoP_GAcZ; // Finding Min, Max & Peak to Peak of gravity value
float Min = 0, Max = 0; // Initial value of Min, Max
int cnt; // Count of calibration process
float Grvt_unit; // Gravity value unit
long period, prev_time; // Period of calculation
//...................................>>>>>>PARA PRUEBA<<<<<<........................................//
//..................................................................................................//


//..................................................................................................//

//...................................>>>>>>PARA PRUEBA<<<<<<........................................//
#define DHTPIN 25     // Pin donde está conectado el sensor
//..................................................................................................//
//...................................>>>>>>PARA PRUEBA<<<<<<........................................//
#define DHTTYPE DHT22   // Sensor DHT22
//..................................................................................................//

#define SCREEN_WIDTH 128 // OLED display ancho, en pixeles //para rptar texo usar display.setRotation(3); con argumentos de 0 a 3
#define SCREEN_HEIGHT 64 // OLED display height, en pixeles
#define OLED_RESET     4 // pin de reset # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< direccion 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH,SCREEN_HEIGHT,&Wire,OLED_RESET);

 
//variables para el control en el menu
int selected = 0;
int entered = -1;

//*****************************************HTU21D****************************************************//
Adafruit_HTU21DF htu = Adafruit_HTU21DF();
//***************************************MLX90614***************************************************//
//Adafruit_MLX90614 termometroIR = Adafruit_MLX90614();
//...................................>>>>>>PARA PRUEBA<<<<<<........................................//
DHT dht(DHTPIN, DHTTYPE);
//..................................................................................................//
//******* sd****************//
File myFile; 

//***************************************Imagenes**************************************************//
static const uint8_t nerv[] PROGMEM = {
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X7E,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X03,0XFF,0XC0,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X0F,0XE7,0XF0,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X1F,0X00,0XFC,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X76,0X00,0X7E,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0XEC,0X00,0X3F,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0XCC,0X00,0X3F,0X80,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X01,0X98,0X00,0X1F,0X80,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0X98,0X00,0X1F,0XC0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0XF8,0X00,0X1F,0X40,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X04,0XD8,0X00,0X1F,0X20,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X04,0XF8,0X00,0X3F,0X20,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0XFC,0X00,0X3F,0X20,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X08,0XFC,0X00,0X7F,0X10,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X08,0XFE,0X00,0XFF,0X10,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X08,0XFF,0X81,0XFF,0X90,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X09,0XFF,0XFF,0XFF,0X90,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X0B,0XFF,0XFF,0XFF,0XF0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X0F,0XFF,0XFF,0XFF,0XF0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X07,0XFF,0XFF,0XFF,0XE0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X07,0XFF,0XE7,0XFF,0XE0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XFC,0X00,0X3F,0XE0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XF0,0X00,0X0F,0XC0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X01,0XE0,0X81,0X07,0X80,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0XE0,0X81,0X07,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X20,0X81,0X06,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X20,0X81,0X04,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X20,0X81,0X04,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X10,0X00,0X08,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X18,0X00,0X18,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X0E,0X00,0X70,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X01,0XFF,0X80,0X00,0X00,0X00,0X00,0X00,0X00,0X00,

};

//************************************ Fin del codigo del menu *************************************//
void setup() {
   Wire.begin();
   init_MPU6050();
  Serial.begin(115200);
  //****************************************Inicia el DHT*********************************************//
 dht.begin();
//pines 
  pinMode(26,INPUT_PULLUP); // boton de arriba
  pinMode(27,INPUT_PULLUP);// boton de abajo
  pinMode(14,INPUT_PULLUP);// boton enter
  pinMode(12,INPUT_PULLUP);// boton atras

//************************************Comprueba que este la pantalla********************************//
  
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
//*************************************Inicia el  HTU21D********************************************//
/*if (!htu.begin()) {
    Serial.println("Couldn't find sensor!");
    while (1);
  }
  */

if (!rtc.begin()) {
    Serial.println(F("Couldn't find RTC"));
    while (1);
  }
//*************************************************SD*************************************//
  Serial.print("Iniciando SD ...");
  if (!SD.begin(5)) {
    Serial.println("No se pudo inicializar");
    return;
  Serial.println("inicializacion exitosa");
  
  if(!SD.exists("datalog.csv"))
  {
      myFile = SD.open("datalog.csv", FILE_WRITE);
      if (myFile) {
        Serial.println("Archivo nuevo, Escribiendo encabezado(fila 1)");
        myFile.println("Tiempo(ms),Sensor1,Sensor2,Sensor3");
        myFile.close();
      } else {

        Serial.println("Error creando el archivo datalog.csv");
      }
  }
  }
 //*******************************MPU6050************************************//
//display.clearDisplay();
//display.setTextSize(2);
//display.setTextColor(SSD1306_WHITE);
//display.setCursor(2, 20);
Gravity_Range_Option();
Calib_MPU6050(); // Calculating calibration value
delay(1500); 
//**************************************Inicia el MLX90614******************************************//
//termometroIR.begin();
//*************************************Mensaje de inicio********************************************//
display.clearDisplay();
display.setTextSize(2);
display.setTextColor(SSD1306_WHITE);
display.setCursor(2, 20);
display.println(F("Bienvenido"));
display.display(); //Muestra la imagen 
delay(2000);
display.clearDisplay(); //Borra el buffer
display.drawBitmap(0,0,nerv,128,32,1);
display.display(); //Muestra la imagen 
delay(2000);
display.clearDisplay();
//...................................>>>>>>PARA PRUEBA<<<<<<........................................//

//..................................................................................................//

}
char Time[]     = "  :  :  ";
char Calendar[] = "  /  /20  ";
char temperature[] = " 00.00";
char temperature_msb;
byte i, second, minute, hour, day, date, month, year, temperature_lsb;
 
void display_day(){
  switch(day){
    case 1:  draw_text(40, 0, " SUNDAY  ", 1); break;
    case 2:  draw_text(40, 0, " MONDAY  ", 1); break;
    case 3:  draw_text(40, 0, " TUESDAY ", 1); break;
    case 4:  draw_text(40, 0, "WEDNESDAY", 1); break;
    case 5:  draw_text(40, 0, "THURSDAY ", 1); break;
    case 6:  draw_text(40, 0, " FRIDAY  ", 1); break;
    default: draw_text(40, 0, "SATURDAY ", 1);
  }
}
 
void DS3231_display(){
  // Convert BCD to decimal
  second = (second >> 4) * 10 + (second & 0x0F);
  minute = (minute >> 4) * 10 + (minute & 0x0F);
  hour   = (hour >> 4)   * 10 + (hour & 0x0F);
  date   = (date >> 4)   * 10 + (date & 0x0F);
  month  = (month >> 4)  * 10 + (month & 0x0F);
  year   = (year >> 4)   * 10 + (year & 0x0F);
  // End conversion
 
  Time[7]     = second % 10 + 48;
  Time[6]     = second / 10 + 48;
  Time[4]     = minute % 10 + 48;
  Time[3]     = minute / 10 + 48;
  Time[1]     = hour   % 10 + 48;
  Time[0]     = hour   / 10 + 48;
  Calendar[9] = year   % 10 + 48;
  Calendar[8] = year   / 10 + 48;
  Calendar[4] = month  % 10 + 48;
  Calendar[3] = month  / 10 + 48;
  Calendar[1] = date   % 10 + 48;
  Calendar[0] = date   / 10 + 48;
  if(temperature_msb < 0){
    temperature_msb = abs(temperature_msb);
    temperature[0] = '-';
  }
  else
    temperature[0] = ' ';
  temperature_lsb >>= 6;
  temperature[2] = temperature_msb % 10  + 48;
  temperature[1] = temperature_msb / 10  + 48;
  if(temperature_lsb == 0 || temperature_lsb == 2){
    temperature[5] = '0';
    if(temperature_lsb == 0) temperature[4] = '0';
    else                     temperature[4] = '5';
  }
  if(temperature_lsb == 1 || temperature_lsb == 3){
    temperature[5] = '5';
    if(temperature_lsb == 1) temperature[4] = '2';
    else                     temperature[4] = '7';
  }
 
  draw_text(4,  14, Calendar, 2);                     // Display the date (format: dd/mm/yyyy)
  draw_text(16, 35, Time, 2);                         // Display the time
 
}
 
void blink_parameter(){
  byte j = 0;
  while(j < 10 && digitalRead(26) && digitalRead(27)){
    j++;
    delay(25);
  }
}
 
byte edit(byte x_pos, byte y_pos, byte parameter){
  char text[3];
  sprintf(text,"%02u", parameter);
  while(!digitalRead(26));                      // Wait until button B1 released
  while(true){
    while(!digitalRead(27)){                    // If button B2 is pressed
      parameter++;
      if(i == 0 && parameter > 31)                   // If date > 31 ==> date = 1
        parameter = 1;
      if(i == 1 && parameter > 12)                   // If month > 12 ==> month = 1
        parameter = 1;
      if(i == 2 && parameter > 99)                   // If year > 99 ==> year = 0
        parameter = 0;
      if(i == 3 && parameter > 23)                   // If hours > 23 ==> hours = 0
        parameter = 0;
      if(i == 4 && parameter > 59)                   // If minutes > 59 ==> minutes = 0
        parameter = 0;
      sprintf(text,"%02u", parameter);
      draw_text(x_pos, y_pos, text, 2);
      delay(200);                                    // Wait 200ms
    }
    draw_text(x_pos, y_pos, "  ", 2);
    blink_parameter();
    draw_text(x_pos, y_pos, text, 2);
    blink_parameter();
    if(!digitalRead(26)){                       // If button B1 is pressed
      i++;                                           // Increament 'i' for the next parameter
      return parameter;                              // Return parameter value and exit
    }
  }
}

void draw_text(byte x_pos, byte y_pos, char *text, byte text_size) {
  display.setCursor(x_pos, y_pos);
  display.setTextSize(text_size);
  display.print(text);
  display.display();
}
void op1(){
  float h = dht.readHumidity(); //Leemos la Humedad
  float t = dht.readTemperature(); //Leemos la temperatura en grados Celsius
 
  myFile = SD.open("datalog.csv", FILE_WRITE);//abrimos  el archivo
 if (myFile) { 
        Serial.print("Escribiendo SD: "); 

        myFile.print(millis());
        myFile.print(",");
        myFile.print(t);
        myFile.print(",");
        myFile.print(t);
        myFile.print(",");
        myFile.println(h);
         myFile.print(",");
        myFile.println(h);

        myFile.close(); //cerramos el archivo
        
  
  } else {
  	// if the file didn't open, print an error:
    Serial.println("Error al abrir el archivo");
  }
  delay(100);
 

//..................................................................................................//
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    //******************** Mensaje de monitoreo************************
    display.setCursor(25,0);
    display.println(F("Monitoreando"));
    //**********************Temperatura********************************
    display.setCursor(0,8);
    display.println("Temperatura 1:");
    display.setCursor(84,8);
   display.print(t);
    display.setCursor(114,8);
    display.println("C");
    display.drawRect(120, 8, 3, 3, WHITE);     // Put degree symbol ( ° )
    display.setCursor(0,17);
    display.println("Temperatura 2:");
    display.setCursor(84,17);
    display.print(t);
    display.setCursor(114,17);
    display.println("C");
    display.drawRect(120, 17, 3, 3, WHITE); 
   
  //**********************Humedad************************************
    display.setCursor(0,25);
    display.println("Humedad:");
    display.setCursor(48,25);
   display.print(h);
    display.setCursor(78,25);
    display.println("%");
    display.setCursor(84,25);
    display.println("RH");
    
    //*********************Vibracion***********************************
    display.setCursor(0,34);
    display.println("Vibracion X|Y|Z ");
    display.setCursor(0,43);
    display.println(String(PtoP_GAcX) + "|" + String(PtoP_GAcY) + "|" + String(PtoP_GAcZ));
    display.setCursor(90,43);
    display.println("G");
}

void op2(){
  //******************************************Opcion 2************************************************/
  //...................................>>>>>>PARA PRUEBA<<<<<<........................................//
  /***************************************DHT sensor*******************************************/
 float h = dht.readHumidity(); //Leemos la Humedad
 float t = dht.readTemperature(); //Leemos la temperatura en grados Celsius
  //float f = dht.readTemperature(true); //Leemos la temperatura en grados Fahrenheit
 //float temp = htu.readTemperature();
 //float rel_hum = htu.readHumidity(); 
//..................................................................................................//
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    //******************** Mensaje de monitoreo************************
    display.setCursor(8,0);
    display.println(F("Variables en tiempo"));
     display.setCursor(50,8);
    display.println(F("Real"));
    //**********************Temperatura********************************
    display.setCursor(0,17);
    display.println("Temperatura 1:");
    display.setCursor(84,17);
    display.print(t);
    display.setCursor(114,17);
    display.println("C");
    display.drawRect(120, 17, 3, 3, WHITE);     // Put degree symbol ( ° )
    display.setCursor(0,26);
    display.println("Temperatura 2:");
    display.setCursor(84,26);
    display.print(t);
    display.setCursor(114,26);
    display.println("C");
    display.drawRect(120, 26, 3, 3, WHITE); 
   
  //**********************Humedad************************************
    display.setCursor(0,35);
    display.println("Humedad:");
    display.setCursor(48,35);
    display.print(h);
    display.setCursor(78,35);
    display.println("%");
    display.setCursor(84,35);
    display.println("RH");
    
    //*********************Vibracion***********************************
    display.setCursor(0,44);
    display.println("Vibracion X|Y|Z ");
    display.setCursor(0,53);
    display.println(String(PtoP_GAcX) + "|" + String(PtoP_GAcY) + "|" + String(PtoP_GAcZ));
    display.setCursor(90,53);
    display.println("G");

} 

void op3(){
    
  display.display();
   display.setTextColor(WHITE, BLACK);
if(!digitalRead(26)){                         // If button B1 is pressed
    i = 0;
    while(!digitalRead(26));                    // Wait for button B1 release
    while(true){
      while(!digitalRead(27)){                  // While button B2 pressed
        day++;                                       // Increment day
        if(day > 7) day = 1;
        display_day();                               // Call display_day function
        delay(200);                                  // Wait 200 ms
      }
      draw_text(40, 0, "         ", 1);
      blink_parameter();                             // Call blink_parameter function
      display_day();                                 // Call display_day function
      blink_parameter();                             // Call blink_parameter function
      if(!digitalRead(26))                      // If button B1 is pressed
        break;
    }
 
    date   = edit(4, 14, date);                      // Edit date
    month  = edit(40, 14, month);                    // Edit month
    year   = edit(100, 14, year);                    // Edit year
    hour   = edit(16, 35, hour);                     // Edit hours
    minute = edit(52, 35, minute);                   // Edit minutes
 
    // Convert decimal to BCD
    minute = ((minute / 10) << 4) + (minute % 10);
    hour = ((hour / 10) << 4) + (hour % 10);
    date = ((date / 10) << 4) + (date % 10);
    month = ((month / 10) << 4) + (month % 10);
    year = ((year / 10) << 4) + (year % 10);
    // End conversion
 
    // Write data to DS3231 RTC
    Wire.beginTransmission(0x68);               // Start I2C protocol with DS3231 address
    Wire.write(0);                              // Send register address
    Wire.write(0);                              // Reset sesonds and start oscillator
    Wire.write(minute);                         // Write minute
    Wire.write(hour);                           // Write hour
    Wire.write(day);                            // Write day
    Wire.write(date);                           // Write date
    Wire.write(month);                          // Write month
    Wire.write(year);                           // Write year
    Wire.endTransmission();                     // Stop transmission and release the I2C bus
    delay(200);                                 // Wait 200ms
  }
 
  Wire.beginTransmission(0x68);                 // Start I2C protocol with DS3231 address
  Wire.write(0);                                // Send register address
  Wire.endTransmission(false);                  // I2C restart
  Wire.requestFrom(0x68, 7);                    // Request 7 bytes from DS3231 and release I2C bus at end of reading
  second = Wire.read();                         // Read seconds from register 0
  minute = Wire.read();                         // Read minuts from register 1
  hour   = Wire.read();                         // Read hour from register 2
  day    = Wire.read();                         // Read day from register 3
  date   = Wire.read();                         // Read date from register 4
  month  = Wire.read();                         // Read month from register 5
  year   = Wire.read();                         // Read year from register 6
  Wire.beginTransmission(0x68);                 // Start I2C protocol with DS3231 address
  Wire.write(0x11);                             // Send register address
  Wire.endTransmission(false);                  // I2C restart
  Wire.requestFrom(0x68, 2);                    // Request 2 bytes from DS3231 and release I2C bus at end of reading
  temperature_msb = Wire.read();                // Read temperature MSB
  temperature_lsb = Wire.read();                // Read temperature LSB
  display_day();
  DS3231_display();    // Diaplay time & calendar

 
}
void op4(){
   ReadDate_MPU6050();
   Calc_Grvt();
   Display_Grvt();
    

}
//Codigo del menu 
void displaymenu() {
 
  int up = digitalRead(26);//Pin del ESP32
  int down = digitalRead(27);//Pin del ESP32
  int enter = digitalRead(14);//Pin del ESP32
  int back = digitalRead(12);//Pin del ESP32

  if (down == LOW && up == LOW) {
  };
  if (down == LOW) {
    selected = selected + 1;
    delay(200);
  };
  if (up == LOW) {
    selected = selected - 1;
    delay(200);
  };
  if (enter == LOW) {
    entered = selected;
    display.clearDisplay();
  };
  if (back == LOW) {
    entered = -1;
 
  };
  const char *options[4] = {
    " Monitoreo",
    " Variables",
    " Config. Fecha",
    " Otros",
};
if (entered == -1) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(20, 0);
    display.println(F("Menu Principal"));
    
    display.println("");
    for (int i = 0; i < 4; i++) {
      if (i == selected) {
        display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
        display.println(options[i]);
        }else if (i != selected) {
        display.setTextColor(SSD1306_WHITE);
        display.println(options[i]);
      }
  

  //para hacer que cuando llegue abajo regrese al inicio
  if(selected >=4){
selected = 0;
entered = -1;
  }

if(selected <= -1){
selected = 3;
entered = -1;
  
  }
 
  }
  
}

//******************************************Opcion 1************************************************//
    else if (entered == 0) {
 op1();
    
}
//******************************************Opcion 2************************************************//
  else if (entered == 1) {
 op2();
  }
//******************************************Opcion 3************************************************//
  else if (entered == 2) {
  op3();
  }
//******************************************Opcion 4************************************************//
   else if (entered == 3) {
  op4();
  } 

  display.display();
}
void init_MPU6050(){
  //MPU6050 Initializing & Reset
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  //MPU6050 Clock Type
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0x03);     // Selection Clock 'PLL with Z axis gyroscope reference'
  Wire.endTransmission(true);

  //MPU6050 Accelerometer Configuration Setting
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1C);  // Accelerometer Configuration register
  if(AFS_SEL == 0) Wire.write(0x00);     // AFS_SEL=0, Full Scale Range = +/- 2 [g]
  else if(AFS_SEL == 1) Wire.write(0x08);     // AFS_SEL=1, Full Scale Range = +/- 4 [g]
  else if(AFS_SEL == 2) Wire.write(0x10);     // AFS_SEL=2, Full Scale Range = +/- 8 [g]
  else  Wire.write(0x18);     // AFS_SEL=3, Full Scale Range = +/- 10 [g]
  Wire.endTransmission(true);

  //MPU6050 DLPF(Digital Low Pass Filter)
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1A);  // DLPF_CFG register
  if(DLPF_SEL == 0) Wire.write(0x00);     // Accel BW 260Hz, Delay 0ms / Gyro BW 256Hz, Delay 0.98ms, Fs 8KHz 
  else if(DLPF_SEL == 1)  Wire.write(0x01);     // Accel BW 184Hz, Delay 2ms / Gyro BW 188Hz, Delay 1.9ms, Fs 1KHz 
  else if(DLPF_SEL == 2)  Wire.write(0x02);     // Accel BW 94Hz, Delay 3ms / Gyro BW 98Hz, Delay 2.8ms, Fs 1KHz 
  else if(DLPF_SEL == 3)  Wire.write(0x03);     // Accel BW 44Hz, Delay 4.9ms / Gyro BW 42Hz, Delay 4.8ms, Fs 1KHz 
  else if(DLPF_SEL == 4)  Wire.write(0x04);     // Accel BW 21Hz, Delay 8.5ms / Gyro BW 20Hz, Delay 8.3ms, Fs 1KHz 
  else if(DLPF_SEL == 5)  Wire.write(0x05);     // Accel BW 10Hz, Delay 13.8ms / Gyro BW 10Hz, Delay 13.4ms, Fs 1KHz 
  else  Wire.write(0x06);     // Accel BW 5Hz, Delay 19ms / Gyro BW 5Hz, Delay 18.6ms, Fs 1KHz 
  Wire.endTransmission(true);
}
void Gravity_Range_Option(){
  switch(AFS_SEL) { // Selecting Gravity unit value
    case 0:
      Grvt_unit = 16384;
      break;
    case 1:
      Grvt_unit = 8192;
      break;
    case 2:
      Grvt_unit = 4096;
      break;
    case 3:
      Grvt_unit = 3276.8;
      break;
  }
}  
////////////**************************/////////
void Calib_MPU6050() {  
  for(int i = 0 ; i < 2000 ; i++) { // Summing Iteration for finding calibration value
    if(i % 200 == 0) {  // Display progress every 200 cycles
      cnt++;
      if(cnt == 1)  { // Characters to display first
     Serial.print("Calculating .");
    //display.clearDisplay();
    //display.setTextSize(1);
    //display.setTextColor(SSD1306_WHITE);
    //display.setCursor(0, 0);
    //display.println(F("Calculating"));
      }
      else  { // Display progress by point
        Serial.print(".");
    //display.clearDisplay();
    //display.setTextSize(1);
    //display.setTextColor(SSD1306_WHITE);
    //display.setCursor(0, 10);
    //display.println(F("."));
      }      
    }    
    
    ReadDate_MPU6050(); // Read Accelerometer data
    
    delay(10);

    // Sum data
    Cal_AcX += AcX;
    Cal_AcY += AcY;
    Cal_AcZ += AcZ;
  }

  // Average Data
  Cal_AcX /= 2000;
  Cal_AcY /= 2000;
  Cal_AcZ /= 2000;

  // Serial Print
  Serial.println("");
  Serial.println("End of Calculation");
  Serial.print("Cal_AcX = "); Serial.print(Cal_AcX);
  Serial.print(" | Cal_AcY = "); Serial.print(Cal_AcY);
  Serial.print(" | Cal_AcZ = "); Serial.println(Cal_AcZ);


  //display.clearDisplay();
  //display.setTextSize(1);
  //display.setTextColor(SSD1306_WHITE);
  //display.setCursor(0, 0);
  //display.println("CalibValue X|Y|Z");
   //display.setCursor(0,1);
 // display.print(String(Cal_AcX) + "|" + String(Cal_AcY) + "|" + String(Cal_AcZ));
delay(1000);

}
/////////////////////////*****//////////////////////
void ReadDate_MPU6050() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  //Wire.requestFrom(MPU_ADDR,14,true);  // request a total of 14 registers
  Wire.requestFrom(MPU_ADDR,6,true);  // request a total of 14 registers
  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)
}
void Calc_Grvt() {
  AcX = (AcX - Cal_AcX);  // Calibrated Accelerometer value
  AcY = (AcY - Cal_AcY);  // Calibrated Accelerometer value
  AcZ = (AcZ - Cal_AcZ);  // Calibrated Accelerometer value
  
  GAcX = AcX / Grvt_unit; // Converting the Calibrated value to Gravity value
  GAcY = AcY / Grvt_unit; // Converting the Calibrated value to Gravity value
  GAcZ = AcZ / Grvt_unit; // Converting the Calibrated value to Gravity value

  //---------- Calculating Min, Max & Peak to Peak of Gravity --------------------------------------

  Min_GAcX = min(Min_GAcX, GAcX);
  Max_GAcX = max(Max_GAcX, GAcX);
  PtoP_GAcX = Max_GAcX - Min_GAcX;

  Min_GAcY = min(Min_GAcY, GAcY);
  Max_GAcY = max(Max_GAcY, GAcY);
  PtoP_GAcY = Max_GAcY - Min_GAcY;

  Min_GAcZ = min(Min_GAcZ, GAcZ);
  Max_GAcZ = max(Max_GAcZ, GAcZ);
  PtoP_GAcZ = Max_GAcZ - Min_GAcZ;
}  

void Display_Grvt() {
  //---------- Serial print aceleration----------------------------------------------------------------------
  //Serial.print("AcX= " + String(AcX));
  //Serial.print(" |AcY= " + String(AcY));
  //Serial.println(" |AcZ= " + String(AcZ));
//---------- Serial print gravity value----------------------------------------------------------------------
  Serial.print("PtoP_GAcX= " + String(PtoP_GAcX));
  Serial.print("|PtoP_GAcY= " + String(PtoP_GAcY));
  Serial.println("|PtoP_GAcZ= " + String(PtoP_GAcZ));
  delay(200);

//---------- LCD Display -----------------------------------------------------------------------
  period = millis() - prev_time;
  
  if(period > 1000) {
    
    prev_time = millis();
    

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(20, 0);
display.println("PeakToPeak X|Y|Z");
display.setCursor(20,20);
display.println(String(PtoP_GAcX) + "|" + String(PtoP_GAcY) + "|" + String(PtoP_GAcZ));

    Min_GAcX = 0;
    Max_GAcX = 0;
    Min_GAcY = 0;
    Max_GAcY = 0;
    Min_GAcZ = 0;
    Max_GAcZ = 0;
  }
}



void loop() {
   displaymenu();
  //comportamiento de las variables de control del programa
  //Serial.println("selected=");Serial.println(selected); 
  //Serial.println("entered=");Serial.println(entered);
 
}