---

#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 = 0;

int Calib_G_red = 0;

int Calib_B_red = 0;

int Calib_R_green = 0;

int Calib_G_green = 0;

int Calib_B_green = 0;

int RED = 0;

int GREEN = 0;

int BLUE = 0;


void Init_MOTORS() {
  pinMode(2, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, INPUT);
  pinMode(8, INPUT);
  pinMode(11, INPUT);
}


void Init_LEDs() {
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);
  digitalWrite(A1, HIGH);
  delay(0.5 * 1000);
  digitalWrite(A1, LOW);
  digitalWrite(A2, HIGH);
  delay(0.5 * 1000);
  digitalWrite(A2, LOW);
  delay(0.5 * 1000);
}


void Init_TCS3200() {
  pinMode(A4, OUTPUT);
  pinMode(A3, OUTPUT);
  pinMode(A0, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(9, INPUT);
  digitalWrite(A4, LOW);
  digitalWrite(A3, HIGH);
  digitalWrite(A1, HIGH);
  delay(Calibrate_time * 1000);
  Mesure_RED();
  Calib_R_green=RED;
  Mesure_GREEN();
  Calib_G_green=GREEN;
  Mesure_BLUE();
  Calib_B_green=BLUE;
  digitalWrite(A1, LOW);
  digitalWrite(A2, HIGH);
  delay(Calibrate_time * 1000);
  Mesure_RED();
  Calib_R_red=RED;
  Mesure_GREEN();
  Calib_G_red=GREEN;
  Mesure_BLUE();
  Calib_B_red=BLUE;
  digitalWrite(A2, LOW);
}


void Robot_TRACKING() {
  if (!digitalRead(8) && digitalRead(7)) {
    digitalWrite(2, LOW);
    digitalWrite(4, LOW);
    analogWrite(5, max_speed - 20);
    analogWrite(6, max_speed - 20);
  }
  if (digitalRead(8) && digitalRead(7)) {
    digitalWrite(2, LOW);
    digitalWrite(4, HIGH);
    analogWrite(5, max_speed);
    analogWrite(6, 255 - max_speed);
  }
  if (digitalRead(8) && !digitalRead(7)) {
    digitalWrite(2, LOW);
    digitalWrite(4, HIGH);
    analogWrite(5, max_speed);
    analogWrite(6, 255 - max_speed);
  }
  if (!digitalRead(8) && !digitalRead(7)) {
    digitalWrite(2, HIGH);
    digitalWrite(4, LOW);
    analogWrite(5, 255 - max_speed);
    analogWrite(6, max_speed);
  }
}


void Blink_BLUE() {
  for (int index = 0; index < 5; index++) {
    digitalWrite(A1, HIGH);
    delay(0.2 * 1000);
    digitalWrite(A1, LOW);
    delay(0.2 * 1000);
  }
}


void Blink_RED() {
  for (int index = 0; index < 5; index++) {
    digitalWrite(A2, HIGH);
    delay(0.2 * 1000);
    digitalWrite(A2, LOW);
    delay(0.2 * 1000);
  }
}


void Mesure_RED() {
  digitalWrite(A0, LOW);
  digitalWrite(10, LOW);
  RED=pulseIn(9, LOW);
}


void Robot_STOP() {
  digitalWrite(2, LOW);
  digitalWrite(4, LOW);
  analogWrite(5, 0);
  analogWrite(6, 0);
}


void Mesure_GREEN() {
  digitalWrite(A0, HIGH);
  digitalWrite(10, HIGH);
  GREEN=pulseIn(9, LOW);
}


void Mesure_BLUE() {
  digitalWrite(A0, LOW);
  digitalWrite(10, HIGH);
  BLUE=pulseIn(9, LOW);
}


void setup() {
  Init_MOTORS();
  Init_LEDs();
  Init_TCS3200();
}

void loop() {
  Mesure_RED();
  Mesure_GREEN();
  Mesure_BLUE();
  if (millis() - last_time > lag_time *1000) {
    if ((Calib_R_red - RED < red_sens && Calib_R_red - RED > -red_sens) && ((Calib_G_red - GREEN < red_sens && Calib_G_red - GREEN > -red_sens) && (Calib_B_red - BLUE < red_sens && Calib_B_red - BLUE > -red_sens))) {
      Robot_STOP();
      Blink_RED();
      last_time=millis();
    }
    else{
      if ((Calib_R_green - RED < green_sens && Calib_R_green - RED > -green_sens) && ((Calib_G_green - GREEN < green_sens && Calib_G_green - GREEN > -green_sens) && (Calib_B_green - BLUE < green_sens && Calib_B_green - BLUE > -green_sens))) {
        Robot_STOP();
        Blink_BLUE();
        last_time=millis();
      }
      else{
        Robot_TRACKING();
      }
    }
  }
  else{
    Robot_TRACKING();
  }
}