const int trigPin1 = 13;// D5;
const int echoPin1 = 12;// D6;
const int trigPin2 = 11;// D7;
const int echoPin2 = 10;// D8;
const int Error_LED = 9;// D4;
const int Code_Run_Led = 8;// D3;
const int Sen_1_LED = 7; // D1
const int Sen_2_LED = 6; // D2

const int Analog = A0;
long duration1, duration2, Error;
int distanceCm1, distanceCm2;

int Time_1 = 5000;
int Time_2 = 7000;
int Sen1_Status = 0;
int Sen2_Status = 0;
int Error_state = 0;
int Delay;
int Check_Led_Status = 0;
//int D = 100;
int Min_Dist = 100;// 0;
int Max_Dist = 200;// 10;

unsigned long Sen1_detect_time = 0;
unsigned long Sen2_detect_time = 0;
unsigned long sen1 = 0;

bool Sen1_detect = false;
bool Sen2_detect = false;

unsigned long Last_millis1 = 0;
long Current_millis1;

void setup() {
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(Error_LED, OUTPUT);
  pinMode(Code_Run_Led, OUTPUT);
  pinMode(Analog, INPUT);
  pinMode(Sen_1_LED, OUTPUT);
  pinMode(Sen_2_LED, OUTPUT);
  Serial.begin(9600);
  Sen1_Status = 0;
  Sen2_Status = 0;
  //delay(5000);
  
}

void loop()
{

  Check_Run();
    
  //Time();
  
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  distanceCm1 = duration1 * 0.034 / 2;
  if (distanceCm1 >= Min_Dist && distanceCm1 <= Max_Dist)
  {
    Sen1_Status = 1;
    digitalWrite(Sen_1_LED, LOW);
  }
  else
  {
    Sen1_Status = 0;
    digitalWrite(Sen_1_LED, HIGH);
  }

  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distanceCm2 = duration2 * 0.034 / 2;
  if (distanceCm2 >= Min_Dist && distanceCm2 <= Max_Dist)
  {
    Sen2_Status = 1;
    digitalWrite(Sen_2_LED, LOW);
  }
  else
  {
    Sen2_Status = 0;
    digitalWrite(Sen_2_LED, HIGH);
  }

  if (Sen1_Status == 1 || Sen2_Status == 1)
  {
    if (Sen1_detect_time == 0 || Sen2_detect_time == 0)
    {
      Sen1_detect_time = millis();
      Sen2_detect_time = millis();
    }
    if (millis() - Sen1_detect_time > Time_1 || millis() - Sen2_detect_time > Time_1)
    {
      Error_state = 1;
    }
  }  
  else
  {
    Sen1_detect_time = 0;
    Sen2_detect_time = 0;
    Error_state = 0;
    Sen1_detect == false;
    Sen2_detect == false;
  }
  if(Error_state)
  {
    // digitalWrite(Error_LED, Error_state);
    digitalWrite(Error_LED, HIGH);
    delay(100);
    digitalWrite(Error_LED, LOW);
    delay(100);
  }

  digitalWrite(Error_LED, Error_state);

  if (Sen1_Status == 1 && !Sen1_detect)
  {
    Sen1_detect = true;
    Sen2_detect = false;
    sen1 = millis();
  }
  if (Sen2_Status == 1 && Sen1_detect && !Sen2_detect)
  {
    if (millis() - sen1 < Time_2)
    {
      digitalWrite(Error_LED, LOW);
      Sen1_detect = false;
      Sen2_detect = true;
    }
  }
  if (millis() - sen1 > Time_2 && Sen1_detect && !Sen2_detect)
  {    
    digitalWrite(Error_LED, HIGH);
    delay(100);
    digitalWrite(Error_LED, LOW);
    delay(100);
  }
  if (Sen2_Status == 1 && !Sen2_detect)
  {
    digitalWrite(Error_LED, LOW);
    Sen1_detect = false;
    Sen2_detect = false;
  }
  
  //  Serial.print(Delay);
  //  Serial.print(distanceCm1);
  //  Serial.print("       ");
  //  Serial.println(distanceCm2);
}

void Time()
{
  Delay = analogRead(Analog);

  if (Delay >= 270 && Delay <= 310)
    Time_2 = 1000;

  if (Delay >= 432 && Delay <= 472)
    Time_2 = 2000;

  if (Delay >= 539 && Delay <= 579)
    Time_2 = 3000;

  if (Delay >= 626 && Delay <= 656)
    Time_2 = 4000;

  if (Delay >= 672 && Delay <= 712)
    Time_2 = 5000;

  if (Delay >= 730 && Delay <= 750)
    Time_2 = 6000;

  if (Delay >= 765 && Delay <= 785)
    Time_2 = 7000;

  if (Delay >= 790 && Delay <= 815)
    Time_2 = 8000;

  if (Delay >= 815 && Delay <= 845)
    Time_2 = 9000;

  if (Delay >= 1000)
    Time_2 = 0;

}

void Check_Run()
{
  Current_millis1 = millis();  

  if(Current_millis1 - Last_millis1 > 200)
  {
    Last_millis1 = Current_millis1;
    if(Check_Led_Status == 1)
    {
      Check_Led_Status = 0;
    }
    else
    {
      Check_Led_Status = 1;
    }
    digitalWrite(Code_Run_Led, Check_Led_Status);
  }
}