const int BUTTON_NUM = 5;
const int upButton = 3; // Up Button
const int downButton = 6;  // Down Button
const int resetButton = 7;  // Enter Button
const int leftButton = 10;  // Left Button
const int rightButton = 4;  // Right Button
int i;
int j;
int k;
int l;
int m;
int n;
//#include <Button2.h>
#include <math.h>
//#include "I2Cdev.h"
#include "Wire.h"
#include <Adafruit_GFX.h>                     // Calls up the OLED graphics folder.
//#include <fonts/Org_01.h>
#include <Adafruit_SSD1306.h>                 // Calls up the OLED display folder.
#define OUTPUT_READABLE_YAWPITCHROLL          // Holds the angular information.
//#define INTERRUPT_PIN 7
#define SEALEVELPRESSURE_HPA (1013.25)
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_RESET -1
Adafruit_SSD1306 display(OLED_RESET);
#define LOGO16_GLCD_HEIGHT 32                  // Sets the display to 032 bits high
#define LOGO16_GLCD_WIDTH  128                 // Sets the display to 128 bits long


// The following code is the bitmap that displays the BenderBud logo on the LCD device.

static const unsigned char PROGMEM logo16_glcd_bmp[] =
{
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x7C, 0x00, 0x78, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x03, 0xFE, 0x03, 0xFC, 0x01, 0x80, 0x78, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x0F, 0x9E, 0x07, 0xB8, 0x07, 0xC0, 0xF0, 0x3E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x3C, 0x1C, 0x1E, 0x38, 0x03, 0x00, 0xE0, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x78, 0x3C, 0x78, 0x78, 0x00, 0x01, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00,
  0x00, 0x01, 0xE0, 0x38, 0xF0, 0x70, 0x00, 0x01, 0xC0, 0x00, 0x00, 0x10, 0x00, 0xF0, 0x0F, 0x00,
  0x00, 0x03, 0xC0, 0x7B, 0xC0, 0xF0, 0x1E, 0x03, 0xC0, 0xF0, 0x03, 0xFF, 0x00, 0xF0, 0x0F, 0x00,
  0x00, 0x07, 0x80, 0x7F, 0x80, 0xE0, 0x1C, 0x07, 0x80, 0xF0, 0x0F, 0x0F, 0x01, 0xE0, 0x1E, 0x00,
  0x00, 0x0F, 0x00, 0xFE, 0x01, 0xE0, 0x3C, 0x07, 0x81, 0xE0, 0x3C, 0x3E, 0x03, 0xC0, 0x1E, 0x00,
  0x00, 0x1E, 0x00, 0xFC, 0x01, 0xE0, 0x78, 0x0F, 0x01, 0xC0, 0xFF, 0xF8, 0x03, 0xC0, 0x3C, 0x00,
  0x00, 0x3C, 0x01, 0xF0, 0x01, 0xC0, 0x78, 0x0F, 0x03, 0xC0, 0xF4, 0x00, 0x07, 0x80, 0xFC, 0x00,
  0x00, 0x78, 0x03, 0xE0, 0x03, 0xC0, 0xF0, 0x1E, 0x07, 0x80, 0xE0, 0x03, 0x87, 0x81, 0xFC, 0x00,
  0x00, 0xF0, 0x03, 0xC0, 0x03, 0x80, 0xE0, 0x1E, 0x07, 0x80, 0xF8, 0x7F, 0x03, 0xCF, 0x1C, 0x00,
  0x00, 0xF0, 0x03, 0x80, 0x00, 0x01, 0xE0, 0x1C, 0x0F, 0x00, 0x3F, 0xF0, 0x01, 0xFC, 0x1C, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x01, 0x80, 0x03, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x1F, 0xE0, 0x0F, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x7D, 0xE0, 0x7F, 0xC0, 0x00, 0x00, 0x00, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xE1, 0xE0, 0xF3, 0x80, 0x00, 0x00, 0x00, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x03, 0xC1, 0xC3, 0xC3, 0x80, 0x00, 0x00, 0x00, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x0F, 0x03, 0xC7, 0x03, 0x80, 0x00, 0x00, 0x01, 0xE0, 0xF0, 0x00, 0x00, 0x00, 0xE1, 0xE0,
  0x00, 0x1E, 0x03, 0x9E, 0x07, 0x00, 0x1F, 0xF0, 0x1F, 0xFF, 0xF0, 0x01, 0xFE, 0x00, 0xEF, 0xF8,
  0x00, 0x3C, 0x07, 0xB8, 0x07, 0x00, 0x7C, 0x78, 0x1F, 0xF0, 0x00, 0x0F, 0x8F, 0x01, 0xFC, 0x38,
  0x00, 0xF8, 0x07, 0xF0, 0x0F, 0x01, 0xE0, 0xF0, 0x07, 0x80, 0x00, 0x3C, 0x1F, 0x01, 0xF0, 0x00,
  0x01, 0xF0, 0x0F, 0xC0, 0x0E, 0x07, 0xFF, 0xE0, 0x07, 0x00, 0x20, 0x7F, 0xFC, 0x01, 0xC0, 0x00,
  0x03, 0xE0, 0x0F, 0x80, 0x1E, 0x0F, 0xF8, 0x00, 0x0F, 0x00, 0xE1, 0xFF, 0x80, 0x03, 0xC0, 0x00,
  0x03, 0xC0, 0x1F, 0x00, 0x1C, 0x07, 0x00, 0x18, 0x0F, 0x03, 0xC0, 0xE0, 0x01, 0x83, 0x80, 0x00,
  0x07, 0x80, 0x3C, 0x00, 0x3C, 0x07, 0x80, 0xF8, 0x07, 0xFF, 0x00, 0xF0, 0x1F, 0x07, 0x80, 0x00,
  0x0F, 0x00, 0x38, 0x00, 0x18, 0x03, 0xFF, 0xC0, 0x01, 0xFC, 0x00, 0x7F, 0xF8, 0x07, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

#define interruptPin 2
#define LED_PIN 13 // 

void i2cSetup() {
  // join I2C bus
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

}
//**********************************************************************************************************
// the setup function runs once when you press reset or power the board
void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(resetButton, INPUT_PULLUP);     // Initialize the pushbutton pin 9 as an input pullup . . .
  pinMode(upButton, INPUT_PULLUP);        // Initialize the pushbutton pin 10 as an input pullup . . .
  pinMode(downButton, INPUT_PULLUP);      // Initialize the pushbutton pin 8 as an input pullup . . .
  pinMode(leftButton, INPUT_PULLUP);      // Initialize the pushbutton pin 3 as an input pullup . . .
  pinMode(rightButton, INPUT_PULLUP);     // Initialize the pushbutton pin 6 as an input pullup . . .

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.drawBitmap(0, 0, logo16_glcd_bmp, 128, 32, 1);     //  Display the logo until the MPU initializes
  display.display();
  delay(2000);

  while ((digitalRead(resetButton) == HIGH ) and (digitalRead(upButton) == HIGH ) and (digitalRead(downButton) == HIGH ) and (digitalRead(leftButton) == HIGH ) and (digitalRead(rightButton) == HIGH ))                             // Begin the instruction loop to operate the device.
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.clearDisplay();
    display.display();
    display.invertDisplay(false);
    display.print("Left/Right= Menus");
    display.display();
    if ((digitalRead(resetButton) == HIGH ) and (digitalRead(upButton) == HIGH ) and (digitalRead(downButton) == HIGH ) and (digitalRead(leftButton) == HIGH ) and (digitalRead(rightButton) == HIGH ))
    {
      delay(1250);
    }

    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.clearDisplay();
    display.display();
    display.invertDisplay(false);
    display.println("Up/Down = Functions ");
    display.display();
    if ((digitalRead(resetButton) == HIGH ) and (digitalRead(upButton) == HIGH ) and (digitalRead(downButton) == HIGH ) and (digitalRead(leftButton) == HIGH ) and (digitalRead(rightButton) == HIGH ))
    {
      delay(1250);
    }

    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.clearDisplay();
    display.display();
    display.invertDisplay(false);
    display.print("To Start: Hold A Key");
    display.display();

    if ((digitalRead(resetButton) == HIGH ) and (digitalRead(upButton) == HIGH ) and (digitalRead(downButton) == HIGH ) and (digitalRead(leftButton) == HIGH ) and (digitalRead(rightButton) == HIGH ))
    {
      delay(1500);
    }
  }                 // End of the WHILE instruction loop awaiting a data button push
  for (int16_t i = 0; i < display.height() / 2; i += 2) {
    display.display();
    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_WHITE);
  }
}

