#define trig_pin 3
#define echo_pin 2
#define led_red 12
#define led_yellow 11
#define led_green 10

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(trig_pin, OUTPUT);
  pinMode(echo_pin, INPUT);
  pinMode(led_red, OUTPUT);
  pinMode(led_green, OUTPUT);
  pinMode(led_yellow, OUTPUT);
}

float controlUltrasonicSensor() {
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);
  int duration = pulseIn(echo_pin, HIGH);
  float distance_cm = duration / 58.5;
  float distance_inch = round(distance_cm) / 2.54;
  
  return distance_cm;
}

void printOutputUltrasonic(float distance) {
  int distan = round(distance);
  float distan2 = distance * 1.00;
  Serial.print("Distance in centimeters: ");
  Serial.println(distan);
  Serial.print("Distance in inches: ");
  Serial.println(distan / 2.54);
  Serial.println(distan2);
}

void controlLEDOutput(float check_unrounded, int threshold1, int threshold2, int threshold3) {
  int check = round(check_unrounded);
  digitalWrite(led_red, LOW);
  digitalWrite(led_yellow, LOW);
  digitalWrite(led_green, LOW);
  if (check >= threshold1) {
    digitalWrite(led_red, HIGH);
  } 
  if (check >= threshold2) {
    digitalWrite(led_yellow, HIGH);
  }
  if (check >= threshold3) {
    digitalWrite(led_green, HIGH);
  }
}

void loop() {
  // put your main code here, to run repeatedly:
  float output_cm = controlUltrasonicSensor();
  controlLEDOutput(output_cm, 5, 15, 30);
  printOutputUltrasonic(output_cm);  
  delay(100);
}