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

// Ініціалізація LCD дисплея 16x2 на адресі 0x27
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Ініціалізація програмної серійної комунікації для Bluetooth модуля HC-05
SoftwareSerial bluetoothSerial(10, 11); // RX, TX

// Пін для кнопки
const int buttonPin = 2;
int screenIndex = 0;

void setup() {
  // Ініціалізація серійного зв'язку
  Serial.begin(9600);
  bluetoothSerial.begin(9600);

  // Ініціалізація LCD дисплея
  lcd.begin(16, 2);  // Вказуємо кількість колонок та рядків
  lcd.backlight();
  
  // Ініціалізація кнопки
  pinMode(buttonPin, INPUT_PULLUP);

  // Виведення стартового повідомлення на дисплей
  lcd.setCursor(0, 0);
  lcd.print("OBD-II Reader");
  delay(2000);
  lcd.clear();
}

void loop() {
  // Перевірка натискання кнопки
  if (digitalRead(buttonPin) == LOW) {
    delay(50);  // Антидребезг
    if (digitalRead(buttonPin) == LOW) {
      screenIndex++;
      if (screenIndex > 2) {
        screenIndex = 0;
      }
      while (digitalRead(buttonPin) == LOW);  // Чекати поки кнопка буде відпущена
    }
  }

  // Отримання даних з OBD-II через Bluetooth
  int rpm = getRPM();
  float fuelLevel = getFuelLevel();
  float fuelRate = getFuelRate();
  float fuelConsumption = calculateFuelConsumption(fuelRate, rpm);

  // Виведення на дисплей залежно від обраного екрану
  lcd.clear();
  switch (screenIndex) {
    case 0:
      lcd.setCursor(0, 0);
      lcd.print("RPM: ");
      lcd.print(rpm);

      lcd.setCursor(0, 1);
      lcd.print("Fuel Lvl: ");
      lcd.print(fuelLevel);
      lcd.print("%");
      break;
      
    case 1:
      lcd.setCursor(0, 0);
      lcd.print("Fuel Rate: ");
      lcd.print(fuelRate);
      lcd.print(" L/h");

      lcd.setCursor(0, 1);
      lcd.print("Cons: ");
      lcd.print(fuelConsumption);
      lcd.print(" L/100km");
      break;

    case 2:
      lcd.setCursor(0, 0);
      lcd.print("RPM: ");
      lcd.print(rpm);
      
      lcd.setCursor(0, 1);
      lcd.print("Fuel Cons:");
      lcd.print(fuelConsumption);
      lcd.print("L/100km");
      break;
  }

  delay(300);  // Затримка перед наступним оновленням
}

// Функція для отримання обертів двигуна (RPM) через Bluetooth
int getRPM() {
  bluetoothSerial.println("010C");  // Запит на RPM
  return parseOBDResponse("010C");
}

// Функція для отримання рівня пального (%) через Bluetooth
float getFuelLevel() {
  bluetoothSerial.println("012F");  // Запит на рівень пального
  return parseOBDResponse("012F") * 100.0 / 255.0;  // Перетворення в %
}

// Функція для отримання розходу пального на годину (L/h) через Bluetooth
float getFuelRate() {
  bluetoothSerial.println("015E");  // Запит на розхід пального
  return parseOBDResponse("015E") / 20.0;  // Перетворення в L/h
}

// Функція для обчислення розходу літрів на 100 км
float calculateFuelConsumption(float fuelRate, int rpm) {
  float speed = (rpm * 0.02);  // Приблизна швидкість на основі RPM
  if (speed > 0) {
    return (fuelRate / speed) * 100.0;  // Обчислення розходу в L/100 км
  } else {
    return 0.0;  // Якщо швидкість 0, повертаємо 0
  }
}

// Функція для обробки відповіді від OBD-II через Bluetooth
int parseOBDResponse(String pid) {
  String response = "";
  delay(100);
  while (bluetoothSerial.available()) {
    char c = bluetoothSerial.read();
    response += c;
  }
  if (response.startsWith("41") && response.substring(2, 4) == pid.substring(2, 4)) {
    int A = strtol(response.substring(6, 8).c_str(), NULL, 16);
    int B = strtol(response.substring(9, 11).c_str(), NULL, 16);
    return (A * 256 + B);
  }
  return 0;
}