// Finished with the void setup()



void loop() {


  if (digitalRead (leftButton) == LOW)
  {
    i = i - 1;
  }
  if (i < 0)
  {
    i = 0;
  }

  if (digitalRead (rightButton) == LOW)
  {
    i = i + 1;
  }
  if (i > 2)
  {
    i = 2;
  }


  if (i + 1 == 1)
  {

    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 8);
    display.clearDisplay();
    //display.setFont(&Org_01);
    display.println("FUNCTION 1");
    display.println("Display 1");
    display.display();
    delay(500);
  }
  display.setFont();

  while (i + 1 == 1)
  {

    if (digitalRead (upButton) == LOW && i + 1 == 1)
    { k = k - 1;
      if (k < 0) {
        k = 10;
      }
    }
    if (digitalRead (downButton) == LOW && i + 1 == 1)
    { k = k + 1;
      if (k > 10) {
        k = 0;
      }
    }
    break;
  }

  /////////////////////////////////////////////
  if (i + 1 == 2)
  {
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 8);
    display.clearDisplay();
    //display.setFont(&Org_01);
    display.println("FUNCTION 2");
    display.println("Display  2");
    display.display();
    delay(500);
  }
  display.setFont();

  while (i + 1 == 2)
  {
    if (digitalRead (upButton) == LOW && i + 1 == 2)
    { l = l - 1;
      if (l < 0) {
        l = 8;
      }
    }
    if (digitalRead (downButton) == LOW && i + 1 == 2)
    { l = l + 1;
      if (l > 8) {
        l = 0;
      }
    }

    break;
  }

  //////////////////////////////////////////
  if (i + 1 == 3)
  {
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 8);
    display.clearDisplay();
    //display.setFont(&Org_01);
    display.println("FUNCTION 3");
    display.println("Display  3");
    display.display();
    delay(1500);
  }
  display.setFont();
}


//  Finished with the void loop():