#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define DIS_WIDTH 128
#define DIS_HEIGHT 64
Adafruit_SSD1306 display(DIS_WIDTH, DIS_HEIGHT, &Wire, -1);

#define PIN_UP 25
#define PIN_DOWN 27
#define PIN_ENTER 26
#define PIN_DEBUG 14

const unsigned int pinUp = 25;
const unsigned int pinDown = 27;
const unsigned int pinEnter = 26;
const unsigned int pinDebug = 14;

struct Button {
    const uint8_t PIN;
    bool pressed;
};

Button buttonEnter = {PIN_ENTER, false};
Button buttonUp = {PIN_UP, false};
Button buttonDown = {PIN_DOWN, false};

const char *mainMenu[] = {
	" MMENU 1",
	" MMENU 2",
	" MMENU 3",
	" MMENU 4",
	" MMENU 5",
	" MMENU 6",
	" MMENU 7",
	" MMENU 8",
	" MMENU 9",
	" MMENU 10"
};

const char *subMenu1[] = {
	" SUB MENU1 1",
	" SUB MENU1 2",
	" SUB MENU1 3",
	" SUB MENU1 4",
	" SUB MENU1 5"
};

const char *subMenu2[] = {
	" SUB MENU2 1",
	" SUB MENU2 2",
	" SUB MENU2 3",
	" SUB MENU2 4",
	" SUB MENU2 5",
	" SUB MENU2 6"
};

const int numMainMenu = *(&mainMenu + 1) - mainMenu;
const int numSubMenu1 = *(&subMenu1 + 1) - subMenu1;
const int numSubMenu2 = *(&subMenu2 + 1) - subMenu2;

unsigned int menuLevel = 1;
unsigned int menuPositions[3] = {1, 1, 1};

const unsigned int posPerPage = 5;
bool debugEnabled = false;

unsigned int position = 1;
unsigned int lastPosition = 1;
unsigned int currentPage = 1;
unsigned int mainMenuLastPosition = 0;
unsigned int menuEnterPressed = 0;

unsigned int positionMin = currentPage*posPerPage-posPerPage+1;
unsigned int positionMax = currentPage*posPerPage;

unsigned long button_time = 0;  
unsigned long last_button_time = 0; 

void displayMenu(int dMpage, int dMposPerPage, int dMlevel, unsigned int dMmenuEnterPressed) {
	int dMnumOptions;
	const char **dMoptions;

	unsigned int dMpositionMin = dMpage*dMposPerPage-dMposPerPage;
	unsigned int dMpositionMax = dMpage*dMposPerPage-1;
	if (debugEnabled) {
		Serial.println((String)"Strona:   "+dMpage);
		Serial.println((String)"Pozycja:  "+position);
	}

	if (position >= 1 && dMmenuEnterPressed > 0) {
		if ( menuLevel == 1) {
			menuPositions[0] = position;
		}
		if ( menuLevel == 2) {
			menuPositions[1] = position;
		}
		if ( menuLevel == 3) {
			menuPositions[2] = position;
		}
		menuEnterPressed = 0;
		menuLevel++;
	}

  display.setCursor(0, 8);
	if (menuLevel == 1) {
		// MAIN MENU
		dMnumOptions = numMainMenu;
		dMoptions = mainMenu;
		display.println(F(">>> MAIN MENU <<<"));
		display.println("");
		Serial.println((String)">>> POZYCJA NAZWA => \""+ dMoptions[menuPositions[0]]+"\" DMLEVEL => " + dMlevel + " menuPositions: " + menuPositions[0] + " <<<");
	} else {
		// SUBMENU
		display.println((String)">>> MENU LEVEL " + dMlevel + " <<<");
		display.println((String)"TABLICA menuLevels 0: "+ menuLevels[0] + " 1: " + menuLevels[1] + " 2: " + menuLevels[2]);
	}
	// switch (menuPositions[0]) {
	// 	default:
	// 		delay(1000);
	// 		menuLevels[1] = mainMenuLastPosition;
	// 		switch (menuLevels[1]) {
	// 			case 1:
	// 				Serial.println((String)"MAIN MENU SELECTED: "+ mainMenuLastPosition + " MENU LEVEL: "+ dMlevel);
	// 				dMnumOptions = numSubMenu1;
	// 				dMoptions = subMenu1;
	// 			break;
	// 			case 2:
	// 				Serial.println((String)"MAIN MENU SELECTED: "+ mainMenuLastPosition + " MENU LEVEL: "+ dMlevel);
	// 				dMnumOptions = numSubMenu2;
	// 				dMoptions = subMenu2;
	// 			break;
	// 			default:
	// 				Serial.println((String)"MAIN MENU SELECTED: "+ mainMenuLastPosition + " MENU LEVEL: "+ dMlevel);
	// 				delay(2000);

	// 		// 	case 1: // SENSORS
	// 		// 	dMnumOptions = numSensorsMenu;
	// 		// 	dMoptions = MenuDsSensors;
	// 		// 	display.println(F(">>> SENSORS <<<"));
	// 		// 	display.println("");
	// 		// break;
	// 		}
	// }
	for (int i = 0; i <= dMnumOptions-1; i++) {
		if (i >= dMpositionMin && i <= dMpositionMax) {
			if (i == position-1) {
				display.setTextColor(BLACK, WHITE);
				display.println(dMoptions[i]);
				if (dMlevel == 1) {
					menuPositions[0] = position-1;
				}
				Serial.println((String)">>> SELECTED MENU LEVEL: " + dMlevel + " POSITION: " + position);
				display.setTextColor(WHITE);
			} else {
				display.println(dMoptions[i]);
			}
		}
	}
	display.display();
}


