#include <LiquidCrystal_I2C.h>
#define PIN_TRIG 3
#define PIN_ECHO 2
#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);
const int rgbPins[] = { 11, 10, 9};
float r, g, b; 

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  lcd.init();
  lcd.backlight();
  for( int i=0; i<3; i++){
    pinMode( rgbPins[i], OUTPUT);
  }
}
int pwmR = 0;
int pwmG = 0;
int pwmB = 0;

void vzdLed(int vzdalenost){
  if(vzdalenost<100){
    pwmR = 255;
    pwmG = 0;
    pwmB = 0;
  }else if(vzdalenost>100 && vzdalenost<200){
    pwmR = 0;
    pwmG = 255;
    pwmB = 0;
  }else{
    pwmR = 0;
    pwmG = 0;
    pwmB = 255;
  }
}

void loop() {
  // put your main code here, to run repeatedly:
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  int duration = pulseIn(PIN_ECHO, HIGH);
  int vzdalenost = duration/58;
  lcd.setCursor(0, 0);
  lcd.print("CM:");
  lcd.setCursor(3, 0);
  lcd.print(duration / 58);
  vzdLed(vzdalenost);
  analogWrite( rgbPins[0], pwmR);
  analogWrite( rgbPins[1], pwmG);
  analogWrite( rgbPins[2], pwmB);
  delay(1500);
}