#include <ESP32Servo.h>
#include <cmath>

//переменные отвечающие за светодоиды
int pin_L1 = 20;
int pin_L2 = 3;
int pin_L3 = 46;
int pin_L4 = 10;
int led_count = 0;

//переменные отвечающие за серво
Servo servo1;
Servo servo2;
bool servo_flag = true;

//переменные отвечающие за моторы
int pin_Motor_1_dir = 16;
int pin_Motor_1_step = 17;
int pin_Motor_2_dir = 18;
int pin_Motor_2_step = 8;
double motor_pos_1 = 0;
double motor_pos_2 = 0;
bool motor_start = true;
bool motor_middle = true;
bool motor_finish = true;
bool motor_flag = true;
bool motor_step_1 = false;
bool motor_step_2 = false;
bool motor_change = false;

//переменные отвечающие за кнопки
int pin_sw_1 = 39;
int pin_sw_2 = 37;
int pin_sw_3 = 35;
bool sw3_flag = true;

//переменные отвечающие за переключение режимов моторов и серво
bool motor_mode = true;
bool servo_mode = true;
int mode = 0;

//пременные отвечающие за энкодер
int pin_encoder_1 = 5;
int pin_encoder_2 = 4;
bool prev;


//millis
unsigned long mTime;
unsigned long mTime_l = 0;
unsigned long mTime_motor = 0;
unsigned long mTime_led = 0; 
unsigned long mTime_encoder = 0;
unsigned long mTime_servo_and_motor = 0;
unsigned long mTime_servo_and_motor_mode = 0;

// управление серво
void change_servo(int mode, int range) {
  int angle1;
  int angle2;
  if (mode == 1) {
    if (servo1.read() + range > 180) angle1 = 180;
    else if (servo1.read() + range < 0) angle1 = 0;
    else angle1 = servo1.read() + range;

    if (servo2.read() + range > 180) angle2 = 180;
    else if (servo2.read() + range < 0) angle2 = 0;
    else angle2 = servo2.read() + range;

    servo1.write(angle1);
    servo2.write(angle2);
  }
  else if (mode == 2) {
    if (servo1.read() + range > 180) angle1 = 180;
    else if (servo1.read() + range < 0) angle1 = 0;
    else angle1 = servo1.read() + range;

    servo1.write(angle1);
  }
  else if (mode == 3) {
    if (servo2.read() + range > 180) angle2 = 180;
    else if (servo2.read() + range < 0) angle2 = 0;
    else angle2 = servo2.read() + range;
    servo2.write(angle2);
  }
  else if (mode == 4) {
    if (servo1.read() + range > 180) angle1 = 180;
    else if (servo1.read() + range < 0) angle1 = 0;
    else angle1 = servo1.read() + range;

    servo1.write(angle1);
    servo2.write(180 - angle1);
  }
};

void moveMotor1ToZero() {
  int stepsToZero = -motor_pos_1; 

  if (stepsToZero > 0) {
    digitalWrite(pin_Motor_1_dir, HIGH); 
  } else {
    digitalWrite(pin_Motor_1_dir, LOW);  
    stepsToZero = -stepsToZero;          
  }
  for (int i = 0; i < stepsToZero; i++) {
    digitalWrite(pin_Motor_1_step, HIGH);
    delayMicroseconds(1); 
    digitalWrite(pin_Motor_1_step, LOW);
    delayMicroseconds(1);
  }
  motor_pos_1 = 0; 
}

void moveMotor2ToZero() {
  int stepsToZero = -motor_pos_2; 

  if (stepsToZero > 0) {
    digitalWrite(pin_Motor_2_dir, HIGH); 
  } else {
    digitalWrite(pin_Motor_2_dir, LOW);  
    stepsToZero = -stepsToZero;          
  }
  for (int i = 0; i < stepsToZero; i++) {
    digitalWrite(pin_Motor_2_step, HIGH);
    delayMicroseconds(1); 
    digitalWrite(pin_Motor_2_step, LOW);
    delayMicroseconds(1);
  }
  motor_pos_2 = 0; 
}

