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

TaskHandle_t Task0;
TaskHandle_t Task1;

#define dirPin 17
#define stepPin 18
#define stopButton 16

#define speedCLK 35  //порт CLK энкодера 1
#define speedDT 32   //порт DT энкодера 1
#define speedSW 33  //порт SW энкодера 1

#define accelCLK 25 //порт CLK энкодера 2
#define accelDT 26  //порт DT энкодера 2
#define accelSW 27  //порт SW энкодера 2

int maxSpeed = 300;
int minSpeed = 10;

int maxAccel = 10;
int minAccel = 1;

int stepPul = 1600;
int speedUpd = 1000;

int speed =120;
int oldSpeed = speed+1;
int accel = 5;
int oldAccel = accel+1;
int rotation = 5000;
int cycle = 10000;
int posSky = 0;
int posSkyOld = 0;
int speedSky = 120;

int turnDebounce = 10;   // мс между срабатываниями поворота

int motorMod = 1; //1 - SPEED, 2 - ROTATE 3 - FOllOW POS, 4 - SKY

bool direct = HIGH; //HIGH - по часовой, LOW - против
bool rotationMode = LOW; //HIGH - регулировка работы, LOW - регулировка цикла
bool stopFunc = HIGH; //HIGH - стоп, LOW - работа
bool stopButtonFunc = HIGH; //то же самое, но кнопка

bool lastSpeedCLK = HIGH;
bool lastAccelCLK = HIGH;

long position = 0;
long oldPosition = position+1;
int posIncrem=360;
int incVar = 10;

int angle =720;
float cycleSpeedDeg = 4*angle/sq(rotation/1000);

uint32_t zaWarudo = 0;

uint32_t workTime = -5000;
uint32_t encoderTime = -5000;
uint32_t cycleTime = -10000;
uint32_t stoppingTime = millis();
uint32_t lastSpeedTurn = 0;
uint32_t lastAccelTurn = 0;

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

GStepper<STEPPER2WIRE> motor(stepPul, stepPin, dirPin);


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(dirPin, OUTPUT);
pinMode(stepPin, OUTPUT);

pinMode(speedCLK, INPUT);
pinMode(speedDT, INPUT);
pinMode(speedSW, INPUT_PULLUP);

pinMode(accelCLK, INPUT);
pinMode(accelDT, INPUT);
pinMode(accelSW, INPUT_PULLUP);

pinMode(stopButton, INPUT_PULLDOWN);

motor.setRunMode(KEEP_SPEED);
motor.setMaxSpeedDeg(6*maxSpeed);
motor.setAcceleration(360*accel);

lcd.setCursor(0, 0); //вывод 1 строки на ЖК дисплее
lcd.print("ACCEL DIR  SPEED");
lcd.setCursor(2, 1);
lcd.print(accel);

lcd.setCursor(7, 1);
lcd.print("+");

lcd.setCursor(12, 1);
lcd.print(speed);

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

  // Создаем задачу с кодом из функции Task1code(),
  // с приоритетом 1 и выполняемую на ядре 1:
  xTaskCreatePinnedToCore(
                    Task1code,   /* Функция задачи */
                    "Task1",     /* Название задачи */
                    10000,       /* Размер стека задачи */
                    NULL,        /* Параметр задачи */
                    1,           /* Приоритет задачи */
                    &Task1,      /* Идентификатор задачи,
                                    чтобы ее можно было отслеживать */
                    1);          /* Ядро для выполнения задачи (1) */
    delay(500); 
}



void Task0code( void * pvParameters ){
  for(;;){
  if ((motorMod == 1||motorMod ==4) && stopFunc == HIGH){}
  else {motor.tick();};
  } 
}



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

