#define DRIVER_STEP_TIME 1
#include <GyverStepper.h>;
#include <LiquidCrystal_I2C.h>
#include <esp_task_wdt.h>

TaskHandle_t Task0;
TaskHandle_t Task1;

#define shnekStopButton 4  //порт кнопки запуска/остановки работы шнеков
#define nozhStopButton 18   //порт кнопки запуска/остановки работы ножа

#define shnekCLK 34  //порт CLK энкодера скорости шаговика шнеков
#define shnekDT 35   //порт DT энкодера скорости шаговика шнеков
#define shnekSW 32   //порт DT энкодера скорости шаговика шнеков

#define nozhCLK 27 //порт CLK энкодера управления ножом
#define nozhDT 14  //порт DT энкодера управления ножом
#define nozhSW 12  //порт SW энкодера управления ножом

#define shnekStep 33 //порт STEP шаговика шнеков
#define nozhkStep 26  //порт STEP шаговика ножа

#define shnekDir 25 //порт DIR шаговика шнеков
#define nozhDir 19  //порт DIR шаговика ножа (не используется)

int shnekMinSpeed = 10; //ввод переменной минимальной скорости шаговика шнеков
int shnekMaxSpeed = 500; //ввод переменной максимальной скорости шаговика шнеков
int speedShnek = 120;  //ввод переменной скорости шаговика шнеков
int shnekAccel = 1;
int oldSpeedShnek = speedShnek+5;  //ввод вспомогательной переменной скорости шаговика шнеков

int pauseCut = 10000; //ввод переменной паузы между махами ножа
int speedCut = 200;  //ввод переменной скорости махов ножа
int nozhUgol = 360; //значение угла поворота вала двигателя ножа

bool shnekLastClk = HIGH; 
bool nozhLastClk = HIGH;
bool shnekStopFlag = HIGH;
bool shnekStopFunc = HIGH; //HIGH - стоп, LOW - работа
bool nozhStopFunc = HIGH; //то же самое, но для ножа
bool nozhMode = LOW; 
bool direction = HIGH; //HIGH - по часовой, LOW - против
bool shnekDirState = LOW; //нужна для отработки переключения


long nozhSpeed = 4000000*nozhUgol/sq(speedCut);

uint32_t encoderTime = millis();
uint32_t timeStopShnek = 0;
uint32_t timeNozh = -30000;
uint32_t shnekStoppingTime = millis();
uint32_t shnekButtonTime = millis();

LiquidCrystal_I2C lcd(0x27, 16, 2);//обозначение ЖК дисплея

GStepper<STEPPER2WIRE> shneki(1600, shnekStep, shnekDir); //определение шаговика "shneki"
GStepper<STEPPER2WIRE> nozh(1600, nozhkStep, nozhDir);   //определение шаговика "hozh"

void setup() {
Serial.begin(115200);

esp_task_wdt_config_t twdt_config = {
        .timeout_ms = 5000,             // оставляем 5 сек (или своё значение)
        .idle_core_mask = (0),
        .trigger_panic = true
    };
    esp_task_wdt_deinit();              // сначала выключаем текущую конфигурацию
    esp_task_wdt_init(&twdt_config);    // применяем новую

lcd.init(); //инициация ЖК дисплея
lcd.backlight();  //включение задней подсветки ЖК дисплея

//Назначение пинов
pinMode(shnekStopButton, INPUT);
pinMode(nozhStopButton, INPUT);

pinMode(shnekCLK, INPUT);
pinMode(shnekDT, INPUT);
pinMode(shnekSW, INPUT_PULLUP);

pinMode(nozhCLK, INPUT);
pinMode(nozhDT, INPUT);
pinMode(nozhSW, INPUT_PULLUP);

pinMode(shnekStep, OUTPUT);
pinMode(shnekDir, OUTPUT);

pinMode(nozhkStep, OUTPUT);
//pinMode(nozhkDir, OUTPUT);

nozh.setRunMode(FOLLOW_POS);  //режим следования к позиции на шаговике ножа
shneki.setRunMode(KEEP_SPEED);//режим поддержания постоянной скорости на шаговике шнеков

nozh.setMaxSpeedDeg(6*3000); //установка максимальной скорости вращения вала шаговика ножа (3000 об/мин)
nozh.setAccelerationDeg(nozhSpeed); //установка ускорения ножа

shneki.setMaxSpeedDeg(6*shnekMaxSpeed);   //установка максимальной скорости вращения вала шаговика шнеков (5оо об/мин)
shneki.setAccelerationDeg(360*shnekAccel); //установка ускорения шаговика шнеков
lcd.setCursor(0, 0); //вывод 1 строки на ЖК дисплее
lcd.print("P,s: C,s: S,RPM:");
//вывод значения паузы между махами в "с"
lcd.setCursor(0, 1);
lcd.print(pauseCut/1000.0,1);
//вывод значения времени маха в "с"
lcd.setCursor(5, 1);
lcd.print(float(speedCut)/1000.0,1);
//вывод направления вращения шаговика шнеков
lcd.setCursor(10, 1);
if (direction == HIGH){lcd.print("+");}
else {lcd.print("-");};
//вывод скорости вращения шаговика шнеков
lcd.setCursor(11, 1);
lcd.print(speedShnek);
//показать стрелочкой выбранный режим настройки (паузы между махами)
lcd.setCursor(4, 0);
lcd.write(127);

  // Создаем задачу с кодом из функции Task0code(),
  // с приоритетом 1 и выполняемую на ядре 0:
  xTaskCreatePinnedToCore(
                    Task0code,
                    "Task0",
                    10000,
                    NULL,
                    1,
                    &Task0,   
                    0);         
  delay(500);

  // Создаем задачу с кодом из функции Task0code(),
  // с приоритетом 1 и выполняемую на ядре 1:
  xTaskCreatePinnedToCore(
                    Task1code,
                    "Task1",
                    10000,
                    NULL,
                    1,
                    &Task1,   
                    1);         
  delay(500);


}

