---

#include <Arduino.h>

int max_speed = 135;
int red_sens = 70;
int green_sens = 50;
int Calibrate_time = 3;
int lag_time = 1;

unsigned long last_time = 0;

int Calib_R_red, Calib_G_red, Calib_B_red;
int Calib_R_green, Calib_G_green, Calib_B_green;
int RED, GREEN, BLUE;

void Init_MOTORS() {
  pinMode(2, OUTPUT); // Направление мотора А
  pinMode(4, OUTPUT); // Направление мотора B
  pinMode(5, OUTPUT); // ШИМ мотора А (Скорость)
  pinMode(6, OUTPUT); // ШИМ мотора B (Скорость)
  pinMode(7, INPUT);  // Датчик линии левый
  pinMode(8, INPUT);  // Датчик линии правый
}

void Measure_Color() {
  digitalWrite(A0, LOW); digitalWrite(10, LOW);
  RED = pulseIn(9, LOW, 20000); 
  digitalWrite(A0, HIGH); digitalWrite(10, HIGH);
  GREEN = pulseIn(9, LOW, 20000);
  digitalWrite(A0, LOW); digitalWrite(10, HIGH);
  BLUE = pulseIn(9, LOW, 20000);
}

// --- Логика движения по линии ---
void Robot_TRACKING() {
  bool left = digitalRead(7);
  bool right = digitalRead(8);

  if (!left && !right) { 
    digitalWrite(2, LOW);
    digitalWrite(4, LOW);
    analogWrite(5, max_speed);
    analogWrite(6, max_speed);
  } 
  else if (left && !right) { 
    digitalWrite(2, HIGH); 
    digitalWrite(4, LOW);
    analogWrite(5, max_speed);
    analogWrite(6, max_speed);
  } 
  else if (!left && right) { 
    digitalWrite(2, LOW);
    digitalWrite(4, HIGH); 
    analogWrite(5, max_speed);
    analogWrite(6, max_speed);
  } 
  else { 
    analogWrite(5, 0);
    analogWrite(6, 0);
  }
}

void Robot_STOP() {
  analogWrite(5, 0);
  analogWrite(6, 0);
}

void Blink_LED(int pin) {
  for (int i = 0; i < 5; i++) {
    digitalWrite(pin, HIGH);
    delay(200);
    digitalWrite(pin, LOW);
    delay(200);
  }
}

bool isColorMatch(int cR, int cG, int cB, int tolerance) {
  return (abs(RED - cR) < tolerance && 
          abs(GREEN - cG) < tolerance && 
          abs(BLUE - cB) < tolerance);
}

void setup() {
  Init_MOTORS();
  pinMode(A1, OUTPUT); 
  pinMode(A2, OUTPUT);
  
  pinMode(A4, OUTPUT); // S3
  pinMode(A3, OUTPUT); // S2
  pinMode(A0, OUTPUT); // S0
  pinMode(10, OUTPUT); // S1
  pinMode(9, INPUT);   // OUT
  
  // Делитель частоты 20% (S0=H, S1=L)
  digitalWrite(A0, HIGH);
  digitalWrite(10, LOW);

  // Калибровка (Зеленый)
  digitalWrite(A1, HIGH); 
  delay(Calibrate_time * 1000);
  Measure_Color();
  Calib_R_green = RED; Calib_G_green = GREEN; Calib_B_green = BLUE;
  digitalWrite(A1, LOW);

  delay(1000);

  digitalWrite(A2, HIGH);
  delay(Calibrate_time * 1000);
  Measure_Color();
  Calib_R_red = RED; Calib_G_red = GREEN; Calib_B_red = BLUE;
  digitalWrite(A2, LOW);
}

void loop() {
  Measure_Color();

  if (isColorMatch(Calib_R_red, Calib_G_red, Calib_B_red, red_sens)) {
    if (millis() - last_time > (unsigned long)lag_time * 1000) {
      Robot_STOP();
      Blink_LED(A2); // Мигаем красным
      last_time = millis();
    }
  } 
  else if (isColorMatch(Calib_R_green, Calib_G_green, Calib_B_green, green_sens)) {
    if (millis() - last_time > (unsigned long)lag_time * 1000) {
      Robot_STOP();
      Blink_LED(A1); // Мигаем синим 
      last_time = millis();
    }
  } 
  // Иначе просто