#define L1 20
#define L2 3
#define L3 46
#define L4 10
#define SW1 39
#define SW2 37
#define SW3 35
#define stepL 8
#define dirL 18
#define stepR 17
#define dirR 16

#include "ESP32Servo.h"
Servo servL;
Servo servR;

volatile int counter = 10;
int prev_counter = 10;
int prev_counterL = 10;
int prev_counterR = 10;
volatile byte f = 0;
unsigned long prevBlink = 0;
int delayBlink = 100;
volatile bool mode;
volatile unsigned long prev;
volatile byte state = 0;
bool kostyl = 1;
volatile unsigned long errorMs = 200;
int mult = 5 * 16;
volatile unsigned long lastButtonMs = 0;
const unsigned long DEBOUNCE_MS = 200;

bool SetLED1 = 1;
bool SetLED2 = 0;





void shaft(){
  if (digitalRead(4) == 1){
    if (counter < 20){
    counter++;
    f = 1;
    }
  }
  if (digitalRead(4) == 0){
  if (counter > 0){
    counter--;
    f = 2;
  }
  }

}

void singleStep(int steps, int pin){
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(pin, HIGH);
    delayMicroseconds(300);
    digitalWrite(pin, LOW);
    delayMicroseconds(300);
  }
}

void stepper(){
  int steps = counter - prev_counter;

  if(steps > 0){
    digitalWrite(dirL, HIGH);
    digitalWrite(dirR, LOW);
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(stepL, HIGH);
    digitalWrite(stepR, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepR, LOW);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
    prev_counterR = 20 - prev_counter;
  }
    if(steps < 0){
    digitalWrite(dirL, LOW);
    digitalWrite(dirR, HIGH);
  for(int i = 0; i < abs(steps) * mult; i++){
    digitalWrite(stepR, HIGH);
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepR, LOW);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
    prev_counterR = 20 - prev_counter;
  } 
}