void Task0code( void * pvParameters ){
  for(;;){
    if (shnekStopFunc == LOW) {shneki.tick();};
nozh.tick();
  }
}

void Task1code( void * pvParameters ){
  for(;;){

  //нажатие на энкодер, выбор направления вращения
  if (shnekStopFunc == HIGH && digitalRead(shnekSW) == LOW && millis() - encoderTime >= 5000){
    if(shnekDirState == HIGH && millis()-shnekButtonTime <= 7500){
        lcd.noBacklight();
        delay(250);   
        lcd.backlight();
        direction = !direction;
      lcd.setCursor(10, 1);
      if (direction == HIGH){lcd.print("+");}
        else {lcd.print("-");};}
    if(shnekDirState == HIGH && millis()-shnekButtonTime > 7500){shnekDirState = LOW;}
    if (shnekDirState == LOW) {
      shnekDirState = HIGH;
      shnekButtonTime = millis();};
  encoderTime = millis();
  };

   //опрос энкодера скорости
  int shnekNewClk = digitalRead(shnekCLK);
  if (shnekNewClk != shnekLastClk && millis()-encoderTime >= 10){
    shnekLastClk = shnekNewClk;
    int shnekValueDT = digitalRead(shnekDT);
    if (shnekNewClk == LOW && shnekValueDT == HIGH) {speedShnek+=5;} //при движении энкодера по часовой
    if (shnekNewClk == LOW && shnekValueDT == LOW) {speedShnek-=5;};//при движении энкодера против часовой
    speedShnek = constrain(speedShnek, shnekMinSpeed, shnekMaxSpeed);
    lcd.setCursor(11, 1);
    lcd.print(speedShnek);
    lcd.print(" ");
    lcd.print(" ");
    lcd.print(" ");
    encoderTime = millis();
  };



  //назначение новых скоростей
  if (speedShnek != oldSpeedShnek && digitalRead(shnekStopButton)==true && millis() - encoderTime >= 1000){
    oldSpeedShnek = speedShnek;
    if (direction == HIGH ) {shneki.setSpeedDeg(6*speedShnek, SMOOTH);}
      else{shneki.setSpeedDeg(-6*speedShnek, SMOOTH);};
    if (shnekStopFunc == HIGH){shnekStopFunc = LOW;};
    if (shnekStopFlag == HIGH){shnekStopFlag = LOW;};
    encoderTime = millis();    
    };



  //останов
  if (shnekStopFlag==HIGH && shnekStopFunc == LOW && millis() - shnekStoppingTime >= timeStopShnek) {shnekStopFunc = HIGH;};

  //расчёт времени для останова
  if (digitalRead(shnekStopButton)==false && shnekStopFlag==LOW) {
    timeStopShnek = abs(((speedShnek/shnekAccel)*16.67));
    shnekStoppingTime = millis();
    oldSpeedShnek = speedShnek+20;
    shneki.stop();
    shnekStopFlag=HIGH;
  };



  //настройка ножа
  if (digitalRead(nozhSW) == LOW && millis() - encoderTime >= 1000) {
  nozhMode = !nozhMode;
  lcd.setCursor(4, 0);
  if (nozhMode == LOW){lcd.write(127);}
  else {lcd.write(126);};
  encoderTime = millis();
  }



  int nozhNewClk = digitalRead(nozhCLK);
  if (nozhNewClk != nozhLastClk && millis()-encoderTime >= 10) {
    nozhLastClk = nozhNewClk;
    int nozhValueDT = digitalRead(nozhDT);
    if(nozhMode == LOW){
      if (nozhNewClk == LOW && nozhValueDT == HIGH) {pauseCut+=500;} //при движении энкодера по часовой
      if (nozhNewClk == LOW && nozhValueDT == LOW) {pauseCut-=500;};//при движении энкодера против часовой
      pauseCut = constrain(pauseCut, 1000, 30000);
      lcd.setCursor(0, 1);
      lcd.print(pauseCut/1000.0,1);
      lcd.print(" ");   
    };



    if(nozhMode == HIGH){
      if (nozhNewClk == LOW && nozhValueDT == HIGH) {speedCut+=100;} //при движении энкодера по часовой
      if (nozhNewClk == LOW && nozhValueDT == LOW) {speedCut-=100;};//при движении энкодера против часовой
      speedCut = constrain(speedCut, 100, 1000);
      lcd.setCursor(5, 1);
      lcd.print(float(speedCut)/1000.0,1);
      lcd.print("  ");
      nozhSpeed = nozhSpeed = 4000000*nozhUgol/sq(speedCut);     
      nozh.setAccelerationDeg(nozhSpeed);
    };
  encoderTime = millis(); 
  }



  //запуск по таймеру
  if (digitalRead(nozhStopButton)==true && millis() - timeNozh >= pauseCut){
    nozh.setTargetDeg(nozhUgol, RELATIVE);
    timeNozh = millis();
  };
//конец подпрограммы
  }
}




void loop() {

  }