//#include <Arduino.h>
//U8G2_SSD1306_128X32_UNIVISION_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE, /* clock=*/ SCL, /* data=*/ SDA);   // pin remapping with ESP8266 HW I2C
/**
   Name:     Grove Beginner Kit for Arduino, example code
   Autor:    Alberto Gil Tesa
   Web:      https://giltesa.com/?p=20129
   License:  CC BY-NC-SA 4.0
   Version:  1.0
   Date:     2020/08/29
   https://wokwi.com/projects/334744137095971410
   https://gist.github.com/giltesa/78ca196e30294fe0389192ea9067dcde
*/
//LIBRARIES
//
#include <Wire.h>
#include <U8g2lib.h>
#include "DHTesp.h"
#include <Adafruit_MPU6050.h>
/*
  #include <DHT.h>
  #include <Seeed_BMP280.h>
  #include <LIS3DHTR.h>
*/
//#define DEBUG
//MACROS
//
#define COUNT(x) sizeof(x) / sizeof(*x)
#define TEXT_HOR_ALIGN_CENTER(t) ((u8g2.getDisplayWidth() - (u8g2.getUTF8Width(t))) / 2)
#define TEXT_HOR_ALIGN_RIGHT(t) (u8g2.getDisplayWidth() - u8g2.getUTF8Width(t))
//#define TEXT_HOR_ALIGN_LEFT         0
//#define ICON_HOR_ALIGN_CENTER(z)    ((u8g2.getDisplayWidth()  - z) / 2 )
//#define TEXT_VER_ALIGN_CENTER       ((u8g2.getDisplayHeight() + 4) / 2)
//#define TEXT_VER_ALIGN_DOWN(t)      (u8g2.getDisplayHeight()  -  u8g2.getUTF8Height(t))
//#define TEXT_VER_ALIGN_UP           0
//#define ICON_VER_ALIGN_CENTER(z)    ((u8g2.getDisplayHeight() - z) / 2 )
//GROVE BEGINNER KIT V2 PINOUT
//
const byte pDHT = 15;
const byte pLED = 4;
const byte pBUZZER = 5;
const byte pBTN = 25;
const byte pPOT = 18;
const byte pSOUND = 35;
const byte pLIGHT = 34;
const byte pBTNH = 27;
const byte pBTNL = 26;
//MELODY TONES
//
const int REST = 0, NOTE_B0 = 31, NOTE_C1 = 33, NOTE_CS1 = 35, NOTE_D1 = 37,
          NOTE_DS1 = 39, NOTE_E1 = 41, NOTE_F1 = 44, NOTE_FS1 = 46, NOTE_G1 = 49,
          NOTE_GS1 = 52, NOTE_A1 = 55, NOTE_AS1 = 58, NOTE_B1 = 62, NOTE_C2 = 65,
          NOTE_CS2 = 69, NOTE_D2 = 73, NOTE_DS2 = 78, NOTE_E2 = 82, NOTE_F2 = 87,
          NOTE_FS2 = 93, NOTE_G2 = 98, NOTE_GS2 = 104, NOTE_A2 = 110, NOTE_AS2 = 117,
          NOTE_B2 = 123, NOTE_C3 = 131, NOTE_CS3 = 139, NOTE_D3 = 147, NOTE_DS3 = 156,
          NOTE_E3 = 165, NOTE_F3 = 175, NOTE_FS3 = 185, NOTE_G3 = 196, NOTE_GS3 = 208,
          NOTE_A3 = 220, NOTE_AS3 = 233, NOTE_B3 = 247, NOTE_C4 = 262, NOTE_CS4 = 277,
          NOTE_D4 = 294, NOTE_DS4 = 311, NOTE_E4 = 330, NOTE_F4 = 349, NOTE_FS4 = 370,
          NOTE_G4 = 392, NOTE_GS4 = 415, NOTE_A4 = 440, NOTE_AS4 = 466, NOTE_B4 = 494,
          NOTE_C5 = 523, NOTE_CS5 = 554, NOTE_D5 = 587, NOTE_DS5 = 622, NOTE_E5 = 659,
          NOTE_F5 = 698, NOTE_FS5 = 740, NOTE_G5 = 784, NOTE_GS5 = 831, NOTE_A5 = 880,
          NOTE_AS5 = 932, NOTE_B5 = 988, NOTE_C6 = 1047, NOTE_CS6 = 1109, NOTE_D6 = 1175,
          NOTE_DS6 = 1245, NOTE_E6 = 1319, NOTE_F6 = 1397, NOTE_FS6 = 1480, NOTE_G6 = 1568,
          NOTE_GS6 = 1661, NOTE_A6 = 1760, NOTE_AS6 = 1865, NOTE_B6 = 1976, NOTE_C7 = 2093,
          NOTE_CS7 = 2217, NOTE_D7 = 2349, NOTE_DS7 = 2489, NOTE_E7 = 2637, NOTE_F7 = 2794,
          NOTE_FS7 = 2960, NOTE_G7 = 3136, NOTE_GS7 = 3322, NOTE_A7 = 3520, NOTE_AS7 = 3729,
          NOTE_B7 = 3951, NOTE_C8 = 4186, NOTE_CS8 = 4435, NOTE_D8 = 4699, NOTE_DS8 = 4978;
//DECLARE DEMO FUNCTIONS
//
void demoLed(byte menuSelec);
void demoBuzzer(byte menuSelec);
void demoOled(byte menuSelec);
void demoButton(byte menuSelec);
void demoPotentiometer(byte menuSelec);
void demoLight(byte menuSelec);
void demoSound(byte menuSelec);
void demoTempHumidify(byte menuSelec);
void demoAxis(byte menuSelec);
//void demoAirPress(byte menuSelec);

