#define GoTime 1000  //время движения первого и второго этапа в миллисекундах

const int Key = A0;  // Пин для кнопки управления этапами

const int L_Dir = 4;  // 4 и 7 пины управления направлением вращения мотора (1 и 0)
const int R_Dir = 7;
const int L_Speed = 5;  // 5 и 6 пины ШИМ управление скоростью вращения моторов (от 0 до 255, 0 - стоп)
const int R_Speed = 6;

int left_speed = 255;  // в этих переменных скорость левого и правого моторов. для всех процедур
int right_speed = 255;
int dlinnapov = 0;

int r_enk = 2;  // 2 и 3 пины используем для обработки сигнала от энкодера
int l_enk = 3;
int coltick = 0;

volatile int c_l_enk = 0;  // в этих переменных будем хранить кол-во тиков левого и правого энкодера
volatile int c_r_enk = 0;
float deltaspeed = 0;  // дельта скорости левого и правого двигателя

// датчик линии
int line1 = A3;
int line2 = A4;

int etap = 0;        // номер этапа
bool p_key = false;  // вспомогательная переменная изменения состояния кнопки

int Dist() {  // функция возвращает растояние до объекта в сантиметрах
  pinMode(8, INPUT);
  pinMode(12, OUTPUT);
  digitalWrite(12, 0);
  delayMicroseconds(3);
  digitalWrite(12, 1);
  delayMicroseconds(10);
  digitalWrite(12, 0);
  int d = pulseIn(8, 1);
  return d / 58;
}

// процедуры работающие по прерыванию увеличивает счетчик на единицу когда приходит сигнал о изменении состояния энкодера
void R_enk() {
  c_r_enk++;
}
void L_enk() {
  c_l_enk++;
}

void setup() {
  Serial.begin(9600);
  pinMode(Key, INPUT_PULLUP);
  // подключаем пины управления моторами на запись
  pinMode(L_Dir, OUTPUT);
  pinMode(R_Dir, OUTPUT);
  pinMode(L_Speed, OUTPUT);
  pinMode(R_Speed, OUTPUT);
  // подключаем пины энкодера на чтение
  pinMode(r_enk, INPUT);
  pinMode(l_enk, INPUT);
  // подключение режима работы по прерыванию процедур
  attachInterrupt(digitalPinToInterrupt(r_enk), R_enk, CHANGE);
  attachInterrupt(digitalPinToInterrupt(l_enk), L_enk, CHANGE);
  // подключаем датчики линии
  pinMode(line1, INPUT);
  pinMode(line2, INPUT);
}

void Go(int L_S, int R_S) {  // процедура управление работой моторов (положительная скорость - вперёд, отрицательная - назад)
  bool L_D = 1;
  bool R_D = 1;  // переменные направления вращения моторов ставим в 1, т.е. вперёд
  if (L_S < 0) {
    L_D = 0;
    L_S = -L_S;
  }  // если скорость имеет отрицательное значение
  // то меняем переменную направления вращения на 0, т.е. назад и меняем у скорости знак
  if (R_S < 0) {
    R_D = 0;
    R_S = -R_S;
  }
  digitalWrite(L_Dir, L_D);
  digitalWrite(R_Dir, R_D);  // устанавливаем направление
  analogWrite(L_Speed, L_S);
  analogWrite(R_Speed, R_S);  // устанавливаем скорость
}

void Go_Time(int Time) {  // процедура движения по времени, Time - заданое время движения, turn - направление движения
  c_l_enk = 0;            // cбрасываем счетчики левого и правого энкодеров
  c_r_enk = 0;

  unsigned long start_time = millis();    // запомним время вхождения в цикл
  while (millis() - start_time < Time) {  // пока текущее время - время старта меньше заданого времени в переменной Time выполняем Go!


    if (c_l_enk > c_r_enk) {
      Go(20, right_speed);
      Serial.println("RIGHT");
    }


    if (c_l_enk < c_r_enk) {
      Go(left_speed, 20);
      Serial.println("LEFT");
    }
    Go(left_speed, right_speed);
    Serial.println((String)c_l_enk + "-" + c_r_enk);
  }
  // deltaspeed=c_l_enk/c_r_enk;
  //if (deltaspeed != 1)  {
  //    right_speed = right_speed*deltaspeed;
  // }
  Go(0, 0);  // установим скорость 0
}

void Go_TimeNAZAD(int Time) {  // процедура движения по времени, Time - заданое время движения, turn - направление движения
  c_l_enk = 0;                 // cбрасываем счетчики левого и правого энкодеров
  c_r_enk = 0;

  unsigned long start_time = millis();    // запомним время вхождения в цикл
  while (millis() - start_time < Time) {  // пока текущее время - время старта меньше заданого времени в переменной Time выполняем Go!

    if (c_l_enk > c_r_enk) {
      Go(20, -right_speed);
      delay(5);
    }

    if (c_l_enk < c_r_enk) {
      Go(-left_speed, 20);
      delay(5);
    }

    Go(-left_speed, -right_speed);
    Serial.println((String)c_l_enk + "-" + c_r_enk);
  }
  deltaspeed = c_l_enk / c_r_enk;
  if (deltaspeed != 1) {
    right_speed = right_speed * deltaspeed;
  }
  Go(0, 0);  // установим скорость 0
}

