#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C LCD(0x27,16,2);

int redPin= A1;
int greenPin = A2;
int  bluePin = A3;

long distance = 500;

class Sensor {
  public:

    long Distance();

    Sensor(int pTrig, int pEcho) {

      this->pTrig = pTrig;
      this->pEcho = pEcho;
      pinMode(pTrig, OUTPUT);
      pinMode(pEcho, INPUT);
    }

  private:
    int pTrig;
    int pEcho;
    long echo, diss;
};

long Sensor::Distance(){
  digitalWrite(pTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(pTrig, HIGH);
  delayMicroseconds(5);
  digitalWrite(pTrig, LOW);
  
  echo = pulseIn(pEcho, HIGH);
  
  diss = echo / 58.31;
  return diss;
}

Sensor sens(13, 12);

void setup() {
  LCD.init();
  LCD.backlight();

  LCD.setCursor ( 0, 1);
  LCD.print("Test LCD");
  delay(1000);
  LCD.clear();
  
  
}

void loop() {

  LCD.setCursor ( 0, 0);
  
  if (distance != sens.Distance()){
    distance = sens.Distance();
    delay(100);
    LCD.clear();
  }

  LCD.print(distance);

  if(distance > 10 && distance < 30){
    setColor(0,255,255);
  }
  else if(distance >= 30 && distance < 50){
    setColor(255,0,255);
  }
  else if(distance >= 50){
    setColor(255,255,0);
  }


}

void setColor(int redValue, int greenValue,  int blueValue) {
  analogWrite(redPin, redValue);
  analogWrite(greenPin,  greenValue);
  analogWrite(bluePin, blueValue);
}