#include <LiquidCrystal.h>

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
int redPin = 6;
int greenPin = 5;
int bluePin = 4;
float m;
bool lightOn = true;

double readUltrasonicDistance(int triggerPin, int echoPin)
{
  pinMode(triggerPin, OUTPUT);
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  return pulseIn(echoPin, HIGH);
}

void setup()
{
  lcd.begin(16, 2);
  lcd.clear();
  pinMode(redPin, OUTPUT);
  digitalWrite(redPin, HIGH);
  pinMode(greenPin, OUTPUT);
  digitalWrite(greenPin, HIGH);
  pinMode(bluePin, OUTPUT);
  digitalWrite(bluePin, HIGH);
}

void SetColor()
{
  if (m > 2)
  {
    digitalWrite(redPin, LOW);
    digitalWrite(greenPin, LOW);
    digitalWrite(bluePin, LOW);
  }
  if (m >= 1 && m <= 2)
  {
    if (millis() % 1000 < 500)
    {
      digitalWrite(redPin, LOW);
      digitalWrite(greenPin, LOW);
      digitalWrite(bluePin, HIGH);
    }else
    {
      digitalWrite(redPin, HIGH);
      digitalWrite(greenPin, HIGH);
      digitalWrite(bluePin, HIGH);
    }
  }
  if (m < 1)
  {
    if(lightOn)
    {
      digitalWrite(redPin, LOW);
      digitalWrite(greenPin, HIGH);
      digitalWrite(bluePin, HIGH);
      lightOn = !lightOn;
    }
    else
    {
      digitalWrite(redPin, HIGH);
      digitalWrite(greenPin, HIGH);
      digitalWrite(bluePin, HIGH);
      lightOn = !lightOn;
    }
  }
}

void loop()
{
  m = readUltrasonicDistance(3, 2) / 5882;
  lcd.setCursor(1, 0);
  lcd.print("Distance");
  lcd.setCursor(1, 1);
  lcd.print(m, 1);
  lcd.setCursor(4, 1);
  lcd.print("m");

  SetColor();

  delay(100);
}