#include <Servo.h>
#include <EncButton.h>

EncButton<EB_TICK, 3> btn1;
EncButton<EB_TICK, 5> btn2;
Servo myservo;


int pos = 0;    // variable to store the servo position
int positions[] = {5, 10, 20, 30, 40, 50, 60, 70, 180};
int cur_pos;
int pos_after_check;
int gears = 8;


void setup() {

  myservo.attach(9);  // Пин серво
  Serial.begin(9600);
  myservo.write(3); // Искуственно ставлю позицию для проверки
  cur_pos = myservo.read(); // Проверяю полодение серво
  Serial.println(cur_pos);
  for (int i = 1; i <= gears; i++) { // Цикл дял нахождения ближайшего угла в массиве углов для серво
    if ( cur_pos < positions[i] ) {
      if ( abs(cur_pos - positions[i]) < abs(cur_pos - positions[i-1]) ) {
        pos_after_check = i;
        break;
      } else {
        pos_after_check = i-1;
        break;
      }
    }
    if ( cur_pos > positions[i] && i == gears) {
      pos_after_check = gears;
      break;
    }
  }
  Serial.println(positions[pos_after_check]);
  myservo.write(positions[pos_after_check]); // Устанавливаю серво в положение ближайшего угла из массива углов
  pos = pos_after_check; // Устанавливаю позицию отсчёта
}

void loop() {


  btn1.tick();
  btn2.tick();


  if (btn1.click() && pos < gears) {
    pos += 1;
    myservo.write(positions[pos]);
    Serial.println(positions[pos]);
  };

  if (btn1.held() && positions[pos] != 180) {
    positions[pos] += 1;
    myservo.write(positions[pos]);
    Serial.println(positions[pos]);
  };

  if (btn2.click() && pos > 0) {
    pos -= 1;
    myservo.write(positions[pos]);
    Serial.println(positions[pos]);
  };

  if (btn2.held() && positions[pos] != 0) {
    positions[pos] -= 1;
    myservo.write(positions[pos]);
    Serial.println(positions[pos]);
  };
  
  myservo.write(positions[pos]);
}

// В переменной храню текущее положение
// 9 положений, то есть 9 элементов массива с углами
// Находясь в положении при длительном нажатии кнопок меняется угол в массиве
// Решить задачу хранения значений углов в массиве после выключения ардуино (EEPROM)
// При включении питания считывать угол и искать в массиве ближайший к нему, устанавливать значение соответствующего элемента массива как текущую передачу
//
// 
//