/**************************************************************************
  This is an example for our Monochrome OLEDs based on SSD1306 drivers

  Pick one up today in the adafruit shop!
  ------> http://www.adafruit.com/category/63_98

  This example is for a 128x64 pixel display using I2C to communicate
  3 pins are required to interface (two I2C and one reset).

  Adafruit invests time and resources providing this open
  source code, please support Adafruit and open-source
  hardware by purchasing products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries,
  with contributions from the open source community.
  BSD license, check license.txt for more information
  All text above, and the splash screen below must be
  included in any redistribution.
 **************************************************************************/

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <string.h>
#include <Keypad.h>

const int ROW_NUM = 4; //four rows
const int COLUMN_NUM = 4; //four columns

char keys[ROW_NUM][COLUMN_NUM] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'.', '0', '#', 'D'}
};

byte pin_rows[ROW_NUM] = {22, 23, 24, 25}; //connect to the row pinouts of the keypad
byte pin_column[COLUMN_NUM] = {26, 27, 28, 29}; //connect to the column pinouts of the keypad

Keypad keypad = Keypad( makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM );
String input_keypad;

unsigned long previousMillis = 0;  // Zmienna przechowująca poprzedni czas
const long interval = 500;  // Interwał migania diody (w milisekundach)
bool lastState = false;

bool mode_auto = true;

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define UP              9
#define DOWN            8
#define ENTER           10
#define BACK            11
#define LEFT            12
#define RIGHT           13

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const unsigned char obrazBez_tytu_u2 [] PROGMEM = {
  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, 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, 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, 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, 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, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x00, 0x0f, 0xf8, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7f, 0xff, 0xf8, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7f, 0xff, 0xf8, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7f, 0xff, 0xf8, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7f, 0xff, 0xf8, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7f, 0xff, 0xf8, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7f, 0xff, 0xf8, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7c, 0x03, 0xf8, 0x7f, 0x00, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7c, 0x03, 0xf8, 0x7f, 0x00, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7c, 0x03, 0xf8, 0x7f, 0x00, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7c, 0x03, 0xf8, 0x7f, 0x00, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7c, 0x03, 0xf8, 0x7f, 0x00, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x7c, 0x03, 0xf8, 0x7f, 0x00, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0xfa, 0x3f, 0xf9, 0xff, 0xcf, 0xfe, 0x7f, 0xf0, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xf8, 0x7f, 0xc0, 0x02, 0x20, 0x09, 0x00, 0x48, 0x02, 0x40, 0x10, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0xfe, 0x2f, 0xf9, 0x7f, 0x4b, 0xfa, 0x5f, 0xd0, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x28, 0x01, 0x41, 0x4a, 0x0a, 0x50, 0x50, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x28, 0x01, 0x41, 0x4a, 0x0a, 0x5f, 0xd0, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x28, 0x01, 0x41, 0x4a, 0x0a, 0x40, 0x10, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x28, 0x01, 0x41, 0x4a, 0x0a, 0x5f, 0xf0, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x28, 0x01, 0x41, 0x4a, 0x0a, 0x50, 0x00, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x28, 0x01, 0x41, 0x4a, 0x0a, 0x50, 0x00, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x28, 0x01, 0x41, 0x4a, 0x0a, 0x50, 0x00, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x2f, 0xf9, 0x7f, 0x4b, 0xfa, 0x5f, 0xf0, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x20, 0x09, 0x00, 0x48, 0x02, 0x40, 0x10, 0x00,
  0x00, 0x01, 0xfc, 0x03, 0xf8, 0x7f, 0x00, 0x00, 0x3f, 0xf9, 0xff, 0xcf, 0xfe, 0x7f, 0xf0, 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, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 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, 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, 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, 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, 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, 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, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

float Kp = 0.01;
float Ki = 0.001;
float Kd = 0.1;


String Menus[] = {"", "HC-SR04", "Bluetooth", "TCRT5000", "PID", "TRYB", "" };
bool TCRT5000[] = {false, false, false, true, true, false, false, false};
int array_len = sizeof(Menus) / sizeof(Menus[0]);

int STEP = 0, SUB_STEP = 0;
int var = 0;

char lastUP = '\0', lastDOWN = '\0', lastLEFT = '\0', lastRIGHT = '\0', lastENTER = '\0', lastBACK = '\0';
bool UPflag = false, DOWNflag = false, LEFTflag = false, RIGHTflag = false, ENTERflag = false, BACKflag = false;

void setup() {
  Serial.begin(9600);
  pinMode(ENTER, INPUT_PULLUP);
  pinMode(BACK, INPUT_PULLUP);
  pinMode(LEFT, INPUT_PULLUP);
  pinMode(RIGHT, INPUT_PULLUP);
  pinMode(UP, INPUT_PULLUP);
  pinMode(DOWN, INPUT_PULLUP);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.clearDisplay();
  display.drawBitmap(0, 0, obrazBez_tytu_u2, 128, 64, 1);
  display.display();
  display.setTextWrap(false);
  delay(2000); // Pause for 2 seconds
}

void loop() {
  if (var == 0)    var = menu1();
  if (var == 1)    var = submenu_OLED();
  if (var == 2)    var = submenu_bluetooth();
  if (var == 3)    var = submenu_TCRT5000();
  if (var == 4)    var = submenu_PID();
  if (var == 5)     var = submenu_mode();
  if (var == 10)   var = changePID();
}


int submenu_OLED() {    // WYSWIETL OKNO POMIARU ODLEGLOSCI
  display.clearDisplay();
  display.drawFastHLine(10, 3, 108, SSD1306_WHITE);
  display.drawFastHLine(10, 30, 108, SSD1306_WHITE);
  display.drawFastVLine(10, 3, 27, SSD1306_WHITE);
  display.drawFastVLine(118, 3, 27, SSD1306_WHITE);
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(18, 10);
  display.println("DISTANCE");
  display.setCursor(35, 45);
  display.println(10.0);
  show_buttons(true, false);
  display.display();


  click();
  if (BACKflag) return 0;

  return 1;

}

void menu()
{
  for (int i = 1; i <= 3; i++)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(5, 2);
    display.println(Menus[i - 1]);
    display.setCursor(5, 24);
    display.println(Menus[i]);
    display.setCursor(5, 45);
    display.println(Menus[i + 1]);
    display.drawRoundRect(2, 20, 124, 22, 3, SSD1306_WHITE);
    show_buttons(false, true);
    display.display();
    delay(2000);
  }
}

