//Defining pins
const int trig = 7;
const int echo = 6;
int redLED = 8;
int blueLED = 9;
int greenLED = 10;
int yellowLED = 11;
int testcount = 0;

//Declare 2 floats, duration and distance, which will hold the 
//length of the sound wave and how far away the object is 
float duration, distance;


void setup() {
  // put your setup code here, to run once:

Serial.begin(9600);

// Setup inputs and outputs
pinMode(echo, INPUT); //output of HC-SR04

pinMode(trig, OUTPUT); //input of HC-SR04
pinMode(redLED, OUTPUT);
pinMode(blueLED, OUTPUT);
pinMode(greenLED, OUTPUT);
pinMode(yellowLED, OUTPUT);

}

void loop() {
  // put your main code here, to run repeatedly:

//Begin with trigger set low
digitalWrite(trig, LOW);
delayMicroseconds(2);


// From HC-SR04 DataSheet: You only need to supply a short 10uS 
// pulse to the trigger input to start the ranging
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);

// When the sound waves hit a objectm it sets the echo pin high for however long the waves are traveling for
// Using pulseIN = waits for the specified pin to go into the state set, then begins timing,
// and then stops time once state is changed. We want to know when echo pin is high 
duration = pulseIn(echo, HIGH);

// After time is calcualted (pulseIN function), distance must be calculated. Therefore:
// time x speed = distance. Speed is known: Speed of sound = 340 m/s = 0.0343 cm/us (pulseIN requires uS)
// Lastly distance must be divided by 2 bc the sound waves will be traveling to the object and back


// calculate the distance
distance = duration * 0.0343 / 2;

//Results to serial monitor
Serial.print("Distance: ");
Serial.print(distance);
Serial.println("cm");


// Programming LEDs
// Note: Pink LED is to confirm Trigger

if (distance >= 4 && distance < 8) {
digitalWrite(redLED, HIGH);
digitalWrite(greenLED, LOW);
digitalWrite(blueLED, LOW);
digitalWrite(yellowLED, LOW);  
}
else if (distance >= 8 && distance < 12) {
digitalWrite(redLED, LOW);
digitalWrite(greenLED, HIGH);
digitalWrite(blueLED, LOW);
digitalWrite(yellowLED, LOW);  
}
else if (distance >= 12 && distance < 16) {
digitalWrite(redLED, LOW);
digitalWrite(greenLED, LOW);
digitalWrite(blueLED, HIGH);
digitalWrite(yellowLED, LOW);  
}
else if (distance >= 16 && distance < 20) {
digitalWrite(redLED, LOW);
digitalWrite(greenLED, LOW);
digitalWrite(blueLED, LOW);
digitalWrite(yellowLED, HIGH);  
}

//When distance is out of range, all LEDs should be off
else {
digitalWrite(redLED, LOW);
digitalWrite(greenLED, LOW);
digitalWrite(blueLED, LOW);
digitalWrite(yellowLED, LOW);  
}



delay(100);
}