//assuming the not gate is the speeding object

#include <Servo.h>
#define ECHO_PIN 2
#define TRIG_PIN 3
int green=13,yellow=12,red=11;
Servo myServo;//servo object
int servoPin = 4;

void setup() {
  Serial.begin(115200);
 // pinMode(13, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(green, OUTPUT);pinMode(yellow, OUTPUT);pinMode(red, OUTPUT);
  pinMode(4, OUTPUT);
  myServo.attach(servoPin);
  myServo.write(90);//intial direction of servo

}

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void loop() {

 float distance1 = readDistanceCM();
  delay(50); // wait for a bit to get the next measurement
  float distance2 = readDistanceCM();
  float speedCMPS = abs((distance1 - distance2)/0.05 ); // two distances over the time between them
bool spdonecheck=false,spdtwocheck=false,spdthreecheck=false;
if(speedCMPS<=250){spdonecheck=!spdonecheck;}
  else if( speedCMPS >=250 &&speedCMPS <2000 ){spdtwocheck=!spdtwocheck;}
  else if(speedCMPS>=2000) {spdthreecheck=!spdthreecheck;}
  //if(speedCMPS>=1000)
Serial.print("  Speed:");//in cm/s
  Serial.println(speedCMPS);
Serial.print("GATE:");
if(spdonecheck){Serial.print("OPEN");}
if(spdtwocheck){Serial.print("CLOSING");}
if(spdthreecheck){Serial.print("CLOSED");}
 
  digitalWrite(green, spdonecheck);
  if(spdonecheck){myServo.write(90);}//open gate resemblance
  digitalWrite(yellow, spdtwocheck);
  if(spdtwocheck){myServo.write(30);}//half gate resemblance
  digitalWrite(red, spdthreecheck);
  if(spdthreecheck){myServo.write(0);}//closed gate resemblance
}
//

//

  
  

  // analogWrite(4, 50);
  // delay(1000);

  // // Move the servo to 60 degrees
  // analogWrite(4, 100);
  // delay(1000);

  // // Move the servo to 90 degrees
  // analogWrite(4, 150);
  ////////////
  // Serial.print("Measured distance1: ");
  // Serial.print(distance1);
  // Serial.print("  Measured distance2: ");
  // Serial.print(distance2);
  
  ////////
  
  

  
    
//geen led on
//servo stays same
//digitalWrite(green, HIGH);
//{spdonecheck=!spdonecheck;}
  
   
  
//yellow led on
//servo closes 30 degrees
// digitalWrite(yellow, HIGH);


  
 
  //dangourus speed
  
//red led on 
//servo closes 60 degrees
// rewrite with boolean digitalWrite(red, HIGH);


  
//  Serial.print("SAFE check:");
//   Serial.print(spdonecheck);
//   Serial.print(" SEMI-SAFE check:");
//   Serial.print(spdtwocheck);
//   Serial.print(" NOT-SAFE check:");
//   Serial.println(spdthreecheck);
  



 


  ////////
  // float distance = readDistanceCM();

  // bool isNearby = distance < 100;
  // digitalWrite(LED_BUILTIN, isNearby);

  // Serial.print("Measured distance: ");
  // Serial.println(readDistanceCM());

  // delay(100);
  ////////
   //if(spee)
  //Serial.print("safe speed: " + (spdonecheck ? "T" : "F") + " moderate speed " + (spdtwocheck ? "T" : "F") + " dangerous speed " + (spdthreecheck ? "T" : "F"));