int menu1()
{
  SUB_STEP = 0;
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(5, 2);
  display.println(Menus[STEP]);
  display.setCursor(5, 24);
  display.println(Menus[STEP + 1]);
  display.setCursor(5, 45);
  display.println(Menus[STEP + 2]);
  display.drawRoundRect(2, 20, 124, 22, 3, SSD1306_WHITE);
  show_buttons(false, true);
  display.display();

  click();
  if (UPflag)    STEP--;
  if (DOWNflag)  STEP++;
  if (STEP > array_len - 3)  STEP = 0;
  if (STEP < 0)  STEP = array_len - 3;
  if (ENTERflag) return (STEP + 1);

  return 0;
}

int submenu_TCRT5000()
{
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(25, 10);
  display.println("Stan czujnikow");
  display.setCursor(35, 20);
  display.println("TCRT 5000");
  for (int i = 0; i < 8; i++)
  {
    if (TCRT5000[i])
      display.fillCircle(11 + i * 15, 40, 5, SSD1306_WHITE);
    else
      display.drawCircle(11 + i * 15, 40, 5, SSD1306_WHITE);

  }
  show_buttons(true, false);
  display.display();

  click();
  if (BACKflag)  return 0;

  return 3;
}

int submenu_bluetooth()
{
  display.clearDisplay();
  display.drawRect(0, 0, 128, 64, SSD1306_WHITE);
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(4, 5);
  display.println("Ustawienia Bluetooth");
  display.setCursor(4, 17);
  display.println("Nazwa:");
  display.setCursor(45, 17);
  display.println("HMsoft");
  display.setCursor(4, 27);
  display.println("Polaczenie:");
  bool polaczenie = false;
  if (polaczenie)
    display.fillCircle(120, 31, 3, SSD1306_WHITE);
  else
    display.drawCircle(120, 31, 3, SSD1306_WHITE);

  display.setCursor(4, 37);
  display.println("ID Modulu:");
  show_buttons(true, false);
  display.display();
  char lol[] = "sadfadsgacvavxv";
  int x = display.width();
  int minX = -6 * strlen(lol);
  int i = 0;
  // while (i < 65)
  // {
  //   display.fillRect(65, 37, 78, 10, BLACK);
  //   display.setCursor(x, 37);
  //   display.print(lol);
  //   x = x - 1;
  //   display.display();
  //   i++;
  // }

  click();
  if (BACKflag)  return 0;

  return 2;
}

int submenu_PID()
{
  input_keypad = "";
  blink();
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(7, 5);
  display.println("KP");
  display.setCursor(35, 5);
  display.println(Kp, 4);
  display.setCursor(80, 5);
  display.println("ZMIEN");

  display.setCursor(7, 17);
  display.println("KI");
  display.setCursor(35, 17);
  display.println(Ki, 4);
  display.setCursor(80, 17);
  display.println("ZMIEN");

  display.setCursor(7, 29);
  display.println("KD");
  display.setCursor(35, 29);
  display.println(Kd, 4);
  display.setCursor(80, 29);
  display.println("ZMIEN");

  switch (SUB_STEP)
  {
    case 0:
      display.drawRoundRect(78, 3, 34, 11, 1, WHITE);
      break;
    case 1:
      display.drawRoundRect(78, 15, 34, 11, 1, WHITE);
      break;
    case 2:
      display.drawRoundRect(78, 27, 34, 11, 1, WHITE);
      break;
  }

  show_buttons(true, true);
  display.display();

  click();
  if (ENTERflag)   return 10;
  if (BACKflag)    return 0;
  if (UPflag)      SUB_STEP--;
  if (DOWNflag)    SUB_STEP++;
  if (SUB_STEP < 0) SUB_STEP = 2;
  if (SUB_STEP > 2) SUB_STEP = 0;


  return 4;
}

