#define CLK 4
#define DT 3
#define SW 2

#include <Wire.h>                       // Подключаем Wire
//#include <Adafruit_PWMServoDriver.h>    // Подключаем Adafruit_PWMServoDriver
#include "GyverEncoder.h"               // Библиотека энкодера
#include "LiquidCrystal_I2C.h"          // Библиотека экрана
#include "Servo.h"


//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); // Адрес I2C 0x40
LiquidCrystal_I2C lcd(0x27, 16, 2);

//#define SERVOMIN  50     // Минимальная длительность импульса для сервопривод
//#define SERVOMAX  600     // Максимальная длина импульса для сервопривода
//#define SERVO_0 0         // Номер порта (0 - 15)
//#define SERVO_1 1         // Номер порта (1 - 15)

// Порты энкодера


#define SrvAmount 5
// --- Порты энкодера --
Encoder enc1(CLK, DT, SW);

int ModeCur=8; //режим
int  Mode = 0;
int ModeAmount=7;

int Srv = 0;


int value = 0;
//uint16_t pulselen;

char* ModeName[7] = {"Calibrate", "Auto", "Demo1", "Demo2","Demo3","Manual", "System"};

int ServoCur=0;           // Текущий серво        

Servo sdrive[SrvAmount];
void setup() 
{
 // pwm.begin();                          // Инициализация
 // pwm.setPWMFreq(60);                   // Частота следования импульсов 60 Гц
 // delay(10);                            // Пауза

Serial.begin(9600);
enc1.setTickMode(AUTO);
enc1.setType(TYPE2);  //   тип энкодера 
//enc1.setFastTimeout(40);    // таймаут на скорость isFastR. По умолч. 50

 sdrive[0].attach(6);
  sdrive[1].attach(7);
  sdrive[2].attach(8);
  sdrive[3].attach(9);
  sdrive[4].attach(10);


for (byte i = 0; i < SrvAmount; i++) {
    sdrive[i].write(0);
    Serial.print(i); 
    }


Serial.println("Go-------->>>");
Serial.println(ModeName[Mode]);
Serial.println("--------------------------"); 
Serial.print("Mode - ");
Serial.println(ModeAmount);  




 // Инициализация и начало работы с дисплеем
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0); // Устанавливаем положение курсора, 0-й, 0-я строка
  lcd.print("PYKA-4.1 by SEVA"); // Выводим текст
  lcd.setCursor(0, 1);
  lcd.print("Hi there!");
  delay(3000);

  lcd.clear();  //Чистим экран
  lcd.setCursor(0, 0);
  lcd.print("Mode");
  lcd.setCursor(4, 0);
  lcd.print(">");
  lcd.setCursor(5, 0);
  lcd.print(ModeName[Mode]);

 
}

void loop() 
{
 enc1.tick();
  // лазер 4096
   // enc1.tick();  // не нужна, в этом режиме (AUTO) она входит в каждую функцию!
  
 
  
  if (enc1.isRight()) Mode--;         // если был поворот
  if (enc1.isLeft()) Mode++;
 
  if (enc1.isClick()) {
    ModeCur=Mode;
    lcd.setCursor(4, 0);
    lcd.print("-");
    lcd.setCursor(5, 0);
    lcd.print(ModeName[Mode]);
    Serial.print("click");
    MenuSwitch();
  } 
 
   
  

     //  if (enc1.isHolded()) Serial.println("Holded");       // выход из режима

   if (enc1.isTurn()) {     // если был совершён поворот
     if(Mode>ModeAmount) Mode=0;
     if(Mode<0) Mode=7; 
    Serial.print("Mode - ");
    Serial.println(Mode);
     lcd.setCursor(5, 0);
     lcd.print("          ");
     lcd.setCursor(5, 0);
     lcd.print(ModeName[Mode]);
  }

}




void MenuSwitch()
{
  // свитч крутится в цикле loop
  switch (ModeCur) {
    case 0: task_0();
      break;
    case 1: task_1();
      break;
    case 2: task_2();
      break;
    case 3: task_3();
      break;
    case 4: task_4();
      break;
    case 5: task_5();
      break;
    case 6: task_6();
      break;
  }
}