//нажатие на энкодер ускорения
if (digitalRead(accelSW) == LOW && digitalRead(stopButton)==false && millis() - encoderTime >= 1000 && stopFunc == HIGH) {
  motorMod = motorMod+1;
  if (motorMod >=5){motorMod = 1;};

  if (motorMod == 1){
    lcd.clear();
    motor.setRunMode(KEEP_SPEED);
    lcd.setCursor(0, 0);
    lcd.print("ACCEL DIR  SPEE");
    lcd.print("D");
    lcd.setCursor(2, 1);
    lcd.print(accel);
    lcd.setCursor(7, 1);
    if(direct == HIGH){lcd.print("+");}
    else {lcd.print("-");};
    lcd.setCursor(12, 1);   
    lcd.print(speed);
    motor.setMaxSpeedDeg(6*maxSpeed);
    motor.setAccelerationDeg(360*accel);
    encoderTime = millis();    
    }

  if (motorMod == 2){
    motor.stop();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("ANGLE ROT");
    if (rotationMode == HIGH){lcd.write(127); lcd.print(" ");}
    if (rotationMode == LOW){lcd.print(" "); lcd.write(126);};
    lcd.print("CYCLE");    
    lcd.setCursor(0, 1);
    lcd.print(angle);
    lcd.setCursor(6, 1);
    lcd.print(float(rotation/1000.0),1);
    lcd.setCursor(12, 1);
    lcd.print(float(cycle/1000.0),1);
    motor.setAccelerationDeg(cycleSpeedDeg);
    motor.setMaxSpeedDeg(60*maxSpeed);
    motor.setCurrentDeg(0);
    motor.setRunMode(FOLLOW_POS);
    encoderTime = millis()-10000;    
    }    

  if (motorMod == 3){
    lcd.clear();
    position = 0;
    oldPosition = position;
    motor.setCurrentDeg(position);
    motor.setTargetDeg(position, ABSOLUTE);
    lcd.setCursor(0, 0);
    lcd.clear();
    lcd.print("INCR  TARGET POS");
    lcd.setCursor(1, 1);
    lcd.print(int(posIncrem));
    lcd.setCursor(8, 1);
    lcd.print(int(position));
    motor.setAccelerationDeg(360*accel);
    motor.setMaxSpeedDeg(6*speed);
    encoderTime = millis();    
    }

  if (motorMod == 4){
    lcd.clear();
    motor.setRunMode(KEEP_SPEED);
    lcd.setCursor(0, 0);
    lcd.print("SKY POS  SPEE");
    lcd.print("D");
    lcd.setCursor(4, 1);
    lcd.print(posSky);
    lcd.setCursor(12, 1);   
    lcd.print(speedSky);
    motor.setMaxSpeedDeg(6*maxSpeed);
    motor.setAccelerationDeg(360*10);
    encoderTime = millis();    
    }
};


//нажатие на энкодер скорости
if (digitalRead(speedSW) == LOW && millis() - encoderTime >= 2500){
  if(motorMod == 1){
    direct = !direct;
    lcd.setCursor(7, 1);
    if (direct == HIGH){lcd.print("+");}
    else { lcd.print("-");};
    if (stopButtonFunc == LOW){
      motor.setAcceleration(float(stepPul*accel));
      if (direct == HIGH ) {motor.setSpeedDeg(6*speed, SMOOTH);}
      else{motor.setSpeedDeg(-6*speed, SMOOTH);};};
    encoderTime = millis();}


  if(motorMod == 2){
        rotationMode =!rotationMode;
    lcd.setCursor(9, 0);
    if (rotationMode == HIGH){lcd.write(127); lcd.print(" ");}
    if (rotationMode == LOW){lcd.print(" "); lcd.write(126);};
  encoderTime = millis();}


  if(motorMod == 3 && stopButtonFunc == HIGH){
    position = 0;
    motor.setCurrentDeg(position);
    lcd.setCursor(8, 1);
    lcd.print(int(position));
    lcd.print("       ");}
};

uint32_t now = millis();  

  // ──── Поворот speed энкодера ─────
    bool currSpeedCLK = digitalRead(speedCLK);
    if (currSpeedCLK != lastSpeedCLK) {
      if (currSpeedCLK == LOW && now - lastSpeedTurn > turnDebounce) {  // FALLING на CLK
        int delta = (digitalRead(speedDT) == HIGH) ? +1 : -1;

        if (motorMod == 1) {
          speed += delta * 5;
          speed = constrain(speed, minSpeed, maxSpeed);
          lcd.setCursor(12, 1); lcd.print(speed); lcd.print("   ");
        }
        if (motorMod == 2) {
          if (!rotationMode) {
            cycle += delta * 500;
            cycle = constrain(cycle, 8000, 30000);
            lcd.setCursor(12, 1); lcd.print(float(cycle)/1000,1); lcd.print("  ");
            if (rotation >= cycle - 1000) {
              rotation = constrain(rotation, 5000, cycle - 3000);
              lcd.setCursor(6, 1); lcd.print(float(rotation)/1000,1); lcd.print("  ");
              cycleSpeedDeg = 4.0 * angle / sq(rotation / 1000.0);
              motor.setAccelerationDeg(cycleSpeedDeg);
            }
          } else {
            rotation += delta * 500;
            rotation = constrain(rotation, 5000, cycle - 3000);
            lcd.setCursor(6, 1); lcd.print(float(rotation)/1000,1); lcd.print("  ");
            cycleSpeedDeg = 4.0 * angle / sq(rotation / 1000.0);
            motor.setAccelerationDeg(cycleSpeedDeg);
          }
        }
        if (motorMod == 3) {
          position += delta * posIncrem;
          lcd.setCursor(8, 1); lcd.print(position); lcd.print("       ");
        }
        if (motorMod == 4) {
          speedSky += delta * 5;
          speedSky = constrain(speedSky, minSpeed, maxSpeed);
          lcd.setCursor(12, 1); lcd.print(speedSky); lcd.print("   ");
        }

        lastSpeedTurn = now;
      }
      lastSpeedCLK = currSpeedCLK;
};

  // ──── Поворот accel энкодера ─────
    bool currAccelCLK = digitalRead(accelCLK);
    if (currAccelCLK != lastAccelCLK) {
      if (currAccelCLK == LOW && now - lastAccelTurn > turnDebounce) {
        int delta = (digitalRead(accelDT) == HIGH) ? +1 : -1;

        if (motorMod == 1) {
          accel += delta;
          accel = constrain(accel, minAccel, maxAccel);
          lcd.setCursor(2, 1); lcd.print(accel); lcd.print(" ");
        }
        if (motorMod == 2) {
          angle += delta * incVar;
          angle = constrain(angle, incVar, 3600);
          lcd.setCursor(0, 1); lcd.print(angle); lcd.print("  ");
          if (angle < 100) lcd.print(" ");
          if (angle < 10) lcd.print(" ");
          cycleSpeedDeg = 4.0 * angle / sq(rotation / 1000.0);
          motor.setAccelerationDeg(cycleSpeedDeg);
        }
        if (motorMod == 3) {
          posIncrem += delta * incVar;
          posIncrem = constrain(posIncrem, incVar, 3600);
          lcd.setCursor(0, 1);
          if (posIncrem >= 1000) {lcd.print(posIncrem);}
          else { lcd.print(" "); lcd.print(posIncrem); }
          lcd.print("   ");
        }
        if (motorMod == 4) {
          posSky += delta;
          posSky = constrain(posSky, -5, 5);
          lcd.setCursor(4, 1); lcd.print(posSky); lcd.print(" ");
        }

        lastAccelTurn = now;
      }
      lastAccelCLK = currAccelCLK;
};

