#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050_light.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

MPU6050 mpu(Wire);

const int buttonPin = 5; // Pin untuk tombol
const int potPin = A3; // Pin untuk potensiometer
bool isKmH = true; // Mulai dengan mode km/h
bool lastButtonState = LOW; // Status tombol terakhir
bool buttonPressed = false; // Status apakah tombol ditekan
float odometerKm = 0.0; // Menghitung jarak yang ditempuh dalam km
float odometerMiles = 0.0; // Menghitung jarak yang ditempuh dalam miles
unsigned long lastUpdateTime = 0; // Waktu terakhir update
const int barPadding = 4; // Padding di kiri dan kanan bar BBM
const int barHeight = 6; // Tinggi bar BBM
const int barYOffset = 14; // Jarak bar BBM dari batas bawah layar
const int labelYOffset = 2; // Jarak label E dan F dari bawah bar BBM

const int blinkCount = 3; // Jumlah kedipan
const int blinkInterval = 500; // Interval kedipan (ms)

bool isBlinking = false; // Status kedipan bar BBM
unsigned long lastBlinkTime = 0; // Waktu terakhir kedipan
int blinkState = 0; // Status kedipan (0: off, 1: on)
int blinkCounter = 0; // Hitung jumlah kedipan

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  display.display();

  // Initialize MPU6050
  Serial.println("Initializing MPU6050...");
  byte status = mpu.begin();
  if (status != 0) {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    while (1);
  }
  mpu.calcOffsets(true, true);  // Calibrate gyroscope and accelerometer

  // Initialize button and potensiometer
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(potPin, INPUT);

  Serial.println("MPU6050 initialized");
  lastUpdateTime = millis();
}

void loop() {
  mpu.update();

  // Get normalized accelerometer values
  float ax = mpu.getAccX();
  float ay = mpu.getAccY();
  float az = mpu.getAccZ();

  // Calculate speed in m/s
  float speed = sqrt(ax * ax + ay * ay + az * az);

  // Convert speed to km/h and mph
  int speedKmH = int(speed * 3.6); // Mengubah kecepatan ke km/h dan membulatkannya menjadi integer
  int speedMph = int(speedKmH * 0.621371); // Mengubah kecepatan ke mph dan membulatkannya menjadi integer

  // Calculate elapsed time since last update
  unsigned long currentTime = millis();
  float elapsedTime = (currentTime - lastUpdateTime) / 1000.0; // dalam detik
  lastUpdateTime = currentTime;

  // Update odometer (dalam km dan miles)
  odometerKm += (speedKmH / 3600.0) * elapsedTime; // jarak = kecepatan * waktu
  odometerMiles += (speedMph / 3600.0) * elapsedTime; // jarak = kecepatan * waktu

  // Check if button is pressed to toggle unit
  bool currentButtonState = digitalRead(buttonPin);
  if (currentButtonState == HIGH && lastButtonState == LOW) {
    buttonPressed = !buttonPressed;
    isKmH = !isKmH;
  }
  lastButtonState = currentButtonState;

  // Read potensiometer value and map to bar range
  int potValue = analogRead(potPin);
  int barWidth = map(potValue, 0, 1023, 0, 64 - 2 * barPadding); // Map potensiometer value to width of bar

  // Determine if the fuel level is low (less than 10%)
  float fuelLevel = map(potValue, 0, 1023, 0, 100); // Fuel level in percentage
  bool isLowFuel = (fuelLevel < 10);

  // Handle blinking
  unsigned long currentMillis = millis();
  if (isLowFuel) {
    if (!isBlinking) {
      isBlinking = true;
      lastBlinkTime = currentMillis;
      blinkCounter = 0;
    }
    if (currentMillis - lastBlinkTime >= blinkInterval) {
      lastBlinkTime = currentMillis;
      blinkState = !blinkState;
      blinkCounter++;
      if (blinkCounter >= blinkCount * 2) {
        isBlinking = false;
        blinkCounter = 0;
      }
    }
  } else {
    isBlinking = false;
  }

  // Display speed, odometer, and unit on OLED
  display.clearDisplay();

  // Draw lines to separate sections
  display.drawLine(64, 0, 64, 64, SSD1306_WHITE); // Vertical line
  display.drawLine(64, 32, 128, 32, SSD1306_WHITE); // Horizontal line

  // Display large speed number
  display.setTextSize(3);
  display.setTextColor(SSD1306_WHITE);
  String speedText = isKmH ? String(speedKmH) : String(speedMph);
  int speedTextWidth = speedText.length() * 24; // Estimasi lebar teks
  display.setCursor((64 - speedTextWidth) / 2, 10); // Center text in speed column
  display.print(speedText); // Tampilkan kecepatan dalam km/h atau mph tanpa desimal

  // Display unit in smaller font below the speed number
  display.setTextSize(1);
  int unitWidth = isKmH ? 24 : 22; // Width of the unit text "km/h" or "mph"
  int unitPosition = (64 - unitWidth) / 2; // Center the unit text below the speed number
  display.setCursor(unitPosition, 50);
  display.print(isKmH ? "km/h" : "mph");

  // Display odometer in smaller font with one decimal place
  display.setTextSize(1);
  display.setCursor(70, 10);
  display.print(isKmH ? String(odometerKm, 1) + " km" : String(odometerMiles, 1) + " mi"); // Tampilkan odometer dengan satu angka desimal dan satuan

  // Display fuel gauge with padding and positioning
  int barStartX = 64 + barPadding;
  int barY = 32 + (32 - barHeight) / 2; // Posisikan bar BBM di tengah kolom

  // Handle bar and label blinking
  if (isBlinking) {
    if (blinkState) {
      display.drawRect(barStartX, barY, 64 - 2 * barPadding, barHeight, SSD1306_WHITE); // Draw the outer rectangle for the fuel gauge
      display.fillRect(barStartX, barY, barWidth, barHeight, SSD1306_WHITE); // Draw the filled rectangle for the fuel gauge
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(barStartX, barY + barHeight + labelYOffset + 2); // Posisi label 'E'
      display.print("E");
      display.setCursor(barStartX + 64 - 2 * barPadding - 6, barY + barHeight + labelYOffset + 2); // Posisi label 'F'
      display.print("F");
    } else {
      display.fillRect(barStartX, barY, 64 - 2 * barPadding, barHeight, SSD1306_BLACK); // Hapus bar BBM
      display.setTextColor(SSD1306_BLACK); // Hapus teks
      display.setCursor(barStartX, barY + barHeight + labelYOffset + 2); // Posisi label 'E'
      display.print("E");
      display.setCursor(barStartX + 64 - 2 * barPadding - 6, barY + barHeight + labelYOffset + 2); // Posisi label 'F'
      display.print("F");
    }
  } else {
    display.drawRect(barStartX, barY, 64 - 2 * barPadding, barHeight, SSD1306_WHITE); // Draw the outer rectangle for the fuel gauge
    display.fillRect(barStartX, barY, barWidth, barHeight, SSD1306_WHITE); // Draw the filled rectangle for the fuel gauge
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(barStartX, barY + barHeight + labelYOffset + 2); // Posisi label 'E'
    display.print("E");
    display.setCursor(barStartX + 64 - 2 * barPadding - 6, barY + barHeight + labelYOffset + 2); // Posisi label 'F'
    display.print("F");
  }

  display.display();
}