void syncMotor2ToMirrorMotor1() {
  int mirroredPosition = -motor_pos_1;
  int stepsToMove = mirroredPosition - motor_pos_2;

  if (stepsToMove > 0) {
    digitalWrite(pin_Motor_2_dir, HIGH);
  } else {
    digitalWrite(pin_Motor_2_dir, LOW);
    stepsToMove = -stepsToMove;
  }

  for (int i = 0; i < stepsToMove; i++) {
    digitalWrite(pin_Motor_2_step, HIGH);
    delay(1);
    digitalWrite(pin_Motor_2_step, LOW);
    delay(1);
  }

  motor_pos_2 = mirroredPosition;
}

void change_motor(int mode, double range) {
  bool direction;
  if (range > 0) direction = true;
  else direction = false;
  if (mode == 1) {
    for (int i = 0; i < 40 * abs(range); i++) {
      digitalWrite(pin_Motor_1_dir, direction);
      digitalWrite(pin_Motor_2_dir, direction);
      digitalWrite(pin_Motor_1_step, LOW);
      digitalWrite(pin_Motor_2_step, LOW);
      motor_pos_1 += range;
      motor_pos_2 += range;
      delay(1);
      digitalWrite(pin_Motor_1_step, HIGH);
      digitalWrite(pin_Motor_2_step, HIGH);
      delay(1);
      digitalWrite(pin_Motor_1_step, LOW);
      digitalWrite(pin_Motor_2_step, LOW);
      delay(1);
    }
  }
  else if (mode == 2) {
    for (int i = 0; i < 40 * abs(range); i++) {
      digitalWrite(pin_Motor_1_dir, direction);
      digitalWrite(pin_Motor_1_step, LOW);
      motor_pos_1 += range;
      delay(1);
      digitalWrite(pin_Motor_1_step, HIGH);
      delay(1);
      digitalWrite(pin_Motor_1_step, LOW);
      delay(1);
    }
  }
  else if (mode == 3) {
    for (int i = 0; i < 40 * abs(range); i++) {
      digitalWrite(pin_Motor_2_dir, direction);
      digitalWrite(pin_Motor_2_step, LOW);
      motor_pos_2 += range;
      delay(1);
      digitalWrite(pin_Motor_2_step, HIGH);
      delay(1);
      digitalWrite(pin_Motor_2_step, LOW);
      delay(1);
    }
  }
  else if (mode == 4) {
    syncMotor2ToMirrorMotor1();
    for (int i = 0; i < 40 * abs(range); i++) {
      digitalWrite(pin_Motor_1_dir, direction);
      digitalWrite(pin_Motor_2_dir, not direction);
      digitalWrite(pin_Motor_1_step, LOW);
      digitalWrite(pin_Motor_2_step, LOW);
      motor_pos_1 += range;
      motor_pos_2 -= range;
      delay(1);
      digitalWrite(pin_Motor_1_step, HIGH);
      digitalWrite(pin_Motor_2_step, HIGH);
      delay(1);
      digitalWrite(pin_Motor_1_step, LOW);
      digitalWrite(pin_Motor_2_step, LOW);
      delay(1);
    }
  }
};

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

  //подключение светодиодов
  pinMode(pin_L1, OUTPUT);
  pinMode(pin_L2, OUTPUT);
  pinMode(pin_L3, OUTPUT);
  pinMode(pin_L4, OUTPUT);

  //подключение серво
  servo1.attach(40);
  servo1.write(90);
  servo2.attach(41);
  servo2.write(90);

  //подключение моторов
  pinMode(pin_Motor_1_dir, OUTPUT);
  pinMode(pin_Motor_1_step, OUTPUT);
  pinMode(pin_Motor_2_dir, OUTPUT);
  pinMode(pin_Motor_2_step, OUTPUT);

  //подключение кнопок
  pinMode(pin_sw_1, INPUT);
  pinMode(pin_sw_2, INPUT);
  pinMode(pin_sw_3, INPUT);
  
  //подключение энкодера
  pinMode(pin_encoder_1, INPUT);
  pinMode(pin_encoder_2, INPUT);

}

