#include <Servo.h>
#include <LiquidCrystal_I2C.h>

/*
 * Title: My Awesome Arduino Program
 * Author: Liz Miller
 * Date: 02/15/2018
 * Version: v1.0
 * Purpose: This code shows you how to write an Arduino Program!
 */

// Step 2 - Define all of your IO
const int ledPin = 6;
const int pingPin = 5;

Servo myServo;
LiquidCrystal_I2C lcd(0x27, 20, 4); // Set the LCD address to 0x27 for a 20 chars and 4 lines display

void setup() {
  // Put your setup code here, to run once:
  myServo.attach(3); // Servo on pin 3
  pinMode(ledPin, OUTPUT);
  pinMode(pingPin, INPUT);
  Serial.begin(9600); // Initialize the serial monitor
  myServo.write(0); // Initialize servo to position 0
  lcd.init(); // Initialize the LCD
  lcd.backlight(); // Turn on backlight
  lcd.clear(); // Clear the LCD
}

// Turns the given LED on
void ledOn(const int ledPin) {
  digitalWrite(ledPin, HIGH);
}

// Moves the servo 180 degrees
void moveServo() {
  myServo.write(180); // Move servo to opposite side (180-deg)
  delay(50); // Delay to give servo enough time to reach the position
}

/*
 * Detect distance from ping sensor
 * Set pingPin to OUTPUT and pulse for 10 microseconds
 * Send LOW, HIGH, LOW signal
 * Set pingPin as INPUT and read the signal
 */
int detectDistance() {
  long duration, inches;

  // Send out a signal
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(pingPin, LOW);

  // Read the signal as a pulse
  duration = pulseIn(pingPin, HIGH);

  // Calculate distance in inches
  inches = duration / 74 / 2; // 74 microseconds per inch (divide by 2 for round trip)

  return inches; // Return the distance as an integer
}

void loop() {
  // Put your main code here, to run repeatedly:
  int distance = detectDistance(); // Bring in the distance from the ping sensor

  if (distance < 25) { // If distance is less than 25
    // Turn LED on
    ledOn(ledPin);
  } else if (distance <= 40 && distance >= 30) { // If distance is between 30 and 40
    digitalWrite(ledPin, LOW); // Turn off LED
    moveServo(); // Move the servo 180 degrees
  }

  float cm = 0.0344 / 2 * readUltrasonicDistance(3, 2);
  float inches = cm / 2.54;

  lcd.setCursor(0, 0);
  lcd.print("Inches:");
  lcd.setCursor(8, 0);
  lcd.print(inches, 1);
  lcd.setCursor(0, 1);
  lcd.print("cm:");
  lcd.setCursor(4, 1);
  lcd.print(cm, 1);
  delay(2000);
  lcd.clear();
}

long readUltrasonicDistance(int triggerPin, int echoPin) {
  pinMode(triggerPin, OUTPUT); // Clear the trigger
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  // Reads the echo pin, and returns
  // the sound wave travel time in microseconds
  return pulseIn(echoPin, HIGH);
}