void stepperL(){
  
  int steps = counter - prev_counterL;
  if(steps > 0){
    digitalWrite(dirL, HIGH);
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
  }
    if(steps < 0){
    digitalWrite(dirL, LOW);
  for(int i = 0; i < abs(steps) * mult; i++){
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
  } 
}

void stepperR(){
  int steps = counter - prev_counterR;
  if(steps > 0){
    digitalWrite(dirR, HIGH);
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(stepR, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepR, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterR = prev_counter;
  }
    if(steps < 0){
    digitalWrite(dirR, LOW);
  for(int i = 0; i < abs(steps) * mult; i++){
    digitalWrite(stepR, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepR, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterR = prev_counter;
  } 
}

void stepperM(){
  int steps = counter - prev_counter;
  if(steps > 0){
    digitalWrite(dirR, HIGH);
    digitalWrite(dirL, HIGH);
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(stepR, HIGH);
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepR, LOW);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
    prev_counterR = prev_counter;
  }
    if(steps < 0){
    digitalWrite(dirR, LOW);
    digitalWrite(dirL, LOW);
  for(int i = 0; i < abs(steps) * mult; i++){
    digitalWrite(stepR, HIGH);
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepR, LOW);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
    prev_counterR = prev_counter;
  } 
}




void servo(){
servL.write(counter * 9);
servR.write(180 -(counter * 9));
}

void servoL(){
servL.write(counter * 9);
}

void servoR(){
servR.write(counter * 9);
}

void servoM(){
servL.write(counter * 9);
servR.write(counter * 9);
}




void buttonStep(){
  unsigned long now = millis();
  if (now - lastButtonMs < DEBOUNCE_MS) return;
  lastButtonMs = now;

  prev = now;
  if (mode == 1) state = 0;
  if (state < 4) state += 1; else state = 1;
  mode = 0;
}

void buttonServ(){
  unsigned long now = millis();
  if (now - lastButtonMs < DEBOUNCE_MS) return;
  lastButtonMs = now;

  if (mode == 0) state = 0;
  if (state < 4) state += 1; else state = 1;
  mode = 1;
}

void buttonReset(){
  unsigned long now = millis();
  if (now - lastButtonMs < DEBOUNCE_MS) return;
  lastButtonMs = now;


  counter = 10;
  int steps = counter - prev_counterL;
  if(steps > 0){
    digitalWrite(dirL, HIGH);
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
  }
    if(steps < 0){
    digitalWrite(dirL, LOW);
  for(int i = 0; i < abs(steps) * mult; i++){
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
    }
    counter = 10;
    steps = counter - prev_counterR;
    if(steps > 0){
    digitalWrite(dirR, HIGH);
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(stepR, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepR, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterR = prev_counter;
  }
    if(steps < 0){
    digitalWrite(dirR, LOW);
  for(int i = 0; i < abs(steps) * mult; i++){
    digitalWrite(stepR, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepR, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterR = prev_counter;
    
  }

  servL.write(90);
servR.write(90);
counter = 10;

}


void demonstration(){
switch (state){
  case 1:
      if (mode == 0){
      if (kostyl){

        counter = 20 - prev_counterR;
      int steps = counter - prev_counterR;

  if(steps > 0){
    digitalWrite(dirL, HIGH);
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
  }
    if(steps < 0){
    digitalWrite(dirL, LOW);
  for(int i = 0; i < abs(steps) * mult; i++){
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
  } 
  kostyl = 0;
  }
      digitalWrite(L1, LOW);
      digitalWrite(L2, HIGH);
      digitalWrite(L3, LOW);
      digitalWrite(L4, HIGH);
        while (state == 1 && mode == 0){
          stepper();
        }
      }
      else{
      digitalWrite(L4, LOW);
      digitalWrite(L3, HIGH);
      digitalWrite(L2, LOW);
      digitalWrite(L1, HIGH);
        while (state == 1 && mode == 1){
          servo();
        }
      }
      break;
  case 2:

      if (mode == 0){
      kostyl = 1;
        counter = prev_counterL;
        digitalWrite(L1, LOW);
      digitalWrite(L2, LOW);
      digitalWrite(L3, LOW);
      digitalWrite(L4, HIGH);
        while (state == 2 && mode == 0){
          stepperL();
        }
      }
      else{
        digitalWrite(L1, HIGH);
      digitalWrite(L2, LOW);
      digitalWrite(L3, HIGH);
      digitalWrite(L4, HIGH);
        while (state == 2 && mode == 1){
          servoL();
        }
      }
      break;
  case 3:
      if (mode == 0){
        counter = prev_counterR;
         digitalWrite(L1, HIGH);
      digitalWrite(L2, HIGH);
      digitalWrite(L3, LOW);
      digitalWrite(L4, HIGH);
        while (state == 3 && mode == 0){
          stepperR();
        }
      }
      else{
         digitalWrite(L1, HIGH);
      digitalWrite(L2, LOW);
      digitalWrite(L3, LOW);
      digitalWrite(L4, LOW);
        while (state == 3 && mode == 1){
          servoR();
        }
      }
      break;
  case 4:
      if (mode == 0){

          int steps = counter - prev_counterL;
  if(steps > 0){
    digitalWrite(dirL, HIGH);
  for(int i = 0; i < steps * mult; i++){
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
  }
    if(steps < 0){
    digitalWrite(dirL, LOW);
  for(int i = 0; i < abs(steps) * mult; i++){
    digitalWrite(stepL, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    delayMicroseconds(300);
  }
    prev_counter = counter;
    prev_counterL = prev_counter;
  } 
        digitalWrite(L3, LOW);
        digitalWrite(L4, HIGH);
        while (state == 4 && mode == 0){
          if(millis() - prevBlink >= 100){
            SetLED1 = !SetLED1;
            SetLED2 = !SetLED2;
            digitalWrite(L2, SetLED2);
            digitalWrite(L1, SetLED1);
            prevBlink = millis();
          }
          stepperM();
        }
      }
      else{
        digitalWrite(L2, LOW);
        digitalWrite(L1, HIGH);
        while (state == 4 && mode == 1){
          if(millis() - prevBlink >= 100){
            SetLED1 = !SetLED1;
            SetLED2 = !SetLED2;
            digitalWrite(L3, SetLED2);
            digitalWrite(L4, SetLED1);
            prevBlink = millis();
          }
          servoM();
        }
      }
      break;
  }
  
}

void setup() {
  // put your setup code here, to run once:
 
  Serial.begin(9600);
  attachInterrupt(digitalPinToInterrupt(5), shaft, FALLING);
  attachInterrupt(digitalPinToInterrupt(SW1), buttonStep, FALLING);
  attachInterrupt(digitalPinToInterrupt(SW2), buttonServ, RISING);
  attachInterrupt(digitalPinToInterrupt(SW3), buttonReset, RISING);
  pinMode(L1, OUTPUT);
  pinMode(L2, OUTPUT);
  pinMode(L3, OUTPUT);
  pinMode(L4, OUTPUT);
  digitalWrite(L1, HIGH);
  digitalWrite(L2, LOW);
  digitalWrite(L3, LOW);
  digitalWrite(L4, HIGH);
  pinMode(SW1, INPUT);
  pinMode(SW2, INPUT);
  pinMode(SW3, INPUT);
  pinMode(4, INPUT); 
  pinMode(stepR, OUTPUT);
  pinMode(dirR, OUTPUT);
  pinMode(stepL, OUTPUT);
  pinMode(dirL, OUTPUT);
  servL.attach(40);
  servR.attach(41);
  servL.write(90);
  servR.write(90);
  delay(1000);
  digitalWrite(dirL, LOW);
  digitalWrite(dirR, LOW);
  for(int i = 0; i < 10 * mult; i++){
    digitalWrite(stepL, HIGH);
    digitalWrite(stepR, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    digitalWrite(stepR, LOW);
    delayMicroseconds(300);
    
    if (i % 5*16 == 0){
      SetLED1 = !SetLED1;
      SetLED2 = !SetLED2;
      digitalWrite(L1, SetLED1);
  digitalWrite(L2, SetLED2);
  digitalWrite(L3, SetLED2);
  digitalWrite(L4, SetLED1);
    }
}

      digitalWrite(dirL, HIGH);
  digitalWrite(dirR, HIGH);
  for(int i = 0; i < 20 * mult; i++){
    digitalWrite(stepL, HIGH);
    digitalWrite(stepR, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    digitalWrite(stepR, LOW);
    delayMicroseconds(300);
    if (i % 5*16 == 0){
      SetLED1 = !SetLED1;
      SetLED2 = !SetLED2;
      digitalWrite(L1, SetLED1);
  digitalWrite(L2, SetLED2);
  digitalWrite(L3, SetLED2);
  digitalWrite(L4, SetLED1);
    }
}


  digitalWrite(dirL, LOW);
  digitalWrite(dirR, LOW);
  for(int i = 0; i < 10 * mult; i++){
    digitalWrite(stepL, HIGH);
    digitalWrite(stepR, HIGH);
    delayMicroseconds(300);
    digitalWrite(stepL, LOW);
    digitalWrite(stepR, LOW);
    delayMicroseconds(300);
    if (i % 5*16 == 0){
      SetLED1 = !SetLED1;
      SetLED2 = !SetLED2;
      digitalWrite(L1, SetLED1);
  digitalWrite(L2, SetLED2);
  digitalWrite(L3, SetLED2);
  digitalWrite(L4, SetLED1);
    }
}
}
void loop() {     
demonstration();

}