//TEXTS, FUNCTIONS, AND ICONS PER EACH OPTION OF THE MENU
//
const char *txtMENU[] = {
  "LED",
  "BUZZER",
  "OLED",
  "BUTTON",
  "POTENTIOMETER",
  "LIGHT",
  "SOUND",
  "TEMP. & HUMIDITY",
  "AIR PRESSURE",
  "3-AXIS ACCELEMETER"
};
const char *dscMENU[] = {
  "BLINKING LED",
  "PLAYING GAME MELODIES",
  "SHOWING A 3D CUBE EFFECT",
  "COUNTING BUTTON PRESSES",
  "MODIFYING CONTRAST LEVEL",
  "",
  "",
  "",
  "",
  "MOVING A BALL BY YOURSELF"
};
const byte itemsMENU = COUNT(txtMENU);
typedef void (*func_type)(byte menuSelec);
static func_type funcDEMO[itemsMENU] = {
  &demoLed,
  &demoBuzzer,
  &demoOled,
  &demoButton,
  &demoPotentiometer,
  &demoLight,
  &demoSound,
  &demoTempHumidify,
  &demoAxis
  //&demoAirPress,
};
const unsigned char xbmMENU[itemsMENU][32] U8X8_PROGMEM =  //16x16px
{
  { 0x00, 0x00, 0xe0, 0x01, 0xe0, 0x01, 0x60, 0x3f, 0x7f, 0x7f, 0x7f, 0xc1, 0x60, 0x81, 0x60, 0x8f, 0x60, 0x8f, 0x60, 0x81, 0x7f, 0xc1, 0x7f, 0x7f, 0x60, 0x3f, 0xe0, 0x01, 0xe0, 0x01, 0x00, 0x00 },
  { 0x00, 0x00, 0x80, 0x01, 0xc0, 0x01, 0xe0, 0x01, 0xb0, 0x11, 0x9e, 0x21, 0x8e, 0x45, 0x86, 0x49, 0x86, 0x49, 0x8e, 0x45, 0x9e, 0x21, 0xb0, 0x11, 0xe0, 0x01, 0xc0, 0x01, 0x80, 0x01, 0x00, 0x00 },
  { 0x00, 0x00, 0x00, 0x00, 0xfe, 0x7f, 0xfe, 0x7f, 0x06, 0x60, 0x06, 0x60, 0x06, 0x60, 0x06, 0x60, 0x06, 0x60, 0xfe, 0x7f, 0xfe, 0x7f, 0x80, 0x01, 0x80, 0x01, 0xf0, 0x0f, 0xf0, 0x0f, 0x00, 0x00 },
  { 0x00, 0x00, 0xe0, 0x07, 0xe0, 0x07, 0xe0, 0x07, 0xe0, 0x07, 0xfe, 0x7f, 0xfe, 0x7f, 0x06, 0x60, 0x06, 0x60, 0x36, 0x6c, 0xfe, 0x7f, 0xfe, 0x7f, 0x30, 0x0c, 0x30, 0x0c, 0x30, 0x0c, 0x00, 0x00 },
  { 0x80, 0x01, 0xc0, 0x03, 0xc0, 0x03, 0xc0, 0x03, 0xc0, 0x03, 0xc0, 0x03, 0xfe, 0x7f, 0xff, 0xff, 0x03, 0xc0, 0x03, 0xc0, 0x9b, 0xd9, 0xff, 0xff, 0xfe, 0x7f, 0x98, 0x19, 0x98, 0x19, 0x98, 0x19 },
  { 0x00, 0x00, 0x80, 0x01, 0xc0, 0x03, 0xc0, 0x03, 0xc0, 0x03, 0x60, 0x06, 0x30, 0x0c, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x1a, 0x58, 0x31, 0x8c, 0xe4, 0x27, 0xc2, 0x43, 0x10, 0x08, 0x08, 0x10 },
  { 0x00, 0x00, 0xc0, 0x03, 0xe0, 0x07, 0x60, 0x06, 0x60, 0x06, 0x60, 0x06, 0x60, 0x06, 0xec, 0x37, 0xcc, 0x33, 0x1c, 0x38, 0xf8, 0x1f, 0xe0, 0x07, 0x80, 0x01, 0x80, 0x01, 0xe0, 0x07, 0xf0, 0x0f },
  { 0xe0, 0x00, 0xf0, 0x01, 0x18, 0x03, 0x58, 0x7b, 0x58, 0x03, 0x58, 0x03, 0x58, 0x7b, 0x58, 0x03, 0x58, 0x03, 0x4c, 0x66, 0xe6, 0x0c, 0xf6, 0x0d, 0xe6, 0x6c, 0x0c, 0x06, 0xf8, 0x03, 0xf0, 0x01 },
  { 0x00, 0x00, 0x00, 0x06, 0x00, 0x0d, 0x00, 0x0c, 0xfc, 0x0f, 0xfc, 0x07, 0x00, 0x00, 0xfc, 0x0f, 0xfc, 0x3f, 0x00, 0x30, 0xfc, 0x34, 0xfc, 0x1d, 0x80, 0x01, 0x90, 0x01, 0xe0, 0x00, 0x00, 0x00 },
  { 0x00, 0x00, 0xfc, 0x3f, 0xfe, 0x7f, 0x06, 0x60, 0xf6, 0x6f, 0xf6, 0x6f, 0x06, 0x67, 0x86, 0x63, 0xc6, 0x61, 0xe6, 0x60, 0xf6, 0x6f, 0xf6, 0x6f, 0x06, 0x60, 0xfe, 0x7f, 0xfc, 0x3f, 0x00, 0x00 }
};
//TEXTS AND ICONS PER EACH OPTION OF THE TONE MENU
//
const char *txtTONES[] = {
  "TETRIS",
  "MARIO",
  "ZELDA"
};
const byte itemsTONES = COUNT(txtTONES);
const unsigned char xbmTONES[itemsTONES][100] U8X8_PROGMEM =  //25x25px
{
  { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0x01, 0x00, 0x00, 0xfc, 0x01, 0x7f, 0x00, 0x8c, 0x01, 0x7f, 0x00, 0x8c, 0x01, 0x63, 0x00, 0x8c, 0x01, 0x63, 0xe0, 0xff, 0x01, 0x63, 0xe0, 0xff, 0x01, 0x7f, 0x60, 0x8c, 0x01, 0x7f, 0x60, 0x8c, 0x01, 0x63, 0x60, 0x8c, 0x01, 0x63, 0xe0, 0xff, 0x01, 0x63, 0xe0, 0xff, 0x01, 0x7f, 0x00, 0x8c, 0x01, 0x7f, 0x00, 0x8c, 0x01, 0x63, 0x00, 0x8c, 0x01, 0x63, 0x00, 0xfc, 0x01, 0x63, 0x00, 0xfc, 0x01, 0xff, 0xff, 0x01, 0x00, 0xff, 0xff, 0x01, 0x00, 0x63, 0x8c, 0x01, 0x00, 0x63, 0x8c, 0x01, 0x00, 0x63, 0x8c, 0x01, 0x00, 0xff, 0xff, 0x01, 0x00, 0xff, 0xff, 0x01, 0x00 },
  { 0x00, 0x00, 0x00, 0x00, 0x00, 0xfe, 0x00, 0x00, 0x00, 0xff, 0x01, 0x00, 0xc0, 0x83, 0x07, 0x00, 0xe0, 0x00, 0x0e, 0x00, 0x70, 0x00, 0x1c, 0x00, 0x38, 0x00, 0x38, 0x00, 0x18, 0x42, 0x30, 0x00, 0x0c, 0x66, 0x60, 0x00, 0x0e, 0xe7, 0xe0, 0x00, 0x06, 0xff, 0xc1, 0x00, 0x86, 0xff, 0xc3, 0x00, 0x86, 0xbb, 0xc3, 0x00, 0xc6, 0x93, 0xc7, 0x00, 0xe6, 0x83, 0xcf, 0x00, 0xce, 0x01, 0xe7, 0x00, 0x8c, 0x01, 0x63, 0x00, 0x18, 0x01, 0x71, 0x00, 0x38, 0x00, 0x38, 0x00, 0x70, 0x00, 0x1c, 0x00, 0xe0, 0x00, 0x0e, 0x00, 0xc0, 0x83, 0x07, 0x00, 0x00, 0xff, 0x01, 0x00, 0x00, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
  { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0xcc, 0x00, 0x00, 0x00, 0xce, 0x00, 0x00, 0x00, 0x86, 0x01, 0x00, 0x00, 0x03, 0x03, 0x00, 0x00, 0x03, 0x03, 0x00, 0x80, 0xff, 0x07, 0x00, 0xc0, 0xff, 0x07, 0x00, 0xc0, 0x03, 0x0f, 0x00, 0xe0, 0x86, 0x1d, 0x00, 0x60, 0xce, 0x19, 0x00, 0x30, 0xcc, 0x30, 0x00, 0x30, 0x78, 0x30, 0x00, 0x18, 0x78, 0x60, 0x00, 0xf8, 0xff, 0x7f, 0x00, 0xfc, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
};
//OTHER XBMP IMAGES
//
const unsigned char xMenuUp[] U8X8_PROGMEM = { /*16x16px*/ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0xe0, 0x01, 0xf0, 0x03, 0x38, 0x07, 0x1c, 0x0e, 0x0e, 0x1c, 0x07, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
const unsigned char xMenuDown[] U8X8_PROGMEM = { /*16x16px*/ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x38, 0x0e, 0x1c, 0x1c, 0x0e, 0x38, 0x07, 0xf0, 0x03, 0xe0, 0x01, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
const unsigned char xMenuLeft[] U8X8_PROGMEM = { /*10x10px*/ 0xe0, 0x00, 0x70, 0x00, 0x38, 0x00, 0x1c, 0x00, 0x0e, 0x00, 0x0e, 0x00, 0x1c, 0x00, 0x38, 0x00, 0x70, 0x00, 0xe0, 0x00 };
const unsigned char xMenuRight[] U8X8_PROGMEM = { /*10x10px*/ 0x1c, 0x00, 0x38, 0x00, 0x70, 0x00, 0xe0, 0x00, 0xc0, 0x01, 0xc0, 0x01, 0xe0, 0x00, 0x70, 0x00, 0x38, 0x00, 0x1c, 0x00 };
//OBJECTS
//
#ifdef DEBUG
HardwareSerial &debug = Serial;
#endif
U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);

DHTesp dhtSensor;
Adafruit_MPU6050 mpu;
/*
  DHT dht(pDHT, DHT11);
  BMP280 bmp280;
  LIS3DHTR <TwoWire> accelemeter;
*/
//ENUMS
//
enum Button { ShortPress,
              LongPress,
              Unknown
            };
/**
*/
void setup() {
  Serial.begin(115200);
  pinMode(pLED, OUTPUT);
  pinMode(pBUZZER, OUTPUT);
  pinMode(pBTN, INPUT);
  pinMode(pDHT, INPUT);
  pinMode(pPOT, INPUT);
  pinMode(pSOUND, INPUT);
  pinMode(pLIGHT, INPUT);
  pinMode(pBTNH, INPUT);
  pinMode(pBTNL, INPUT);
  digitalWrite(pBUZZER, LOW);
#ifdef DEBUG
  debug.begin(9600);
  while (!debug);
#endif
  /*
    if (!bmp280.init()) {
    #ifdef DEBUG
      debug.println(F("BMP280 init error!"));
    #endif
    }
    accelemeter.begin(Wire, LIS3DHTR_ADDRESS_UPDATED);
    delay(100);
    accelemeter.setOutputDataRate(LIS3DHTR_DATARATE_50HZ);
    if (!accelemeter) {
    #ifdef DEBUG
      debug.println(F("LIS3DHTR didn't connect!"));
    #endif
    }
  */
  if (mpu.begin()) {
    //mpu.begin();
    delay(10);
    mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
    mpu.setGyroRange(MPU6050_RANGE_250_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  }
  else Serial.println("Failed to find MPU6050 chip");
  dhtSensor.setup(pDHT, DHTesp::DHT22);
  u8g2.begin();
}
/**
*/
void loop() {
  static byte menuSelec = 0;
  static byte prevMenuSelec = menuSelec;
  Button btnPressed;
  //DRAWING THE DEMO SCREEN SELECTED AND STAY THERE UNTIL A LONG PRESS BUTTON:
  //
  funcDEMO[menuSelec](menuSelec);
  //DRAWING THE MENU SCREEN AND STAY THERE UNTIL A SHORT PRESS BUTTON:
  //
  do {
    //menuSelec = readPot(0, 1023, 0, itemsMENU - 1);
    readKey(menuSelec, 1, 0, itemsMENU - 1);
    btnPressed = readButton();
    if (menuSelec != prevMenuSelec) {
      prevMenuSelec = menuSelec;
      beep('S');
    }
    printMenu(menuSelec);
  } while (btnPressed != Button::ShortPress);
}
/**
*/
void printMenu(byte menuSelec) {
  u8g2.firstPage();
  do {
    //ROW 1:
    if (menuSelec > 0) {
      u8g2.drawXBMP(4, 2, 14, 14, xMenuUp);
      u8g2.setFont(u8g2_font_haxrcorp4089_tr);
      u8g2.setCursor(22, 13);
      u8g2.print(txtMENU[menuSelec - 1]);
    }
    //ROW 2:
    u8g2.setFont(u8g2_font_nokiafc22_tu);
    u8g2.drawBox(0, 18, 128, 27);
    u8g2.setColorIndex(0);
    u8g2.drawXBMP(3, 24, 16, 16, xbmMENU[menuSelec]);
    if (strlen(dscMENU[menuSelec]) == 0) {
      u8g2.drawStr(22, 35, txtMENU[menuSelec]);
    } else {
      u8g2.drawStr(22, 30, txtMENU[menuSelec]);
      u8g2.setFont(u8g2_font_micro_mr);
      u8g2.drawStr(22, 40, dscMENU[menuSelec]);
    }
    u8g2.setColorIndex(1);
    //ROW 3:
    if (menuSelec < itemsMENU - 1) {
      u8g2.drawXBMP(4, 48, 14, 14, xMenuDown);
      u8g2.setFont(u8g2_font_haxrcorp4089_tr);
      u8g2.setCursor(22, 58);
      u8g2.print(txtMENU[menuSelec + 1]);
    }
    drawFrame();
  } while (u8g2.nextPage());
}
/**
   CHANGING THE BLINKING SPEED OF THE RED LED
   The code is doing a lot of things, so it doesn't have time to blink at less than 85ms
*/
void demoLed(byte menuSelec) {
  unsigned long time = 0;
  static unsigned long prevTime = time;
  static byte interval;
  static byte blinking;
  Button btnPressed;
  do {
    time = millis();
    //interval = readPot(0, 1023, 0, 100);
    //blinking = readPot(0, 1023, 0, 1000);
    readKey(blinking, 1, 0, 255);
    interval = map(blinking, 0, 255, 0, 100);
    btnPressed = readButton();
    if (time - prevTime >= blinking) {
      prevTime = time;
      digitalWrite(pLED, !digitalRead(pLED));
    }
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.drawFrame(13, 30, 102, 12);
      u8g2.drawFrame(14, 31, 100, 10);
      for (int x = 0; x < interval; x++) {
        u8g2.drawVLine(15 + x, 32, 8);
      }
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(14, 51);
      u8g2.print(F("MILLISECONDS:"));
      u8g2.setCursor(TEXT_HOR_ALIGN_RIGHT(String(blinking).c_str()) - 14, 51);
      u8g2.print(blinking);
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
  digitalWrite(pLED, LOW);
}
/**
   PLAYING A LIST OF TONES
   They have had to be shorted because ATmega328p doesn't have memory enough.
*/
void demoBuzzer(byte menuSelec) {
  static byte toneSelec = 0;
  static byte prevToneSelec = toneSelec;
  Button btnPressed;
  do {
    //toneSelec = readPot(0, 1023, 0, itemsTONES - 1);
    readKey(toneSelec, 1, 0, itemsTONES - 1);
    btnPressed = readButton();
    if (toneSelec != prevToneSelec) {
      prevToneSelec = toneSelec;
      beep('S');
    }
    if (btnPressed == Button::ShortPress) {
      playTone(toneSelec);
    }
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.drawXBMP(51, 23, 25, 25, xbmTONES[toneSelec]);
      if (toneSelec > 0) {
        u8g2.drawHLine(1, 50, 11);
        u8g2.drawHLine(1, 51, 11);
        u8g2.drawHLine(1, 52, 11);
        u8g2.drawHLine(1, 53, 11);
        u8g2.drawHLine(1, 54, 11);
        u8g2.drawHLine(1, 55, 11);
        u8g2.drawHLine(1, 56, 11);
        u8g2.drawHLine(1, 57, 11);
        u8g2.drawHLine(1, 58, 11);
        u8g2.drawHLine(1, 59, 11);
        u8g2.drawHLine(1, 60, 11);
        u8g2.drawHLine(2, 61, 10);
        u8g2.drawHLine(3, 62, 9);
        u8g2.setColorIndex(0);
        u8g2.drawXBMP(2, 52, 10, 10, xMenuLeft);
        u8g2.setColorIndex(1);
      }
      if (toneSelec < itemsTONES - 1) {
        u8g2.drawHLine(116, 50, 11);
        u8g2.drawHLine(116, 51, 11);
        u8g2.drawHLine(116, 52, 11);
        u8g2.drawHLine(116, 53, 11);
        u8g2.drawHLine(116, 54, 11);
        u8g2.drawHLine(116, 55, 11);
        u8g2.drawHLine(116, 56, 11);
        u8g2.drawHLine(116, 57, 11);
        u8g2.drawHLine(116, 58, 11);
        u8g2.drawHLine(116, 59, 11);
        u8g2.drawHLine(116, 60, 11);
        u8g2.drawHLine(116, 61, 10);
        u8g2.drawHLine(116, 62, 9);
        u8g2.setColorIndex(0);
        u8g2.drawXBMP(116, 52, 10, 10, xMenuRight);
        u8g2.setColorIndex(1);
      }
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(TEXT_HOR_ALIGN_CENTER(txtTONES[toneSelec]), 60);
      u8g2.print(txtTONES[toneSelec]);
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
}
/**
   SHOWING A 3D CUBE EFFECT.
   ATmega328p doesn't have memory enough, so using this board you can't see this demo.
*/
void demoOled(byte menuSelec) {
#if defined(__AVR_ATmega328P__)
  Button btnPressed;
  do {
    btnPressed = readButton();
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(10, 38);
      u8g2.print(F("INSUFFICIENT MEMORY"));
      u8g2.setCursor(10, 48);
      u8g2.print(F("TO LOAD THE 3D DEMO"));
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
#else
  static float rot, rotx, roty, rotz, rotxx, rotyy, rotzz, rotxxx, rotyyy, rotzzz;
  static int wireframe[24][2];
  static float vector;
  const int originx = 64;
  const int originy = 32;
  Button btnPressed;
  do {
    //cube vertices - {x, y, z}
    int cube_vertex[8][3] = {
      { -20, -20, 20 },
      { 20, -20, 20 },
      { 20, 20, 20 },
      { -20, 20, 20 },
      { -20, -20, -20 },
      { 20, -20, -20 },
      { 20, 20, -20 },
      { -20, 20, -20 }
    };
    for (int angle = 0; angle <= 360; angle = angle + 3) {
      btnPressed = readButton();
      if (btnPressed == Button::LongPress) {
        break;
      }
      u8g2.firstPage();
      do {
        for (int i = 0; i < 8; i++) {
          rot = angle * 0.0174532;  //0.0174532 = one degree
          //rotateY
          rotz = cube_vertex[i][2] * cos(rot) - cube_vertex[i][0] * sin(rot);
          rotx = cube_vertex[i][2] * sin(rot) + cube_vertex[i][0] * cos(rot);
          roty = cube_vertex[i][1];
          //rotateX
          rotyy = roty * cos(rot) - rotz * sin(rot);
          rotzz = roty * sin(rot) + rotz * cos(rot);
          rotxx = rotx;
          //rotateZ
          rotxxx = rotxx * cos(rot) - rotyy * sin(rot);
          rotyyy = rotxx * sin(rot) + rotyy * cos(rot);
          rotzzz = rotzz;
          //orthographic projection
          rotxxx = rotxxx + originx;
          rotyyy = rotyyy + originy;
          //store new vertices values for wireframe drawing
          wireframe[i][0] = rotxxx;
          wireframe[i][1] = rotyyy;
          wireframe[i][2] = rotzzz;
        }
        //Draw wireframe:
        //Face A
        vector = {
          (wireframe[0][0] * wireframe[1][1] - wireframe[0][1] * wireframe[1][0]) + (wireframe[1][0] * wireframe[2][1] - wireframe[1][1] * wireframe[2][0]) + (wireframe[2][0] * wireframe[3][1] - wireframe[2][1] * wireframe[3][0]) + (wireframe[3][0] * wireframe[0][1] - wireframe[3][1] * wireframe[0][0])
        };
        if (vector >= 0) {
          u8g2.drawLine(wireframe[0][0], wireframe[0][1], wireframe[1][0], wireframe[1][1]);
          u8g2.drawLine(wireframe[1][0], wireframe[1][1], wireframe[2][0], wireframe[2][1]);
          u8g2.drawLine(wireframe[2][0], wireframe[2][1], wireframe[3][0], wireframe[3][1]);
          u8g2.drawLine(wireframe[3][0], wireframe[3][1], wireframe[0][0], wireframe[0][1]);
        }
        //Face B
        vector = {
          (wireframe[0][0] * wireframe[4][1] - wireframe[0][1] * wireframe[4][0]) + (wireframe[4][0] * wireframe[5][1] - wireframe[4][1] * wireframe[5][0]) + (wireframe[5][0] * wireframe[1][1] - wireframe[5][1] * wireframe[1][0]) + (wireframe[1][0] * wireframe[0][1] - wireframe[1][1] * wireframe[0][0])
        };
        if (vector >= 0) {
          u8g2.drawLine(wireframe[0][0], wireframe[0][1], wireframe[4][0], wireframe[4][1]);
          u8g2.drawLine(wireframe[4][0], wireframe[4][1], wireframe[5][0], wireframe[5][1]);
          u8g2.drawLine(wireframe[5][0], wireframe[5][1], wireframe[1][0], wireframe[1][1]);
          u8g2.drawLine(wireframe[1][0], wireframe[1][1], wireframe[0][0], wireframe[0][1]);
        }
        //Face C
        vector = {
          (wireframe[4][0] * wireframe[5][1] - wireframe[4][1] * wireframe[5][0]) + (wireframe[5][0] * wireframe[6][1] - wireframe[5][1] * wireframe[6][0]) + (wireframe[6][0] * wireframe[7][1] - wireframe[6][1] * wireframe[7][0]) + (wireframe[7][0] * wireframe[4][1] - wireframe[7][1] * wireframe[4][0])
        };
        if (vector <= 0) {
          u8g2.drawLine(wireframe[4][0], wireframe[4][1], wireframe[5][0], wireframe[5][1]);
          u8g2.drawLine(wireframe[5][0], wireframe[5][1], wireframe[6][0], wireframe[6][1]);
          u8g2.drawLine(wireframe[6][0], wireframe[6][1], wireframe[7][0], wireframe[7][1]);
          u8g2.drawLine(wireframe[7][0], wireframe[7][1], wireframe[4][0], wireframe[4][1]);
        }
        //Face D
        vector = {
          (wireframe[3][0] * wireframe[7][1] - wireframe[3][1] * wireframe[7][0]) + (wireframe[7][0] * wireframe[6][1] - wireframe[7][1] * wireframe[6][0]) + (wireframe[6][0] * wireframe[2][1] - wireframe[6][1] * wireframe[2][0]) + (wireframe[2][0] * wireframe[3][1] - wireframe[2][1] * wireframe[3][0])
        };
        if (vector <= 0) {
          u8g2.drawLine(wireframe[3][0], wireframe[3][1], wireframe[7][0], wireframe[7][1]);
          u8g2.drawLine(wireframe[7][0], wireframe[7][1], wireframe[6][0], wireframe[6][1]);
          u8g2.drawLine(wireframe[6][0], wireframe[6][1], wireframe[2][0], wireframe[2][1]);
          u8g2.drawLine(wireframe[2][0], wireframe[2][1], wireframe[3][0], wireframe[3][1]);
        }
        //Face E
        vector = {
          (wireframe[0][0] * wireframe[4][1] - wireframe[0][1] * wireframe[4][0]) + (wireframe[4][0] * wireframe[7][1] - wireframe[4][1] * wireframe[7][0]) + (wireframe[7][0] * wireframe[3][1] - wireframe[7][1] * wireframe[3][0]) + (wireframe[3][0] * wireframe[0][1] - wireframe[3][1] * wireframe[0][0])
        };
        if (vector <= 0) {
          u8g2.drawLine(wireframe[0][0], wireframe[0][1], wireframe[4][0], wireframe[4][1]);
          u8g2.drawLine(wireframe[4][0], wireframe[4][1], wireframe[7][0], wireframe[7][1]);
          u8g2.drawLine(wireframe[7][0], wireframe[7][1], wireframe[3][0], wireframe[3][1]);
          u8g2.drawLine(wireframe[3][0], wireframe[3][1], wireframe[0][0], wireframe[0][1]);
        }
        //Face F
        vector = {
          (wireframe[1][0] * wireframe[5][1] - wireframe[1][1] * wireframe[5][0]) + (wireframe[6][0] * wireframe[6][1] - wireframe[5][1] * wireframe[6][0]) + (wireframe[6][0] * wireframe[2][1] - wireframe[6][1] * wireframe[2][0]) + (wireframe[2][0] * wireframe[1][1] - wireframe[2][1] * wireframe[1][0])
        };
        if (vector >= 0) {
          u8g2.drawLine(wireframe[1][0], wireframe[1][1], wireframe[5][0], wireframe[5][1]);
          u8g2.drawLine(wireframe[5][0], wireframe[5][1], wireframe[6][0], wireframe[6][1]);
          u8g2.drawLine(wireframe[6][0], wireframe[6][1], wireframe[2][0], wireframe[2][1]);
          u8g2.drawLine(wireframe[2][0], wireframe[2][1], wireframe[1][0], wireframe[1][1]);
        }
      } while (u8g2.nextPage());
    }
  } while (btnPressed != Button::LongPress);
#endif
}
/**
   COUNTING THE NUMBER OF TIMES THE BUTTON IS PRESSED
   Sometimes the ATmega328p can't read the pulsation because it's doing another thing.
   it can be fixed using interruptions but you must connect the button to a compatible pin,
   for example to the D2 or D3 pint (not D6)
*/
void demoButton(byte menuSelec) {
  unsigned long time = 0;
  static unsigned long prevTime = time;
  static unsigned int pulsations = 0;
  Button btnPressed;
  do {
    time = millis();
    btnPressed = readButton();
    if (btnPressed == Button::ShortPress && time - prevTime >= 250) {
      prevTime = time;
      pulsations++;
    }
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(14, 43);
      u8g2.print(F("PULSATIONS:"));
      u8g2.setCursor(TEXT_HOR_ALIGN_RIGHT(String(pulsations).c_str()) - 14, 43);
      u8g2.print(pulsations);
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
}
/**
   CHANGING THE CONTRACTS OF THE OLED SCREEN
*/
void demoPotentiometer(byte menuSelec) {
  byte interval;
  byte contrast;
  Button btnPressed;
  do {
    //interval = readPot(0, 1023, 0, 100);
    //contrast = readPot(0, 1023, 0, 255);
    readKey(contrast, 1, 0, 255);
    interval = map(contrast, 0, 255, 0, 100);
    btnPressed = readButton();
    u8g2.setContrast(contrast);
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.drawFrame(13, 30, 102, 12);
      u8g2.drawFrame(14, 31, 100, 10);
      for (int x = 0; x < interval; x++) {
        u8g2.drawVLine(15 + x, 32, 8);
      }
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(14, 51);
      u8g2.print(F("CONTRAST:"));
      u8g2.setCursor(TEXT_HOR_ALIGN_RIGHT(String(interval).c_str()) - 14, 51);
      u8g2.print(interval);
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
}
/**
*/
void demoLight(byte menuSelec) {
  unsigned int value;
  byte disc;
  Button btnPressed;
  do {
    value = analogRead(pLIGHT);
    disc = mapPlus1(value, 0, 4095, 0, 4);
    btnPressed = readButton();
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.drawCircle(105, 39, 8, U8G2_DRAW_ALL);
      switch (disc) {
        case 1:
          u8g2.drawDisc(105, 39, 8, U8G2_DRAW_UPPER_LEFT);
          break;
        case 2:
          u8g2.drawDisc(105, 39, 8, U8G2_DRAW_UPPER_RIGHT | U8G2_DRAW_UPPER_LEFT);
          break;
        case 3:
          u8g2.drawDisc(105, 39, 8, U8G2_DRAW_LOWER_LEFT | U8G2_DRAW_UPPER_RIGHT | U8G2_DRAW_UPPER_LEFT);
          break;
        case 4:
          u8g2.drawDisc(105, 39, 8, U8G2_DRAW_ALL);
          break;
      }
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(14, 38);
      u8g2.print(F("LIGHT LEVEL:"));
      u8g2.setCursor(14, 48);
      u8g2.print(value);
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
}
/**
*/
void demoSound(byte menuSelec) {
  int value;
  const byte lenSoundQueue = 10;
  int soundQueue[lenSoundQueue] = { 0 };
  Button btnPressed;
  for (int i = 0; i < lenSoundQueue; i++) {
    soundQueue[i] = analogRead(pSOUND);
  }
  do {
    value = soundFilter(analogRead(pSOUND), soundQueue, lenSoundQueue);
    btnPressed = readButton();
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(14, 43);
      u8g2.print(F("SOUND LEVEL:"));
      u8g2.setCursor(TEXT_HOR_ALIGN_RIGHT(String(value).c_str()) - 14, 43);
      u8g2.print(value);
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
}
/**
   The DHT11 sensor has a precision of 1, so it doesn't have decimals and the code use int variable instead of float.
   Change it for float if you decide to change the DHT11 for a DHT22 sensor.
*/
void demoTempHumidify(byte menuSelec) {
  /*
    int humid;
    int temp;
    float tempMcu;
  */
  Button btnPressed;
  do {
    /*
      humid = (int)dht.readHumidity();
      tempMcu = getMcuTemp();
      temp = (int)dht.readTemperature();
    */
    TempAndHumidity data = dhtSensor.getTempAndHumidity();
    String temp = String(data.temperature, 2);
    String humid = String(data.humidity, 1);
    String tempMcu = String(getMcuTemp());
    //Serial.println("Temperature: " + String(data.temperature, 2) + "°C");
    btnPressed = readButton();
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(14, 31);
      u8g2.print(F("HUMIDITY:"));
      u8g2.setCursor(TEXT_HOR_ALIGN_RIGHT(String(humid).c_str()) - 22, 31);
      u8g2.print(humid);
      u8g2.setCursor(109, 31);
      u8g2.print(F("%"));
      u8g2.setCursor(14, 44);
      u8g2.print(F("TEMPERATURE:"));
      u8g2.setCursor(TEXT_HOR_ALIGN_RIGHT(String(temp).c_str()) - 22, 44);
      u8g2.print(temp);
      u8g2.setCursor(109, 44);
      u8g2.print(F("C"));
      u8g2.setCursor(14, 57);
      u8g2.print(F("TEMP. MCU:"));
      u8g2.setCursor(TEXT_HOR_ALIGN_RIGHT(String(tempMcu).c_str()) - 22, 57);
      u8g2.print(tempMcu);
      u8g2.setCursor(109, 57);
      u8g2.print(F("C"));
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
}
/**
*/
/*
  void demoAirPress(byte menuSelec) {
  long pressure;
  Button btnPressed;
  do {
    pressure = (long)bmp280.getPressure();
    btnPressed = readButton();
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(14, 43);
      u8g2.print(F("PRESSURE:"));
      u8g2.setCursor(TEXT_HOR_ALIGN_RIGHT(String(pressure).c_str()) - 27, 43);
      u8g2.print(pressure);
      u8g2.setCursor(104, 43);
      u8g2.print(F("PA"));
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
  }
*/
/**
   This module is soldering in the wrong position,
   so I  have needed to invert the axis to coincide the orientation with the board:
   X = This axis is the axis Y from the sensor, it has inverting positive numbers to negative and vice versa.
   Y = This axis is the axis X from the sensor.
   Z = Z
*/
void demoAxis(byte menuSelec) {
  //Modify these if you want:
  static const byte sfx = 50;  //Start Frame X
  static const byte sfy = 17;  //Start Frame Y
  static const byte width = 77;
  static const byte height = 46;
  static const byte szBall = 2;  //Size Ball
  //Don't modify these:
  static const byte efx = sfx + width;
  static const byte efy = sfy + height;
  static const byte sbfx = sfx + 2;
  static const byte sbfy = sfy + 2;
  static const byte ebfx = efx - 2;
  static const byte ebfy = efy - 2;
  static byte x = sfx + width / 2;
  static byte y = sfy + height / 2;
  float ax, ay, az;
  Button btnPressed;
  do {
    /*
      ax = accelemeter.getAccelerationY() * -1.0;  //Check commentary.
      ay = accelemeter.getAccelerationX();         //Check commentary.
      az = accelemeter.getAccelerationZ();
    */
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    ax = a.acceleration.x;
    ay = a.acceleration.y;
    az = a.acceleration.z;
    Serial.println("Gyro_x: " + String(g.gyro.x, 2));
    Serial.println("Gyro_y: " + String(g.gyro.y, 2));
    Serial.println("Gyro_z: " + String(g.gyro.z, 2));
    Serial.println("Mpu_temp: " + String(temp.temperature, 2));
    btnPressed = readButton();
    if (x >= sbfx) {
      if (ax < -0.2 && ax > -0.5 && x - 1 >= sbfx) {
        x -= 1;
      } else if (ax <= -0.5 && ax > -0.8 && x - 2 >= sbfx) {
        x -= 2;
      } else if (ax <= -0.8 && x - 3 >= sbfx) {
        x -= 3;
      }
    }
    if (x <= ebfx) {
      if (ax > 0.2 && ax < 0.5 && x + 1 <= ebfx) {
        x += 1;
      } else if (ax >= 0.5 && ax < 0.8 && x + 2 <= ebfx) {
        x += 2;
      } else if (ax >= 0.8 && x + 3 <= ebfx) {
        x += 3;
      }
    }
    if (y >= sbfy) {
      if (ay > 0.2 && ay < 0.5 && y - 1 >= sbfy) {
        y -= 1;
      } else if (ay >= 0.5 && ay < 0.8 && y - 2 >= sbfy) {
        y -= 2;
      } else if (ay >= 0.8 && y - 3 >= sbfy) {
        y -= 3;
      }
    }
    if (y <= ebfy) {
      if (ay < -0.2 && ay > -0.5 && y + 1 <= ebfy) {
        y += 1;
      } else if (ay <= -0.5 && ay > -0.8 && y + 2 <= ebfy) {
        y += 2;
      } else if (ay <= -0.8 && y + 3 <= ebfy) {
        y += 3;
      }
    }
    u8g2.firstPage();
    do {
      drawFrame();
      drawHeader(menuSelec);
      u8g2.setFont(u8g2_font_nokiafc22_tu);
      u8g2.setCursor(4, 31);
      u8g2.print(F("X:"));
      u8g2.setCursor(20, 31);
      u8g2.print(ax);
      u8g2.setCursor(4, 44);
      u8g2.print(F("Y:"));
      u8g2.setCursor(20, 44);
      u8g2.print(ay);
      u8g2.setCursor(4, 57);
      u8g2.print(F("Z:"));
      u8g2.setCursor(20, 57);
      u8g2.print(az);
      //u8g2.drawLine(sfx, sfy, efx, sfy);
      //u8g2.drawLine(efx, sfy, efx, efy);
      //u8g2.drawLine(efx, efy, sfx, efy);
      u8g2.drawLine(sfx, sfy, sfx, efy);
      u8g2.drawDisc(x, y, szBall, U8G2_DRAW_ALL);
    } while (u8g2.nextPage());
  } while (btnPressed != Button::LongPress);
}
/**
   This function reads the button pulsations and detect when the user does a short or long pulsation.
*/
Button readButton() {
  unsigned long time = 0;
  static unsigned long tPrevBlink = 0;
  static unsigned long tBtnPressed = 0;
  static unsigned long tBtnReleased = 0;
  const unsigned long tBlinking = 50;
  const unsigned long tShortPress = 500;
  static bool bForceExit = false;
  Button btnPressed = Button::Unknown;
  if (digitalRead(pBTN) && tBtnPressed == 0 && tBtnReleased == 0) {
    tBtnPressed = millis();
    digitalWrite(pLED, HIGH);
  } else if (digitalRead(pBTN) && tBtnPressed > 0 && tBtnReleased == 0 && !bForceExit) {
    time = millis();
    bForceExit = (time - tBtnPressed > tShortPress);
    if (time - tPrevBlink >= tBlinking) {
      tPrevBlink = time;
      digitalWrite(pLED, !digitalRead(pLED));
    }
  } else if ((!digitalRead(pBTN) || bForceExit) && tBtnPressed > 0 && tBtnReleased == 0) {
    tBtnReleased = millis();
    digitalWrite(pLED, LOW);
  } else if (tBtnPressed > 0 && tBtnReleased > 0) {
    if (tBtnReleased - tBtnPressed <= tShortPress) {
      beep('S');
      btnPressed = Button::ShortPress;
    } else {
      beep('L');
      btnPressed = Button::LongPress;
      if (bForceExit) {
        while (digitalRead(pBTN));
        delay(250);
      }
    }
    tBtnPressed = 0;
    tBtnReleased = 0;
    bForceExit = false;
  }
  return btnPressed;
}
/**
   The potentiometer module returns a value of 1023 when is rotate on the left and 0 when is rotate on the right.
   I think it's working in the wrong way, left should be 0 and right 1023, so I inverted the values.
*/
int readPot(int minIn, int maxIn, int minOut, int maxOut) {
  return map(mapPlus1(analogRead(pPOT), minIn, maxIn, minOut, maxOut), 0, maxOut, maxOut, 0);
}
/**
   The map() function has some problems when the difference entre the in_min and in_max is too big between out_min and out_max.
   This function solves this problem.
   https://github.com/arduino/ArduinoCore-API/issues/51#issuecomment-69585614
*/
long mapPlus1(long x, long in_min, long in_max, long out_min, long out_max) {
  return (x - in_min) * (out_max - out_min + 1) / (in_max - in_min + 1) + out_min;
}
/**
   Some utilities to draw the user interface and not to repeat the code.
*/
void readKey(byte &count, byte countStep, byte countMin, byte countMax)
{ if (digitalRead(pBTNH) != digitalRead(pBTNL)) {
    delay(20);
    if (digitalRead(pBTNH) != digitalRead(pBTNL)) {
      unsigned long buttonmillis = millis();
      if (digitalRead(pBTNH) == HIGH) {
        count = constrain(count + countStep, countMin, countMax);
        while (digitalRead(pBTNH) == HIGH) {
          if (millis() - buttonmillis >= 500) {
            count = constrain(count + 20, countMin, countMax);
            break;
          }
        }
      } else {
        count = constrain(count - countStep, countMin, countMax);
        while (digitalRead(pBTNL) == HIGH) {
          if (millis() - buttonmillis >= 500) {
            count = constrain(count - 20, countMin, countMax);
            break;
          }
        }
      }
    }
  }
}
/**
*/
void drawFrame() {
  u8g2.drawRFrame(0, 0, 128, 64, 5);
}
void drawHeader(byte menuSelec) {
  u8g2.drawHLine(2, 1, 124);
  u8g2.drawHLine(1, 2, 126);
  u8g2.drawHLine(0, 3, 128);
  u8g2.drawBox(0, 4, 128, 14);
  u8g2.setColorIndex(0);
  u8g2.drawXBMP(4, 1, 16, 16, xbmMENU[menuSelec]);
  u8g2.setFont(u8g2_font_nokiafc22_tu);
  u8g2.drawStr(22, 12, txtMENU[menuSelec]);
  u8g2.setColorIndex(1);
}
/**
*/
void beep(char mode) {
  tone(pBUZZER, 300);
  switch (mode) {
    case 'S': delay(10); break;
    case 'L': delay(200); break;
  }
  noTone(pBUZZER);
}
/**
   All melodies are from Robson Couto, 2019
   https://github.com/robsoncouto/arduino-songs
   1. Tetris
   2. Super Mario Bros
   3. The legend of Zelda
*/
void playTone(int playTone) {
  int *pMelody, tempo, notes, wholenote, divider = 0, noteDuration = 0;
  if (playTone == 0) {
    static const int melody[] = { NOTE_E5, 4, NOTE_B4, 8, NOTE_C5, 8, NOTE_D5, 4, NOTE_C5, 8, NOTE_B4, 8, NOTE_A4, 4, NOTE_A4, 8, NOTE_C5, 8, NOTE_E5, 4, NOTE_D5, 8, NOTE_C5, 8, NOTE_B4, -4, NOTE_C5, 8, NOTE_D5, 4, NOTE_E5, 4, NOTE_C5, 4, NOTE_A4, 4, NOTE_A4, 8, NOTE_A4, 4, NOTE_B4, 8, NOTE_C5, 8, NOTE_D5, -4, NOTE_F5, 8, NOTE_A5, 4, NOTE_G5, 8, NOTE_F5, 8, NOTE_E5, -4, NOTE_C5, 8, NOTE_E5, 4, NOTE_D5, 8, NOTE_C5, 8, NOTE_B4, 4, NOTE_B4, 8, NOTE_C5, 8, NOTE_D5, 4, NOTE_E5, 4, NOTE_C5, 4, NOTE_A4, 4, NOTE_A4, 4, REST, 4 };
    pMelody = (int*) &melody;
    tempo = 144;
    notes = sizeof(melody) / sizeof(melody[0]) / 2;
  } else if (playTone == 1) {
    static const int melody[] = { NOTE_E5, 8, NOTE_E5, 8, REST, 8, NOTE_E5, 8, REST, 8, NOTE_C5, 8, NOTE_E5, 8, NOTE_G5, 4, REST, 4, NOTE_G4, 8, REST, 4, NOTE_C5, -4, NOTE_G4, 8, REST, 4, NOTE_E4, -4, NOTE_A4, 4, NOTE_B4, 4, NOTE_AS4, 8, NOTE_A4, 4, NOTE_G4, -8, NOTE_E5, -8, NOTE_G5, -8, NOTE_A5, 4, NOTE_F5, 8, NOTE_G5, 8, REST, 8, NOTE_E5, 4, NOTE_C5, 8, NOTE_D5, 8, NOTE_B4, -4, NOTE_C5, -4, NOTE_G4, 8, REST, 4, NOTE_E4, -4, NOTE_A4, 4, NOTE_B4, 4, NOTE_AS4, 8, NOTE_A4, 4, NOTE_G4, -8, NOTE_E5, -8, NOTE_G5, -8, NOTE_A5, 4, NOTE_F5, 8, NOTE_G5, 8, REST, 8, NOTE_E5, 4, NOTE_C5, 8, NOTE_D5, 8, NOTE_B4, -4 };
    pMelody = (int*) &melody;
    tempo = 200;
    notes = sizeof(melody) / sizeof(melody[0]) / 2;
  } else if (playTone == 2) {
    static const int melody[] = { NOTE_AS4, 4, NOTE_F4, -4, NOTE_AS4, 8, NOTE_AS4, 16, NOTE_C5, 16, NOTE_D5, 16, NOTE_DS5, 16, NOTE_F5, 2, NOTE_F5, 8, NOTE_F5, 8, NOTE_F5, 8, NOTE_FS5, 16, NOTE_GS5, 16, NOTE_AS5, -2, NOTE_AS5, 8, NOTE_AS5, 8, NOTE_GS5, 8, NOTE_FS5, 16, NOTE_GS5, -8, NOTE_FS5, 16, NOTE_F5, 2 };
    pMelody = (int*) &melody;
    tempo = 88;
    notes = sizeof(melody) / sizeof(melody[0]) / 2;
  } else {
#ifdef DEBUG
    debug.println(F("Unknown melody!"));
#endif
    return;
  }
  wholenote = (60000 * 4) / tempo;
  for (int thisNote = 0; thisNote < notes * 2; thisNote = thisNote + 2) {
    divider = *(pMelody + thisNote + 1);
    if (readButton() == Button::ShortPress) {
      break;
    }
    if (divider > 0) {
      noteDuration = (wholenote) / divider;
    } else if (divider < 0) {
      noteDuration = (wholenote) / abs(divider);
      noteDuration *= 1.5;
    }
    tone(pBUZZER, *(pMelody + thisNote), noteDuration * 0.9);
    delay(noteDuration);
    noTone(pBUZZER);
  }
}
/**
*/
int soundFilter(int NEW_DATA, int QUEUE[], char n) {
  int max;
  int min;
  int sum;
  char i;
  QUEUE[0] = NEW_DATA;
  if (QUEUE[0] < 0) {
    QUEUE[0] = 0;
  }
  max = QUEUE[0];
  min = QUEUE[0];
  sum = QUEUE[0];
  for (i = n - 1; i != 0; i--) {
    if (QUEUE[i] > max) {
      max = QUEUE[i];
    } else if (QUEUE[i] < min) {
      min = QUEUE[i];
    }
    sum = sum + QUEUE[i];
    QUEUE[i] = QUEUE[i - 1];
  }
  i = n - 2;
  sum = sum - max - min + i / 2;
  sum = sum / i;
  return ((int)sum);
}
/**
   Get the temperature from the microcontroller, not all microcontroller has an internal temperature sensor.
*/
double getMcuTemp() {
  /*
    ADMUX = (_BV(REFS1) | _BV(REFS0) | _BV(MUX3));
    ADCSRA |= _BV(ADEN);
    delay(20);
    ADCSRA |= _BV(ADSC);
    while (bit_is_set(ADCSRA, ADSC));
    return (ADCW - 324.31) / 1.22;
  */
  return 35;
}
/**
   http://jeelabs.org/2011/05/22/atmega-memory-use
*/
/*
  int freeRam()
  {
  extern int __heap_start, *__brkval;
  int v;
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
  }
*/