#include <Servo.h>

Servo myservo;
int posservo = 0;

#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);
  myservo.attach(6);
}

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 tt1, int tt2, int tt3) {
  int distan = round(distance);
  float distan2 = distance * 1.00;
  Serial.print("Threshold for red    : ");
  Serial.println(tt1);
  Serial.print("Threshold for yellow : ");
  Serial.println(tt2);
  Serial.print("Threshold for green  : ");
  Serial.println(tt3);
  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 controlServo(float cm_ss, int trh1, int trh2, int trh3) {
  float cm_cs = round(cm_ss) * 1.00;
  if (cm_cs < trh1) {
    myservo.write(0);
  } else if (cm_cs < trh2) {
    myservo.write(60);
  } else if (cm_cs < trh3) {
    myservo.write(120);
  } else {
    myservo.write(180);
  }
  
}

int t1 = 100; // original: 5
int t2 = 200; // original: 15
int t3 = 300; // original: 30

void loop() {
  // put your main code here, to run repeatedly:
  float output_cm = controlUltrasonicSensor();
  controlLEDOutput(output_cm, t1, t2, t3);
  printOutputUltrasonic(output_cm, t1, t2, t3);
  controlServo(output_cm, t1, t2, t3);  
  delay(200);
}








/*
#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.println("Threshold for red    : 5");
  Serial.println("Threshold for yellow : 15");
  Serial.println("Threshold for green  : 30");
  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);
  }
}
int t1 = 50;
int t2 = 150;
int t3 = 300;
void loop() {
  // put your main code here, to run repeatedly:
  float output_cm = controlUltrasonicSensor();
  controlLEDOutput(output_cm, t1, t2, t3);
  printOutputUltrasonic(output_cm);  
  delay(200);
}
*/