void loop() {
  mTime = millis(); 
  //запуск светодиодов при включении
  if (led_count < 12) {
    //выключение
    if (mTime - mTime_led > 200 && mTime - mTime_led <= 400) {
      digitalWrite(pin_L1, HIGH);
      digitalWrite(pin_L2, LOW);
      digitalWrite(pin_L3, LOW);
      digitalWrite(pin_L4, HIGH);
    }
    //включение
    else if (mTime - mTime_led <= 200) {
      digitalWrite(pin_L1, LOW);
      digitalWrite(pin_L2, HIGH);
      digitalWrite(pin_L3, HIGH);
      digitalWrite(pin_L4, LOW);
    }
    if (mTime - mTime_led > 400){
      mTime_led = mTime;
      led_count++;
    }
  }
  
  //запуск серво
  if (servo_flag) {
    if (mTime - mTime_servo_and_motor <= 800) {
      servo1.write(180);
      servo2.write(180);
    }
    else if (mTime - mTime_servo_and_motor > 800 && mTime - mTime_servo_and_motor <= 1600) {
      servo1.write(0);
      servo2.write(0);
    }
    if (mTime - mTime_servo_and_motor > 1600 && mTime - mTime_servo_and_motor <= 2400) {
      servo1.write(90);
      servo2.write(90);
    }
    if (mTime - mTime_servo_and_motor > 2400) servo_flag = false;
  }

  //запуск моторов
  if (motor_flag) {
    if (0 <= motor_pos_1 && motor_pos_1 < 100 && motor_start) {
      unsigned long now = micros();
      digitalWrite(pin_Motor_1_dir, HIGH);
      digitalWrite(pin_Motor_2_dir, HIGH);
      if (now - mTime_l >= 1000) {
        digitalWrite(pin_Motor_1_step, motor_step_1);
        digitalWrite(pin_Motor_2_step, motor_step_2);
        mTime_l = now;
        motor_step_1 = !motor_step_1;
        motor_step_2 = !motor_step_2;
        if (motor_step_1) motor_pos_1 += 0.125;
        if (motor_step_2) motor_pos_2 += 0.125;
      }
    }
    else if (-100 < motor_pos_1 && motor_pos_1 <= 100 && motor_middle) {
      motor_start = false;
      unsigned long now = micros();
      digitalWrite(pin_Motor_1_dir, LOW);
      digitalWrite(pin_Motor_2_dir, LOW);
      if (now - mTime_l >= 1000) {
        digitalWrite(pin_Motor_1_step, motor_step_1);
        digitalWrite(pin_Motor_2_step, motor_step_2);
        mTime_l = now;
        motor_step_1 = !motor_step_1;
        motor_step_2 = !motor_step_2;
        if (motor_step_1) motor_pos_1 -= 0.125;
        if (motor_step_2) motor_pos_2 -= 0.125;
      }
    }
    else if (-100 <= motor_pos_1 && motor_pos_1 < 0 && motor_finish) {
      motor_middle = false;
      unsigned long now = micros();
      digitalWrite(pin_Motor_1_dir, HIGH);
      digitalWrite(pin_Motor_2_dir, HIGH);
      if (now - mTime_l >= 1000) {
        digitalWrite(pin_Motor_1_step, motor_step_1);
        digitalWrite(pin_Motor_2_step, motor_step_2);
        mTime_l = now;
        motor_step_1 = !motor_step_1;
        motor_step_2 = !motor_step_2;
        if (motor_step_1) motor_pos_1 += 0.125;
        if (motor_step_2) motor_pos_2 += 0.125;
      }
    }
    else motor_flag = false;
  }

  //переключение режимов МОТОРОВ с помощью кнопки 1
  if (digitalRead(pin_sw_1) && mTime - mTime_servo_and_motor_mode > 100){
    if (servo_mode) {
      servo_mode = false;
      motor_mode = true;
      mode = 1;
      digitalWrite(pin_L1, LOW);
      digitalWrite(pin_L2, HIGH);
      digitalWrite(pin_L3, LOW);
      digitalWrite(pin_L4, HIGH);
      mTime_servo_and_motor_mode = mTime;
    }
    else {
      if (mode == 1) {
        mode++;
        digitalWrite(pin_L1, LOW);
        digitalWrite(pin_L2, LOW);
      }
      else if (mode == 2) {
        mode++;
        digitalWrite(pin_L1, HIGH);
        digitalWrite(pin_L2, HIGH);
      }
      else if (mode == 3) mode++;
      else if (mode == 4) {
        mode = 1;
        digitalWrite(pin_L1, LOW);
        digitalWrite(pin_L2, HIGH);
      }
      if (mTime - mTime_servo_and_motor_mode > 100) mTime_servo_and_motor_mode = mTime;
    }
  }
  if (motor_mode && mode == 4) {
    if (mTime - mTime_led > 200 && mTime - mTime_led <= 400) {
      digitalWrite(pin_L1, HIGH);
      digitalWrite(pin_L2, LOW);
    }
    else if (mTime - mTime_led <= 200) {
      digitalWrite(pin_L1, LOW);
      digitalWrite(pin_L2, HIGH);
    }
    if (mTime - mTime_led > 400) mTime_led = mTime;
  }

  //переключение режимов СЕРВО с помощью кнопки 2
  if (digitalRead(pin_sw_2) && mTime - mTime_servo_and_motor_mode > 100){
    if (motor_mode) {
      motor_mode = false;
      servo_mode = true;
      mode = 1;
      digitalWrite(pin_L1, HIGH);
      digitalWrite(pin_L2, LOW);
      digitalWrite(pin_L3, HIGH);
      digitalWrite(pin_L4, LOW);
      mTime_servo_and_motor_mode = mTime;
    }
    else {
      if (mode == 1) {
        mode++;
        digitalWrite(pin_L3, HIGH);
        digitalWrite(pin_L4, HIGH);
      }
      else if (mode == 2) {
        mode++;
        digitalWrite(pin_L3, LOW);
        digitalWrite(pin_L4, LOW);
      }
      else if (mode == 3) mode++;
      else if (mode == 4) {
        mode = 1;
        digitalWrite(pin_L3, HIGH);
        digitalWrite(pin_L4, LOW);
      }
      if (mTime - mTime_servo_and_motor_mode > 100) mTime_servo_and_motor_mode = mTime;
    }
  }
  if (servo_mode && mode == 4) {
    if (mTime - mTime_led > 200 && mTime - mTime_led <= 400) {
      digitalWrite(pin_L3, LOW);
      digitalWrite(pin_L4, HIGH);
    }
    else if (mTime - mTime_led <= 200) {
      digitalWrite(pin_L3, HIGH);
      digitalWrite(pin_L4, LOW);
    }
    if (mTime - mTime_led > 400) mTime_led = mTime;
  }

  //изменение положения моторов и серво с помощью кнопки 3
  if (digitalRead(pin_sw_3) && mTime - mTime_servo_and_motor_mode > 100 && sw3_flag){
    servo1.write(90);
    servo2.write(90);
    moveMotor1ToZero();
    moveMotor2ToZero();
    sw3_flag = false;

    if (mTime - mTime_servo_and_motor_mode > 100) mTime_servo_and_motor_mode = mTime;
  }

  //обработка энкодера
  if (digitalRead(pin_encoder_1) == 0 && mTime - mTime_encoder > 50 && not servo_flag && not motor_flag) {
    int range = 0;
    if (digitalRead(pin_encoder_2) == 1) range = 1;
    else if (digitalRead(pin_encoder_2) == 0) range = -1;
    if (servo_mode) change_servo(mode, range * 5);
    if (motor_mode) change_motor(mode, range);
    sw3_flag = true;
    mTime_encoder = mTime;
  }  
}