if(motorMod == 1){//режим скорости

  //назначение новых скоростей через промежуток времени
  if (speed != oldSpeed || oldAccel != accel && millis() - workTime >= 50){
   if (digitalRead(stopButton)==true){
    if (speed != oldSpeed){
      oldSpeed = speed;
      if (direct == HIGH ) {motor.setSpeedDeg(6*speed, SMOOTH);}
      else{motor.setSpeedDeg(-6*speed, SMOOTH);};};
      if (oldAccel != accel){
        oldAccel = accel;
        motor.setAcceleration(float(stepPul*accel));};      
    if (stopButtonFunc == HIGH){stopButtonFunc = LOW;};
    if (stopFunc == HIGH){stopFunc = LOW;};
    workTime = millis();};};      
      


  //останов
  if (stopButtonFunc==HIGH && stopFunc == LOW && millis() - stoppingTime >= zaWarudo) {stopFunc = HIGH;};

 
  //расчёт времени для останова
    if (digitalRead(stopButton)==false && stopButtonFunc==LOW && millis() - workTime >= 1000) {
    zaWarudo = abs(((speed/accel)*16.67));
    oldSpeed = speed+20;
    oldAccel = accel+20;
    if (oldAccel != accel){
      oldAccel = accel;
      motor.setAcceleration(float(stepPul*accel));};
    motor.stop();
    stopButtonFunc=HIGH;
    stoppingTime = millis();
    workTime = millis();};
}


if(motorMod == 2){//режим поворота

  //запуск по таймеру
  if (digitalRead(stopButton)==true && millis() - cycleTime >= cycle){ 
    if (direct == HIGH) {motor.setTargetDeg(angle, RELATIVE);}
    else{motor.setTargetDeg(-angle, RELATIVE);};
    if(stopButtonFunc == HIGH){stopButtonFunc = LOW;};
    cycleTime = millis();
  };    


  //останов
  if (digitalRead(stopButton)==false && stopButtonFunc == LOW) {
    motor.stop();
    stopButtonFunc = HIGH;  
  };
}



if(motorMod == 3){//режим координаты

  //перемещение в координату
  if (digitalRead(stopButton)==true && oldPosition != position){
    oldPosition = position;
    motor.setTargetDeg(position, ABSOLUTE);
    if(stopButtonFunc == HIGH){stopButtonFunc = LOW;};
    };
 

  if (stopFunc == HIGH && digitalRead(stopButton)==true && motor.getCurrent() != position){
    motor.setTargetDeg(position, ABSOLUTE);
    if(stopButtonFunc == HIGH){stopButtonFunc = LOW;}; 
    if(stopFunc == HIGH){stopFunc = LOW;};     
  };


  //останов
  if (digitalRead(stopButton)==false && stopButtonFunc == LOW) {
    motor.stop();
    if(stopButtonFunc == LOW){stopButtonFunc = HIGH;};
    if(stopFunc == LOW){stopFunc = HIGH;};
    };
}


if(motorMod == 4){//режим лыжного конвеера


  if (posSky != posSkyOld){
   if (digitalRead(stopButton)==true){
    int posSpeed = map(posSky,-5,5,-speedSky,speedSky);
    motor.setSpeedDeg(6*posSpeed, SMOOTH);
    if (stopFunc == HIGH){stopFunc = LOW;};
    posSkyOld = posSky;};
    };


  if (digitalRead(stopButton)==false || posSky == 0 && stopFunc == LOW){
  motor.brake();
  (stopFunc = HIGH);};
};

}
}


void loop() {

}