const int trigPin1 = 14;
const int echoPin1 = 12;
const int trigPin2 = 13;
const int echoPin2 = 15;
const int Error_LED = 8;
const int Check_Led = 2;

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

int Time_1 = 5000;
int Time_2;
int Sen1_Status = 0;
int Sen2_Status = 0;
int Error_state = 0;
int Delay;
int Check_Led_Status = 0;
int D = 100;

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(Check_Led , OUTPUT);
  pinMode(Analog, INPUT);
  Serial.begin(9600);
}

void loop()
{
  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(Check_Led, Check_Led_Status);
  }
    
    
 
  
  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 >= 100 && distanceCm1 <= 200)
    Sen1_Status = 1;
  else
    Sen1_Status = 0;

  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distanceCm2 = duration2 * 0.034 / 2;
  if (distanceCm2 >= 100 && distanceCm2 <= 200)
    Sen2_Status = 1;
  else
    Sen2_Status = 0;

  if (Sen1_Status == 1)
  {
    if (Sen1_detect_time == 0)
    {
      Sen1_detect_time = millis();
    }
    if (millis() - Sen1_detect_time > Time_1)
    {
      Error_state = 1;
    }
  } else if (Sen2_Status == 1)
  {
    if (Sen2_detect_time == 0)
    {
      Sen2_detect_time = millis();
      Error_state = 0;
    }
    if (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, 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);
  }
  if (Sen2_Status == 1 && !Sen2_detect)
  {
    digitalWrite(Error_LED, LOW);
    Sen1_detect = false;
    Sen2_detect = false;
  }
    
  //Serial.println(Time_2);
}

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

  if (Delay >= 100 && Delay <= 200)
    Time_2 = 1000;

  if (Delay >= 200 && Delay <= 300)
    Time_2 = 2000;

  if (Delay >= 300 && Delay <= 400)
    Time_2 = 3000;

  if (Delay >= 400 && Delay <= 500)
    Time_2 = 4000;

  if (Delay >= 500 && Delay <= 600)
    Time_2 = 5000;

  if (Delay >= 600 && Delay <= 700)
    Time_2 = 6000;

  if (Delay >= 700 && Delay <= 800)
    Time_2 = 7000;

  if (Delay >= 800 && Delay <= 900)
    Time_2 = 8000;

  if (Delay >= 900 && Delay <= 1024)
    Time_2 = 9000;

}