// Задачи (режимы)
void task_0() {

   
  do {
enc1.tick();
// Калибровка приводов для сборки манипулятора
 if (enc1.isRight()) value--;         // если был поворот
  if (enc1.isLeft()) value++;
 if (enc1.isFastR()) value -= 10;    // если был быстрый поворот направо, 
  if (enc1.isFastL()) value += 10;
  if (enc1.isClick()) {  }
 
     //  if (enc1.isHolded()) Serial.println("Holded");       // выход из режима

   if (enc1.isTurn()) {     // если был совершён поворот
     //if(value>180) value=0;
     //if(value<0) Mode=180; 
//    pulselen = map(value, 0, 180, SERVOMIN, SERVOMAX);
 //   pwm.setPWM(SERVO_0, 0, value);
    Serial.print(value);
    Serial.println(" / ");
  //  Serial.println(pulselen);
    lcd.setCursor(0, 1);
     lcd.print("   ");
     lcd.setCursor(5, 1);
     lcd.print("   ");

     lcd.setCursor(0, 1);
     lcd.print(value);
     lcd.setCursor(3, 1);
     lcd.print("/");
     lcd.setCursor(5, 1);
   //   lcd.print(pulselen);
    
  }



 if (enc1.isHolded()){ // Выход из этого меню
  ModeCur=8;       
  lcd.setCursor(4, 0);
  lcd.print(">");
 }



} while (ModeCur==0);
}


void task_1() {
  Serial.println("Режим АВТО");
  
  do {
enc1.tick();
//for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) 
 // {
 //   pwm.setPWM(SERVO_1, 0, pulselen);
 // }
 // delay(250);
  // Вращение сервопривода 1 вправо
 // for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) 
 // {
 //   pwm.setPWM(SERVO_1, 0, pulselen);
 // }
 // delay(250);


 if (enc1.isHolded()) ModeCur=8;   


} while (ModeCur=1);

 

}
void task_2() {
 lcd.setCursor(4, 0);
  lcd.print("-");
  lcd.setCursor(5, 0);
  lcd.print(ModeName[Mode]);
   
  do {
enc1.tick();
Serial.println("Режим калибровки");

 if (enc1.isHolded()){

  ModeCur=8;       // 8 режим главнго меню
lcd.setCursor(4, 0);
  lcd.print(">");

 } 
} while (ModeCur==2);
}
void task_3() {
  Serial.println("Резервный режим");
  
}
void task_4() {
  Serial.println("Резервный режим");
  
}
void task_5() {
  Serial.println("Режим ручного управления");
  lcd.setCursor(0, 1);
  lcd.print("Drv");
  do {
    enc1.tick();
    if (enc1.isRight()) Srv--;         // если был поворот
    if (enc1.isLeft()) Srv++;
    if (enc1.isTurn()) {     // если был совершён поворот
      if(Srv>SrvAmount) Srv=0;
      if(Srv<0) Srv=SrvAmount; 
      lcd.setCursor(4, 1);
      lcd.print(Srv);
    }
    if (enc1.isClick()) {
      ModeCur=51; 
lcd.setCursor(6, 1);
  lcd.print("Angle");
  do {
    enc1.tick();
    if (enc1.isRight()) value--;         // если был поворот
    if (enc1.isLeft()) value++;
    if (enc1.isTurn()) {     // если был совершён поворот
      if(value>180) value=0;
      if(value<0) Srv=180; 
      lcd.setCursor(12, 1);
      lcd.print("   ");
      lcd.setCursor(12, 1);
      lcd.print(value);
    }
if (enc1.isHolded()){ // Выход из этого меню
      ModeCur=5;       
    lcd.setCursor(6, 1);
    lcd.print("         ");
     }
     } while(ModeCur==51);

    }

    if (enc1.isHolded()){ // Выход из этого меню
      ModeCur=8;       
      lcd.setCursor(4, 0);
      lcd.print(">");
      lcd.setCursor(0, 1);
      lcd.print("     ");
     }
   
  } while(ModeCur==5);
}

void task_6() {
  Serial.println("Резервный режим");
  

}



void RotateServo(){
//for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) 
//  {
//    pwm.setPWM(ServoCur, 0, pulselen);
//  }
//  delay(250);
  // Вращение сервопривода 1 вправо
//  for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) 
 // {
 //   pwm.setPWM(ServoCur, 0, pulselen);
 // }
  delay(250);

}