#include <LiquidCrystal.h>

#define LED_RED 7
#define LED_GREEN 6
#define LED_BLUE 5

#define TRIG 2
#define ECHO 3

class RgbLed
{
public:
  RgbLed(int pinR, int pinG, int pinB)
  {
    pinMode(LED_RED, OUTPUT);
    pinMode(LED_GREEN, OUTPUT);
    pinMode(LED_BLUE, OUTPUT);
    _pinRed = pinR;
    _pinGreen = pinG;
    _pinBlue = pinB;
    digitalWrite(_pinRed, HIGH);
    digitalWrite(_pinGreen, HIGH);
    digitalWrite(_pinBlue, HIGH);
  }
  ~RgbLed() = default;

  void setColour(int colour)
  {
    //colour = 0(000) - Выключено
    //colour = 1(001) - Красный
    //colour = 2(010) - Зелёный
    //colour = 3(011) - жёлтый
    //colour = 4(100) - Синий
    //colour = 5(101) - Сиреневый
    //colour = 6(110) - Бюриюзовый
    //colour = 7(111) - Белый
    _red = !(colour & 0b1);
    _green = !(colour & 0b10);
    _blue = !(colour & 0b100);

    digitalWrite(_pinRed, _red);
    digitalWrite(_pinGreen, _green);
    digitalWrite(_pinBlue, _blue);
  }

  void turnOn()
  {
    digitalWrite(_pinRed, _red);
    digitalWrite(_pinGreen, _green);
    digitalWrite(_pinBlue, _blue);
  }

  void turnOff()
  {
    digitalWrite(_pinRed, 1);
    digitalWrite(_pinGreen, 1);
    digitalWrite(_pinBlue, 1);
  } 

private:
  
  int _red;
  int _green;
  int _blue;

  int _pinRed;
  int _pinGreen;
  int _pinBlue;
};

RgbLed led(LED_RED, LED_GREEN, LED_BLUE);
LiquidCrystal lcd(13, 12, 11, 10, 9, 8);

bool flash = false;
int time = 0;
int curDistanceRange= 0;//диапазон против постоянной перезаписи цвета светоиода

void process()
{
  //обработка дальномера
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  double distance = pulseIn(ECHO, HIGH)/5800.0;

  //преобразование в char и вывод на дисплей
  const int kSize = 6;
  char bufer[kSize];
  lcd.setCursor(0,1);
  dtostrf(distance, 1, 1, bufer);
  print(bufer, kSize);

  //задание режима работы светодиода
  if(distance > 0 && distance <= 1 && curDistanceRange != 1)
  {
    flash = true;
    led.setColour(1);
    time = 100;
    curDistanceRange = 1;
  }
  else if(distance > 1 && distance <= 2 && curDistanceRange != 2)
  {
    flash = true;
    led.setColour(3);
    time = 500;
    curDistanceRange = 2;
  }
  else if(distance > 2 && curDistanceRange != 3)
  {
    flash = false;
    led.setColour(2);
    curDistanceRange = 3;
  }
}

//вспомогательная функция печати на дисплей, чтобы не выполнять clear каждый новый цикл (защита от мерцания)
void print(char *bufer, int size)
{
  bool endString = false;
  for(int i = 0; i < size; ++i)
  {
    if (bufer[i] == '\0')
    {
      endString = true;
      lcd.print(" m");
      continue;
    }
    if(endString)
    {
      lcd.print(" ");
    }    
    else
    {
      lcd.print(bufer[i]);
    }    
  }
}

void setup() 
{
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  lcd.begin(16, 2, LCD_5x8DOTS);
  lcd.clear();
  lcd.print("Distance:");
}

bool processed = false;

void loop() 
{
  unsigned long curLoopTime = millis();
  //обеспечение интервала опроса дальномера = 100 мс, чтобы не блокировать поток моргания светодиода через delay(100)
  if(curLoopTime % 100 > 90 && !processed)
  {
    process();
    processed = true;
  }
  else if(curLoopTime % 100 < 90)
  {
    processed = false;
  }
  
  if(flash)
  {
    int range = curLoopTime % (2 * time);
    if( range > 0 && range < time)
    {
      led.turnOff();
    }
    else
    {
      led.turnOn();
    }    
  }   
  
}