void stop() {
  bool L_D1 = digitalRead(L_Dir);
  bool R_D1 = digitalRead(R_Dir);
  digitalWrite(L_Dir, !L_D1);
  digitalWrite(R_Dir, !R_D1);  // устанавливаем направление
  analogWrite(L_Speed, left_speed);
  analogWrite(R_Speed, right_speed);  // устанавливаем скорость
  delay(20);
  Go(0, 0);
}

void len(int dlinna) {
  c_l_enk = 0;
  c_r_enk = 0;
  coltick = dlinna / 21 * 40;
  while (c_l_enk < coltick) {
    if (c_l_enk > c_r_enk) {
      Go(20, right_speed);
      Serial.println("RIGHT");
    }

    if (c_l_enk < c_r_enk) {
      Go(left_speed, 20);
      Serial.println("LEFT");
    }

    Go(left_speed, right_speed);
  }
  stop();
}

void lenNAZAD(int dlinna) {
  c_l_enk = 0;
  c_r_enk = 0;
  coltick = dlinna / (6.5 * PI) * 40;
  while (c_l_enk < coltick) {
    if (c_l_enk > c_r_enk) {
      Go(20, -right_speed);
      delay(5);
    }

    if (c_l_enk < c_r_enk) {
      Go(-left_speed, 20);
      delay(5);
    }

    Go(-left_speed, -right_speed);
  }
  stop();
}

void TpovR(int gradus) {  // Танковый поворот вправо на х градусов
  c_l_enk = 0;
  c_r_enk = 0;
  dlinnapov = (14 * PI) / 360 * gradus;
  coltick = dlinnapov / (6.7 * PI) * 40;
  while (c_l_enk < coltick) {
    Go(left_speed, -right_speed);
  }
  stop();
}

void TpovL(int gradus) {  // Танковый поворот влево на х градусов
  c_l_enk = 0;
  c_r_enk = 0;
  dlinnapov = (13 * PI) / 360 * gradus;
  coltick = dlinnapov / (6.5 * PI) * 40;
  while (c_r_enk < coltick) {
    Go(-left_speed, right_speed);
  }
  stop();
}

void PovR(int gradus) {  // Танковый поворот вправо на х градусов
  c_l_enk = 0;
  c_r_enk = 0;
  dlinnapov = (26 * PI) / 360 * gradus;
  coltick = dlinnapov / (6.5 * PI) * 40;
  while (c_l_enk < coltick) {
    Go(left_speed, 0);
  }
  stop();
}

void PovL(int gradus) {  // Поворот на одном колесе влево на х градусов
  c_l_enk = 0;
  c_r_enk = 0;
  dlinnapov = (26 * PI) / 360 * gradus;
  coltick = dlinnapov / (6.5 * PI) * 40;
  while (c_r_enk < coltick) {
    Go(0, right_speed);
  }
  stop();
}

void DoLinii() {  //Едем до черной линии
  int Rline1 = digitalRead(line1);
  int Rline2 = digitalRead(line2);
  Serial.println((String)Rline1 + " " + Rline2);
  while (Rline1 != 1 && Rline2 != 1) {
    Go(left_speed, right_speed);
    Serial.println((String)Rline1 + " " + Rline2);
  }
  stop();
}


void DoSteni(int dist) {  // остановка на растоянии x cм от препятствия
  while (Dist() > dist) {
    Go(left_speed, right_speed);
  }
  stop();
}


void loop() {
  bool key = !digitalRead(Key);
  delay(20);
  // устанавливаем скорость моторов
  left_speed = 90;
  right_speed = 108;

  if (key && !p_key) { p_key = true; }  // нажали кнопку
  if (!key && p_key) {                  // отпустили кнопку
    p_key = false;
    etap = etap + 1;  // считаем этапы (отпускание кнопки)
    if (etap > 10) { etap = 0; }
    Serial.println(etap);
    switch (etap) {  // переключатель по значению переменной etap

      case 0:  // если etap = 0, то ничего не делаем
        break;

      case 1:  // eсли 1, то запускаем процедуру Go_time с аргументом направления вперёд
        len(40);

        break;

      case 2:  // eсли 2, то запускаем процедуру Go_time с аргументом направления назад
        TpovL(90);

        break;

      case 3:  // вперед на х сантиметров
        lenNAZAD(40);
        break;

      case 4:  // назад на х сантиметров
        TpovR(180);
        break;

      case 5:  // танковый поворот НАПРАВО на х градусов
        len(60);
        break;

      case 6:  // танковый поворот НАЛЕВО на х градусов
        lenNAZAD(30);
        break;

      case 7:  // поворот НАПРАВО на х градусов
        TpovR(90);
        break;

      case 8:  // поворот НАЛЕВО на х градусов
        len(30);
        break;

      case 9:  // остановка в х сантиметрах от стены
        PovL(90);
        break;

      case 10:  // движение вперед до черной линии и остановка на ней
        len(40);
        break;
    }
  }
}