byte opt;
const byte LedError = 13;
#include <Wire.h>
#include <VL53L1X.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <U8g2_for_Adafruit_GFX.h>
#include <SoftwareSerial.h>
//#include <Servo.h>
U8G2_FOR_ADAFRUIT_GFX u8g2_for_adafruit_gfx;
//#define EMIC_RX 2
//#define EMIC_TX 3
#define DISTANCE_THRESHOLD 100  // Distance threshold in centimeters to switch display medium
#define OLED_RESET -1
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define MODE_CHANGE_INTERVAL 200  // Change mode every 10 seconds
#define EMIC_START_CHAR 0xFE      // Start character for Emic 2

VL53L1X sensor;
//SoftwareSerial emicSerial(EMIC_RX, EMIC_TX);  // Connection for EMIC-2
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Servo servo1;             // First servo
//Servo servo2;             // Second servo
//int buttonPin = 7;    // Pin where the push button is connected
//int buttonState = 0;  // Variable to store the button state
//int lastButtonState = 0;  // Variable to store the last button state
//int angle = 0;            // Initial angle of the servos

unsigned long ultimamedicion = 0;

const unsigned char epd_bitmap_xd[] PROGMEM = {
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x07, 0xfc, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x38, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x00, 0x00, 0x20, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xf9, 0xf0, 0x00, 0x00, 0xf8, 0x00, 0x7e, 0x0f, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xf7, 0x80, 0x00, 0x00, 0x70, 0x01, 0xc0, 0x07, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xcf, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x03, 0xcf, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0x38, 0x01, 0xf0, 0x00, 0x03, 0xe0, 0x1e, 0x03, 0xf3, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xfe, 0x30, 0x7f, 0xff, 0xe0, 0x1e, 0x3f, 0xff, 0x07, 0xf9, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xfc, 0x01, 0xff, 0xff, 0xf8, 0xf8, 0x7f, 0xff, 0xff, 0x18, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xf8, 0x07, 0xc3, 0xf8, 0x7f, 0x01, 0xfc, 0x3f, 0xff, 0x80, 0x7f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xf0, 0x1f, 0x3c, 0x0f, 0x80, 0x07, 0xe7, 0xfb, 0xff, 0xe0, 0x3f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xe0, 0x1e, 0x3c, 0x03, 0x80, 0x0f, 0x8f, 0x00, 0x1f, 0xe0, 0x3f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xe0, 0x1e, 0x3c, 0x00, 0x00, 0x1f, 0x3c, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xe0, 0x0f, 0x8f, 0xe0, 0x00, 0x1e, 0x70, 0x00, 0x00, 0x01, 0x9f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xe0, 0x07, 0xff, 0xfc, 0x00, 0x3e, 0x70, 0x00, 0x3f, 0xe1, 0x9f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xc0, 0x00, 0xff, 0x7f, 0xe0, 0x3e, 0x70, 0x19, 0xff, 0xe1, 0x9f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xe0, 0x00, 0x07, 0xf8, 0x7c, 0x3e, 0x70, 0x03, 0xff, 0xe3, 0x9f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x1f, 0x9e, 0x1e, 0x38, 0x00, 0x1f, 0xe3, 0x9f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xe3, 0x00, 0x00, 0x07, 0x9f, 0x1f, 0x3c, 0x00, 0x1f, 0xe7, 0x3f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xf3, 0x1f, 0xf8, 0x00, 0x1f, 0x07, 0x8f, 0x80, 0x7f, 0xc6, 0x3f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xf3, 0xcf, 0xfc, 0x00, 0x3e, 0x03, 0xe0, 0x03, 0xff, 0x86, 0x7f, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xf8, 0xc7, 0xfe, 0x01, 0xfc, 0x00, 0xe0, 0x1f, 0xfe, 0x00, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xfc, 0xf7, 0x87, 0xff, 0xf0, 0x78, 0x00, 0x7f, 0xf8, 0x01, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0x3f, 0x03, 0xfe, 0x1f, 0xc0, 0x00, 0x3e, 0x00, 0x03, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0x9e, 0x00, 0x00, 0x7c, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x03, 0xe0, 0x20, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0xfc, 0x00, 0xf8, 0x00, 0x00, 0x60, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0x03, 0xf0, 0x00, 0x30, 0x00, 0x00, 0xe3, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x3f, 0xe0, 0x00, 0x1f, 0xf0, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x01, 0xef, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfd, 0xff, 0xff,
  0xff, 0xe7, 0xbf, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xcf, 0xf0, 0xff, 0xff,
  0xff, 0xe1, 0xbf, 0x7b, 0xff, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x3d, 0xf3, 0xcf, 0xb0, 0xff, 0x7f,
  0xff, 0xe0, 0x0e, 0x38, 0xee, 0x3c, 0xff, 0x1f, 0xfe, 0x1f, 0x38, 0xe3, 0x8e, 0x30, 0xe7, 0xff,
  0xff, 0xf0, 0x0e, 0x38, 0xc6, 0x38, 0x7f, 0x1f, 0xfe, 0x1f, 0x38, 0xe3, 0x8e, 0x30, 0xc7, 0xff,
  0xff, 0xc7, 0x8e, 0x30, 0xc6, 0x1c, 0x7f, 0x0f, 0xff, 0x9e, 0x38, 0xe3, 0x8e, 0x30, 0xe1, 0xff,
  0xff, 0xff, 0xfe, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xfe, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

void setup() {

  pinMode(LedError, OUTPUT);
  Serial.begin(9600);
  //emicSerial.begin(9600);  // Initialize serial communication for EMIC-2
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  u8g2_for_adafruit_gfx.begin(display);
  display.clearDisplay();
  display.display();

// Initialize VL53L1X distance sensor
//Wire.begin();
/*
  sensor.setTimeout(500);
  if (!sensor.init()) {
    //Serial.println("Failed to initialize VL53L1X sensor!");
    while (true) {
      digitalWrite(LedError, HIGH);  // LED INDICADOR ERROR SENSOR
      delay(1000);
      digitalWrite(LedError, LOW);
      delay(1000);
    }
  }
*/
  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    //Serial.println(F("SSD1306 allocation failed"));
    while (true) {
      digitalWrite(LedError, HIGH);  // LED INDICADOR ERROR PANTALLA
      delay(400);
      digitalWrite(LedError, LOW);
      delay(400);
    }
  }

  // Initialize servos
  //servo1.attach(9);           // Assign pin 9 as control pin for the first servo
  //servo2.attach(10);          // Assign pin 10 as control pin for the second servo
  //inMode(buttonPin, INPUT);  // Configure button pin as input

  // Show welcome message on OLED display
  Serial.print("V18,CR");  // 'Speak' command
  Serial.print("N8");      // 'Speak' command
  Serial.print("L1,CR");
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(3, 25);
  display.println("Bienvenido");
  display.display();

  // Send welcome message to Emic 2
  //sendTextToEmic("Welcome to Space Guide");
  delay(4000);  // Wait for 2 seconds before continuing
  display.clearDisplay();
  display.drawBitmap(0, 0, epd_bitmap_xd, 128, 64, SSD1306_WHITE);
  display.display();
  delay(2000);
  display.clearDisplay();
  display.display();  // Clear the display
}

