#define up_ 13
#define ok_ 12
#define down_ 11

#include <EncButton.h>
Button UP(up_);
Button OK(ok_);
Button DOWN(down_);

#include <GyverOLED.h>
GyverOLED<SSD1306_128x64, OLED_NO_BUFFER> oled;

int val_work = 0;

int tof_delta = 0;
int tof_with = 0;
int lasttof_with = 1;

int menu = 1;
bool stan = 0;

void setup() {
  oled.init();        // инициализация
  oled.clear();       // очистка
  oled.setScale(2);   // масштаб текста (1..4)
  oled.home();        // курсор в 0,0
  oled.print("start!");
  delay(1000);
  oled.clear();       // очистка

}

void loop() {

  UP.tick();
  OK.tick();
  DOWN.tick();

  if (OK.hold()) {
    oled.clear();       // очистка
    menu = 0; 
  }
  switch (menu){
    case 0:
      S_mot();
      break;
    case 1:
      kalibrovka_TOF();
      break;
    case 2:
      kalibrovka();
      break;
    case 3:
      ust();
      break;
    case 4:
      work();
      break;

      // якщо працюватиме повільно винести функцію work зі світча, і додати змінну яка буде дозволяти калібровку. А під час роботи перевіряти лише датчик ТОФ
  }
}

void S_mot(){

  if (UP.click()) stan = 1;
  if (DOWN.click()) stan = 0;
  if (OK.click()) {
    oled.clear();       // очистка
    menu = 1; 
  }

  oled.home();        // курсор в 0,0
  oled.setScale(1);
  oled.println("Iнвертувати стан мотора");
  oled.println(String(stan));
}

void kalibrovka_TOF(){

  if (UP.click()) tof_delta++;
  if (OK.click()) {
    oled.clear();       // очистка
    menu = 2; 
  }
  if (DOWN.click()) tof_delta--;

  if (tof_delta <= 0) tof_delta = 0;

  oled.home();        // курсор в 0,0
  oled.setScale(1);
  oled.println("Вiдстань вiд датчика/n до кiнця лазера");
  oled.println(String(tof_delta) + "mm");
}

void kalibrovka(){  // рух мотору перевірка датчика відстані доки не буде встновлене 0мм

  TOF_ism();        // Функція вимірів датчика з поправкою на відстань від датчика до лазера

  if(tof_with < 0) UPdr();
  if(tof_with > 0) downdr();
  if(tof_with == 0) {
    stopdr();
    oled.clear();       // очистка
    menu = 3; 
  }

  oled.home();        // курсор в 0,0
  oled.println("Встановлення нуля, зачекайте...");
  oled.print(String(tof_with) + "mm");

}

void ust(){
  if (UP.click()) UPdr();
  if (DOWN.click()) downdr();

  oled.home();        // курсор в 0,0
  oled.println("Фокусна вiдстань вiд лазера до поврехнi");
  oled.print(String(tof_with) + "mm");

  TOF_ism();
  if (OK.click()){
    val_work = tof_with;
    oled.clear();       // очистка
    oled.home();        // курсор в 0,0
    oled.println("Відстань встановлено");
    oled.print(String(val_work) + "mm");
    delay(1000);
    oled.clear();       // очистка
    menu = 4;
  } 

}

void work(){

}

void stopdr(){

}

void UPdr(){
  if(menu < 4) int speed = 200;
  else int speed = 600;

  if(stan = 0){

  } else {

  }
}


void downdr(){
  if(menu < 4) int speed = 200;
  else int speed = 600;

  if(stan = 0){

  } else {
    
  }
}

void TOF_ism(){
  tof_with = 0;

  if (lasttof_with != tof_with) {
    tof_with = tof_with - tof_delta;
  }
  lasttof_with = tof_with; 
}