/* Sweep
 by Dhaval Sanepara
 This example code is in the public domain.

 Created on 6 Nov 2022
 by Dhaval Sanepara
*/

// This will include the servo related code tha code is in c 
#include <Servo.h>

// This 2 defaine is for ultra-sonic 1 that is use to detect the zebra crossing
#define ECHO_PIN_US_1 4
#define TRIG_PIN_US_1 5
// This 2 defaines is for ultra-sonic 2 that is used for car detection
#define ECHO_PIN_US_2 2
#define TRIG_PIN_US_2 3

// This 4 defaine is for 4 LEDs that LEDs used when car is ordered to stop;
// And Zebra crossing is started
#define LED_1 10
#define LED_2 11
#define LED_3 12
#define LED_4 13


Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;    // variable to store the servo position
bool car_is_already_there = false;
float sensor_max_value = 20.00;
int Time_to_reset_servo = 0;

void setup() {
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  Serial.begin(115200);
  //Setting LEDs as output of arduino
  pinMode(LED_1, OUTPUT);
  pinMode(LED_2, OUTPUT);
  pinMode(LED_3, OUTPUT);
  pinMode(LED_4, OUTPUT);
  //setting ultra-sonic pin modes
  pinMode(TRIG_PIN_US_1, OUTPUT);
  pinMode(ECHO_PIN_US_1, INPUT);
  pinMode(TRIG_PIN_US_2, OUTPUT);
  pinMode(ECHO_PIN_US_2, INPUT);
}

float read_Distance_on_Zebra() {
  digitalWrite(TRIG_PIN_US_1, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_US_1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_US_1, LOW);
  int duration = pulseIn(ECHO_PIN_US_1, HIGH);
  return duration * 0.034 / 2;
}

float read_Distance_of_Car() {
  digitalWrite(TRIG_PIN_US_2, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_US_2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_US_2, LOW);
  int duration = pulseIn(ECHO_PIN_US_2, HIGH);
  return duration * 0.034 / 2;
}

void loop() {
  //Serial.print("Measured distance on Zebra: ");
  //Serial.println(read_Distance_on_Zebra());
  //Serial.print("Measured distance on Car: ");
  //Serial.println(read_Distance_of_Car());
  //delay(1000);
  float distance_on_Zebra = read_Distance_on_Zebra();
  float distance_of_Car = read_Distance_of_Car();

  bool Car_is_Nearby = distance_of_Car < 150;
  
  if(distance_on_Zebra >= (sensor_max_value + 5)) {
    digitalWrite(LED_1, LOW);
    digitalWrite(LED_2, LOW);
    digitalWrite(LED_3, LOW);
    digitalWrite(LED_4, LOW);
  }
  if(distance_on_Zebra < sensor_max_value ) {
    digitalWrite(LED_1, HIGH);
  }
  if(distance_on_Zebra < (sensor_max_value - 4.00)) {
    digitalWrite(LED_2, HIGH);
  }
  if(distance_on_Zebra < (sensor_max_value - 8.00)) {
    digitalWrite(LED_3, HIGH);
  }
  if(distance_on_Zebra < (sensor_max_value - 16.00)) {
    digitalWrite(LED_4, HIGH);
  }
  
  if ((distance_of_Car >= sensor_max_value)) {
    myservo.write(0);
    //Time_to_reset_servo = 0;
  }
  else if (distance_of_Car > (sensor_max_value - 8.00) ) {
    myservo.write(90);
  }
  else {
    myservo.write(0);
    //Time_to_reset_servo = 0;
  }
  delay(500);
  //Time_to_reset_servo = Time_to_reset_servo + 1;
}