#include <Arduino.h>
#include <LiquidCrystal_I2C.h>

#define ECHO_PIN 12
#define TRIG_PIN 14

#define LCD_ADDRESS 0x27
#define LCD_COLUMNS 16
#define LCD_ROWS 2

LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);

TaskHandle_t TaskSensor;
TaskHandle_t TaskLCD;

volatile float distance;
const long timeout = 4000;

void setup() {
  Serial.begin(115200);
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  lcd.init();
  lcd.backlight();

  xTaskCreatePinnedToCore(
    TaskSensorCode, "TaskSensor", 10000, NULL, 1, &TaskSensor, 0
  );

  xTaskCreatePinnedToCore(
    TaskLCDCode, "TaskLCD", 10000, NULL, 1, &TaskLCD, 1
  );
}

void loop() {
}

void TaskSensorCode(void *pvParameters) {
  for (;;) {
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    distance = pulseIn(ECHO_PIN, HIGH, timeout) * 0.034 / 2;
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

void TaskLCDCode(void *pvParameters) {
  char buffer[LCD_COLUMNS];
  for (;;) {
    lcd.setCursor(0, 0);
    snprintf(buffer, LCD_COLUMNS, "Dist: %.2f cm", distance);
    lcd.print(buffer);
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}