void loop() {

  oledPrincipal0();

  unsigned long currentTime = millis(); //unit for time in seconds

  if (currentTime - ultimamedicion >= MODE_CHANGE_INTERVAL) {
    ultimamedicion = currentTime;
    int distance = sensor.readRangeSingleMillimeters() / 10;
    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(15, 0);
    display.print("Distance: ");
    display.setTextSize(4);
    display.setCursor(25, 25);
    display.print(distance);
    display.setTextSize(2);
    display.setCursor(85, 40);
    display.println(" cm");
    display.display();

    // Convert text to speech and play with Emic 2
    //String measurement = "The distance is " + String(distance) + " centimeters";
    String measurement = "La distancia es  " + String(distance) + "centimetros";
    sendTextToEmic(measurement);
  }

  // Read the current state of the button
  //buttonState = digitalRead(buttonPin);

  // Check if the button has been pressed
  //if (buttonState != lastButtonState && buttonState == LOW) {
  // Change the angle of both servos
  //if (angle == 0) {
  // angle = 90;
  //} else {
  //angle = 0;
  //}
  // Move both servos to the calculated angle
  //servo1.write(angle);
  //servo2.write(angle);
  //delay(500);  // Wait a little to debounce
  //}

  // Update the last button state
  //lastButtonState = buttonState;
}

void sendTextToEmic(String text) {
  // Send text to Emic 2 for conversion
  Serial.print("S");  // 'Speak' command
  Serial.println(text);
}

void oledPrincipal0(){
  display.clearDisplay();
  u8g2_for_adafruit_gfx.setForegroundColor(WHITE);      // apply Adafruit GFX color
  u8g2_for_adafruit_gfx.setFont(u8g2_font_luBS08_te);  // select u8g2 font from here: https://github.com/olikraus/u8g2/wiki/fntlistall
  u8g2_for_adafruit_gfx.setCursor(5,10);                // start writing at this position
  u8g2_for_adafruit_gfx.print(F("> Abrir guante"));
  u8g2_for_adafruit_gfx.setFont(u8g2_font_luBS19_te);
  u8g2_for_adafruit_gfx.setCursor(20,25);                // start writing at this position
  u8g2_for_adafruit_gfx.print(F("Medir distancia"));
  u8g2_for_adafruit_gfx.setCursor(20,40);                // start writing at this position
  u8g2_for_adafruit_gfx.print(F("Configuraciones"));
  display.display();
  opt = 1;
}