int changePID()
{
  int length = input_keypad.length();
  char input = click();
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(7, 22);
  switch (SUB_STEP)
  {
    case 0:
      display.println("KP=");
      break;
    case 1:
      display.println("KI=");
      break;
    case 2:
      display.println("KD=");
      break;
  }
  display.setCursor(52, 22);
  if (input) {
    if (input != '#' && input != 'A' && input != 'B' && input != 'C' && input != 'D')
      input_keypad += input;
    if (input == '#') {
      input_keypad[length - 1] = ' ';
      input_keypad.trim();
    }
  }
  display.println(input_keypad);
  show_buttons(true, true);
  display.display();
  if (ENTERflag) {
    if (SUB_STEP == 0)   Kp = input_keypad.toFloat();
    if (SUB_STEP == 1)   Ki = input_keypad.toFloat();
    if (SUB_STEP == 2)   Kd = input_keypad.toFloat();
    return 4;
  }
  if (BACKflag)  return 4;

  return 10;
}

int submenu_mode()
{
  display.clearDisplay();
  display.drawRect(0, 0, 128, 64, SSD1306_WHITE);
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(30, 5);
  display.println("Obecny tryb");
  if (mode_auto) {
    display.setCursor(50, 20);
    display.println("Auto");
  }
  else {
    display.setCursor(35, 20);
    display.println("Bluetooth");
  }
  display.setCursor(47, 35);
  display.println("ZMIEN");
  display.drawRoundRect(45, 33, 34, 11, 1, WHITE);
  show_buttons(true, true);
  display.display();
  click();
  if (BACKflag)     return 0;
  if (ENTERflag)
    if (mode_auto)   mode_auto = false;
    else            mode_auto = true;

  return 5;
}

void show_buttons(bool button_back, bool button_enter)
{
  if (button_back)
  {
    display.drawTriangle(11, 52, 11, 58, 4, 55, SSD1306_WHITE);
    display.drawRoundRect(2, 49, 13, 13, 2, SSD1306_WHITE);
  }
  if (button_enter)
  {
    display.drawCircle(119, 55, 3, SSD1306_WHITE);
    display.drawRoundRect(113, 49, 13, 13, 2, SSD1306_WHITE);
  }
}

void blink() {
  unsigned long currentMillis = millis();  // Odczyt aktualnego czasu

  if (currentMillis - previousMillis >= interval) {  // Sprawdzenie, czy upłynął interwał czasu
    previousMillis = currentMillis;  // Zapisanie aktualnego czasu

    // Przełączanie stanu diody LED
    if (lastState == false) {
      lastState = true;
    } else {
      lastState = false;
    }
  }
}

char click()
{
  char input = keypad.getKey();
  UPflag = false;
  DOWNflag = false;
  ENTERflag = false;
  BACKflag = false;
  if (input)
  {
    if (input == 'A') {
      UPflag = true;
    }
    if (input == 'B') {
      DOWNflag = true;
    }
    if (input == 'D') {
      ENTERflag = true;
    }
    if (input == 'C') {
      BACKflag = true;
    }
    delay(50);
  }
  return input;
}















// void click_old()
// {
//   byte buttonState1 = digitalRead(BACK);
//   BACKflag = false;
//   if (buttonState1 != lastBACK) {
//     if (buttonState1 == LOW)
//       BACKflag = true;
//     delay(50);
//   }
//   lastBACK = buttonState1;

//   byte buttonState2 = digitalRead(ENTER);
//   ENTERflag = false;
//   if (buttonState2 != lastENTER) {
//     if (buttonState2 == LOW)
//       ENTERflag = true;
//     delay(50);
//   }
//   lastENTER = buttonState2;

//   byte buttonState3 = digitalRead(UP);
//   UPflag = false;
//   if (buttonState3 != lastUP) {
//     if (buttonState3 == LOW)
//       UPflag = true;
//     delay(50);
//   }
//   lastUP = buttonState3;

//   byte buttonState4 = digitalRead(DOWN);
//   DOWNflag = false;
//   if (buttonState4 != lastDOWN) {
//     if (buttonState4 == LOW)
//       DOWNflag = true;
//     delay(50);
//   }
//   lastDOWN = buttonState4;

//   byte buttonState5 = digitalRead(LEFT);
//   LEFTflag = false;
//   if (buttonState5 != lastLEFT) {
//     if (buttonState5 == LOW)
//       LEFTflag = true;
//     delay(50);
//   }
//   lastLEFT = buttonState5;

//   byte buttonState6 = digitalRead(RIGHT);
//   RIGHTflag = false;
//   if (buttonState6 != lastRIGHT) {
//     if (buttonState6 == LOW)
//       RIGHTflag = true;
//     delay(50);
//   }
//   lastRIGHT = buttonState6;
// }