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

LiquidCrystal_I2C lcd(0x27, 16, 2);

#define SERVOMIN  50     // Минимальная длительность импульса для сервопривод
#define SERVOMAX  600     // Максимальная длина импульса для сервопривода


// Порты энкодера
#define CLK 3
#define DT 4
#define SW 5
// --- Порты энкодера --

Encoder enc1(CLK, DT, SW);
int ModeCur=8; //Текущий режим
int  Mode = 0;
int ModeAmount=7;  //Количество режимов
int Srv = 0; //Дефолтное значение
#define SrvAmount 6 //Количество приводов
int value = 0; //Дефолтное значение
int ServoCur=0;           // Текущий серво 
uint16_t pulselen;

#define LASER A0 // задаем имя для A0
int i;



char* ModeName[7] = {"Calibrate", "Auto", "Demo1", "Demo2","Demo3","Manual", "System"}; //Режимы

byte mode2prg[50][2]; //Массив режима обучения


Servo sdrive[SrvAmount]; //Приводы
int DefAngle[6] = {90, 0, 90, 90, 90, 117};  //Положение по умолчанию
int CurAngle[6] = {90, 0, 90, 90, 90, 117};  //Текущее положение

Adafruit_VL53L0X lox = Adafruit_VL53L0X();

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

// Включаем приводы

  sdrive[0].attach(6); //Ось 0 - поворот вокруг Z
  sdrive[1].attach(7); //Ось 1 - Редуктор. наклон 0-180 (90 вперед)
  sdrive[2].attach(8); //Ось 2 - Наклон 0-180 (90 середина)
  sdrive[3].attach(9); //Ось 3 - Наклон рабочего органа
  sdrive[4].attach(10); // Схват
  sdrive[5].attach(11); // Сканер (резерв)

for (int i = 0; i = 6; i++) {
sdrive[i].write(DefAngle[i]);
}

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

   pinMode(LASER, OUTPUT); // инициализируем Pin10 как выход

Serial.begin(9600); //Консоль
Serial.println("Go-------->>>");
Serial.println(ModeName[Mode]);
Serial.println("--------------------------"); 
Serial.print("Mode - ");
Serial.println(ModeAmount);  

 Serial.println("Adafruit VL53L0X test");
 if (!lox.begin()) {
   Serial.println(F("Failed to boot VL53L0X"));
   // while(1);
  }

 // Инициализация и начало работы с дисплеем
  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(); // не нужна, в этом режиме (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]);
    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();
 Serial.println("Калибровка приводов для сборки манипулятора");

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

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

    
    
    
  }
if (enc1.isClick()) sdrive[0].write(value);


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



} while (ModeCur==0);
}


void task_1() {
  Serial.println("Режим АВТО");
  
  do {
enc1.tick();
wealding();
measure();
  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("                ");
  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) value=180; 
      lcd.setCursor(12, 1);
      lcd.print("   ");
      lcd.setCursor(12, 1);
      lcd.print(value);
    }
    if (enc1.isClick()) RotateServo();     // sdrive[Srv].write(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 HomePosition(){
  sdrive[0].write(90); //Ось 0 - поворот вокруг Z
  sdrive[1].write(0); //Ось 1 - Редуктор. наклон 0-180 (90 вперед)
  sdrive[2].write(0); //Ось 2 - Наклон 0-180 (90 середина)  РЕДАККТИРОВАТЬ
  sdrive[3].write(160); //Ось 3 - Наклон рабочего органа
  sdrive[4].write(10); // Схват    РЕДАКТИРОВАТЬ
  sdrive[5].write(117); // Сканер (резерв)

}

void RotateServo(){
 

//sdrive[Srv].write(value);
if (CurAngle[Srv]==value) {
  sdrive[Srv].write(value);
  CurAngle[Srv]=value;
}

if (CurAngle[Srv]>value) {
  for (i = CurAngle[Srv]; i = value; i++) {
  sdrive[Srv].write(value);
}
} else {
 for (i = CurAngle[Srv]; i = value; i--) {
  sdrive[Srv].write(value);
}

}

for (int i = 0; i < 100; i++) {
  // тело цикла
}

sdrive[Srv].write(value);

}


void measure(){

 VL53L0X_RangingMeasurementData_t measure;
  
  Serial.print("Reading a measurement... ");
  lox.rangingTest(&measure, false); 

  if (measure.RangeStatus != 4) {  
    Serial.print("Distance (mm): "); Serial.println((measure.RangeMilliMeter));
  } else {
    Serial.println(" out of range ");
  }
    
  delay(100);

}




void wealding(){

  for (int i=0; i<=24; i++) // мигание лазерным светодиодом
   {
      digitalWrite(LASER, HIGH);
      delay(50);
      digitalWrite(LASER, LOW);
      delay(50);
   }
   delay(3000);
}