void showWelcomeMessage() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Uruchamiam");
  display.println("System");
  display.println("OLED");
  display.display();
  delay(1000);
}

void ICACHE_RAM_ATTR upButton() {
	button_time = millis();
	if (button_time - last_button_time > 250) {
		buttonUp.pressed = true;
		if (position > 1 ) {
			position--;
		}
		last_button_time = button_time;
	}
}

void ICACHE_RAM_ATTR downButton() {
	button_time = millis();
	if (button_time - last_button_time > 250) {
		buttonDown.pressed = true;
		if (position < numMainMenu) {
			position++;
		}
		last_button_time = button_time;
	}
}

void ICACHE_RAM_ATTR enterButton() {
		button_time = millis();
		if (button_time - last_button_time > 250) {
			buttonEnter.pressed = true;
			last_button_time = button_time;
		}
}

void setup() {
  pinMode(PIN_UP, INPUT_PULLUP);
  pinMode(PIN_DOWN, INPUT_PULLUP);
  pinMode(pinDebug, INPUT_PULLUP);
	pinMode(buttonEnter.PIN, INPUT_PULLUP);

	attachInterrupt(PIN_UP, upButton, FALLING);
	attachInterrupt(PIN_DOWN, downButton, FALLING);
	attachInterrupt(buttonEnter.PIN, enterButton, FALLING);

  Serial.begin(9600);

	if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
  	Serial.println(F("SSD1306 nie dziala"));
  	for (;;); 
	} 
	showWelcomeMessage();
}

void loop() {
	if (digitalRead(pinDebug) == LOW) {
		debugEnabled = true;
	} else {
		debugEnabled = false;
	}

  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.clearDisplay();
  display.setCursor(0, 0);
	if (debugEnabled) {
		Serial.println(F("WYSWIETLAM MENU"));
		Serial.println((String)"LOOP-CurrentPage: "+currentPage);
		Serial.println((String)"LOOP-position:    "+position);
	}

	if (lastPosition != position) {
		if (debugEnabled) {
			Serial.println(F("<<<<<< ZMIANA POZYCJI >>>>>>"));
 			Serial.println((String)"LOOP-CurrentPage:  "+currentPage);
			Serial.println((String)"LOOP-position:     "+position);
			Serial.println((String)"LOOP-numMainMenu: "+numMainMenu);
			Serial.println((String)"LOOP-positionMax:  "+positionMax);
		}

		if (position < lastPosition) {
			if (debugEnabled) {
				Serial.println(F(">>> UP UP <<<"));
			}

			if (position > 1) {
				positionMin = currentPage*posPerPage-posPerPage+1;
				if (position < positionMin) {
					currentPage--;
				}
			}

			if (debugEnabled) {
				Serial.println((String)"PIN_UP___ PAGE:     "+currentPage);
				Serial.println((String)"PIN_UP___ position: "+position);
			}
		}
		else if (position > lastPosition ) {
			if (debugEnabled) {
				Serial.println(F(">>> DOWN DOWN <<<"));
			}
			if (position < numMainMenu) {
				positionMax = currentPage*posPerPage;
				if (position > positionMax) {
					currentPage++;
				}
			}
			if (debugEnabled) {
				Serial.println((String)"PIN_DOWN___ PAGE:     "+currentPage);
				Serial.println((String)"PIN_DOWN___ position: "+position);
			}
		}
	}
	lastPosition = position;
// ENTER BUTTON
	if (buttonEnter.pressed == true) {
		if (debugEnabled) {
			Serial.println(F("ENTER PRESSED"));
		}
		menuEnterPressed++;
		buttonEnter.pressed = false;
	}

	displayMenu(currentPage, posPerPage, menuLevel, menuEnterPressed);
}