#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>

#define i2c_Address 0x3c




Adafruit_SH1106G display = Adafruit_SH1106G(128, 64, &Wire, -1);
// Változás figyelés ===================
int last =0;

int Servo_val_last =0;

// Élfigyelés ==========================
int bs_old = 1;
int bs_new = 1;

int bs_old2 = 1;
int bs_new2 = 1;

int bs_old3 = 1;
int bs_new3 = 1;
// =====================================

int mode = 0; //Módok közti váltás

int move = 0;

int Servo_val = 0;

bool selector_button_val = false;

void setup()   {

  Serial.begin(9600);
	pinMode(2, INPUT_PULLUP);
	pinMode(3, INPUT_PULLUP);
	pinMode(4, INPUT_PULLUP);



  delay(250); 
  display.begin(i2c_Address, true); 
	display.clearDisplay();
	main_display();
	
	

}






void loop(){
if (mode != last){
	menu_move();
	menu_selector();
	last=mode;
	
}


menu_choose();

Serial.println(mode);

}




void menu_selector(){ //Main menü a kijelzőhöz
											//10 karakter hosszú a kijelző

display.clearDisplay();

disp_print("Main      ",0,4-move,change_color(0)); 

disp_print("Servo:",0,24-move,change_color(1));
disp_print(String(Servo_val),70,24-move,change_color(1));

disp_print("menu_3    ",0,44-move,change_color(2));

disp_print("menu_4    ",0,64-move,change_color(3));

disp_print("menu_5    ",0,84-move,change_color(4));

disp_print("menu_6    ",0,104-move,change_color(5));

}



void main_display(){
	display.clearDisplay();

	disp_print("Main      ",0,4,false); //10 karakter hosszú a kijelző



}

int menu_choose(){

	selector_button_val = selector_button(selector_button_val);

	if (selector_button_val == true & mode == 1){
		Servo_val = button(Servo_val);

			if (Servo_val != Servo_val_last){
			menu_selector();
		
			Servo_val_last = Servo_val;
			}
	}
	else{
		mode = button(mode);
	}


}

int button(int val){ //Gombok élfigyelése

	bs_new = digitalRead(2); //Le gomb

	if (bs_old != bs_new){
		delay(10);
		if (bs_new==HIGH){
		val++;
		}

		bs_old=bs_new;
		Serial.println(val);
	}

	bs_new2 = digitalRead(3); //Fel gomb

	if (bs_old2 != bs_new2){
		delay(10);
		if (bs_new2==HIGH){
		val--;
		}

		bs_old2=bs_new2;
		Serial.println(val);
	}
  return val;
}


	 
bool selector_button(bool val){
	bs_new3 = digitalRead(4); //Le gomb

	if (bs_old3 != bs_new3){

		delay(10);

		if (bs_new3==HIGH){
			val = !val;
		}
	}
		bs_old3=bs_new3;
		return val;
}



void menu_move(){ //Menü görgetése 
	if (mode > 2){
		move = map(mode,3,5,20,60);
	}
	else{
		move=0;
	}
}

int change_color(int a){ //Menü pontok kijelölése
	if (mode==a){
		return true;
		}
	else{
		return false;
	}
}

int disp_print(String szoveg,int x,int y,int color){ //Szöveg kiiratás function
	//	display.clearDisplay();

	if (color == true){

  display.setTextColor(SH110X_BLACK,SH110X_WHITE);}
	else{
	display.setTextColor(SH110X_WHITE);
	}

  display.setTextSize(2);
	display.setCursor(x,y);
	display.println(szoveg);
	display.display( );   
}