//#include <U8g2lib.h>
#include <Bounce2.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>


byte button_pins[] = {33,27,2};  // button pins, 4,5 = up/down, 6 = select
#define NUMBUTTONS sizeof(button_pins)
Bounce* buttons = new Bounce[NUMBUTTONS];

#define SCREEN_WIDTH 128     // OLED display width, in pixels
#define SCREEN_HEIGHT 64     // OLED display height, in pixels
#define OLED_RESET -1        // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C  ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


//U8X8_SSD1306_128X64_NONAME_HW_I2C display(U8X8_PIN_NONE);
const int MENU_SIZE = 4;
int currentScreen = 0;
char* menu[MENU_SIZE] = { "Set Clicks ", "Set Speed ", "Delay Time", "Run Test " };
int cursor = 0;
int menuPlacment = 0;

int clicks = 12;
int speed = 300;
int dt = 300;
int dt2 = 200;
int ButtonRun = 0;
int solenoidPin = 8;



void setup() {
  Serial.begin(9600);

  // Make input & enable pull-up resistors on switch pins
  for (int i = 0; i < NUMBUTTONS; i++) {
    buttons[i].attach(button_pins[i], INPUT_PULLUP);  // setup the bounce instance for the current button
    buttons[i].interval(25);                          // interval in ms
  }
  pinMode(solenoidPin, OUTPUT);





  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;  // Don't proceed, loop forever
  }
  display.setTextSize(1);               // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);
  display.display();
  delay(100);
  display.clearDisplay();
  showMenu();
}

void loop() {
  // process button press:
  for (int i = 0; i < NUMBUTTONS; i++) {
    buttons[i].update();      // Update the Bounce instance
    if (buttons[i].fell()) {  // If it fell
      if (i == 2) {           // select

        //display.clearLine(7);
        display.setCursor(0, 7);
        display.print(">>");
        display.setCursor(3, 40);
        display.print(menu[cursor]);
        //display.clearLine(50);
        executeChoice(cursor);
      } else {
        // erase previous cursor:
        display.setCursor(0, cursor);
        display.print(' ');
        if (i == 0) {  // up
          cursor++;
          if (cursor > (MENU_SIZE - 1)) cursor = 0;
        } else {  // down
          cursor--;
          if (cursor < 0) cursor = (MENU_SIZE - 1);
        }
        // show cursor at new line:
        display.setCursor(0, cursor);
        display.print('>');
        display.display();
      }
    }  // end if button fell...
  }    // end for-loop of button check
}

/**
   Clear display and show the menu.
*/
void showMenu() {
  display.clearDisplay();
  // show menu items:
  for (int i = 0; i < MENU_SIZE; i++) {
    display.setCursor(7,menuPlacment);
    display.println(menu[i]);
    display.display();
    menuPlacment = menuPlacment + 10;
  }
  display.setCursor(0, 0);
  display.print('>');
  display.display();
}

/**
   Execute the task which matches the chosen menu item.
*/
void executeChoice(int choice) {

  switch (choice) {
    case 0:
      Serial.print("Execute choice ");
      Serial.print(choice);
      Serial.print(" - ");
      Serial.println(menu[choice]);
      break;
    case 1:
      Serial.print("Execute choice ");
      Serial.print(choice);
      Serial.print(" - ");
      Serial.println(menu[choice]);
      break;
    case 2:
      Serial.print("Execute choice ");
      Serial.print(choice);
      Serial.print(" - ");
      Serial.println(menu[choice]);
      break;
    case 3:
      Serial.print("Execute choice ");
      Serial.print(choice);
      Serial.print(" - ");
      Serial.println(menu[choice]);
      ButtonRun = 1;
      TriggerTest();
      break;
    case 4:
      Serial.print("Execute choice ");
      Serial.print(choice);
      Serial.print(" - ");
      Serial.println(menu[choice]);
      break;
    default:
      Serial.print("Execute choice ");
      Serial.print(choice);
      Serial.print(" - ");
      Serial.println(menu[choice]);
      break;
  }
}

void TriggerTest() {
  if (ButtonRun == 1) {
    ButtonRun = 0;
    int j;
    int ctr = 12;
    //delay(500);
    for (j = 1; j <= ctr; j = j + 1) {
      Serial.println(j);
      display.clearDisplay();
      display.setCursor(20, 0);
      display.print("Test Running");
      display.setCursor(30, 30);
      display.print(j);
      display.display();
      digitalWrite(solenoidPin, HIGH);
      delay(dt);
      digitalWrite(solenoidPin, LOW);
      delay(dt2);
    }
    display.clearDisplay();
    display.display();
    showMenu();
  }
  //display.clearLine(7);
  display.setCursor(0, 7);
  display.print(">>");
  display.setCursor(2, 40);
  display.print("Test Complete");

}
// void SetClicks () {
// if (condition) {
// statements
// }
//   display.clearDisplay();
//   display.setCursor(0, 30);
//   display.print(choice);
//   display.setCursor(20, 30);


// }