#include "LiquidCrystal_I2C.h"

LiquidCrystal_I2C lcd(0x27, 16, 2);

#define TRIG_PIN 9
#define ECHO_PIN 8
#define PIZZA_PIN 5
#define BUT_PIN 6

byte lastButState = 0;
uint64_t lastButTime = 0;
byte timer = 200;
byte timerBut = 100;

uint64_t lastDistTime = 0;
int dist = 0;

uint64_t lastPizTime = 0;

byte warring_dist = 50;
byte safe_dist = 20;

bool opasno = false;

bool new_safe = false;

const byte LED_PINS[3] = {2, 3, 4};


void setup() {
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Safe: " + String(safe_dist) + " cm");

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(PIZZA_PIN, OUTPUT);
  pinMode(BUT_PIN, INPUT);
  for(byte i {0}; i < 3; i++) {
    pinMode(LED_PINS[i], OUTPUT);
  }
  Serial.begin(9600);
}

void loop() {
  checkBut();

  if(millis() - lastDistTime > timer) {
    lastDistTime = millis();
    dist = measureDistance();
    lcd.setCursor(0, 1);
    lcd.print("Dist: " + String(dist) + " cm    ");
  }

  if (new_safe) {
    safe_dist = dist;
    warring_dist = safe_dist + 30;
    Serial.println(warring_dist);
  }
  else {
    if(dist > warring_dist) {
      if(opasno) {
        opasno = false;
        lcd.setCursor(0, 0);
        lcd.print("Safe: " + String(safe_dist) + " cm         ");
      }
      set_leds(2);
      noTone(PIZZA_PIN);
    }
    else if (warring_dist >= dist && dist > safe_dist) {
      if(opasno) {
        opasno = false;
        lcd.setCursor(0, 0);
        lcd.print("Safe: " + String(safe_dist) + " cm        ");
      }
      set_leds(1);
      piz(1000);
    }
    else {
      set_leds(0);
      piz(200);
      if(!opasno) {
        opasno = true;
        lcd.setCursor(0, 0);
        lcd.print("!! TOO CLOSE !!     ");
      }
    }
  }
}


int measureDistance() {
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  return (duration * 0.0343) / 2;
}

void set_leds(byte index) {
  digitalWrite(LED_PINS[index], HIGH);
  for(byte i {0}; i < 3; i++) {
    if(index != i) {
      digitalWrite(LED_PINS[i], LOW);
    }
  }
}

void piz(int timer) {
  if(millis() - lastPizTime > timer) {
    lastPizTime = millis();
    int time = timer / 2;
    if(time > 200) {
      time = 200;
    }
    tone(PIZZA_PIN, 500, time);
  }
}

void checkBut() {
  byte butState = digitalRead(BUT_PIN);
  if(butState == HIGH && butState != lastButState) {
    if (millis() - lastButTime > timerBut) {
      lastButTime = millis();
      new_safe = !new_safe;
      if(new_safe) {
        lcd.setCursor(0, 0);
        lcd.print("SET SAFE DIST    ");
      }
      else {
        lcd.setCursor(0, 0);
        lcd.print("Safe: " + String(safe_dist) + " cm     ");
      }
    }
  }
  lastButState = butState;
}