#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <TinyGPS.h>
#include <SoftwareSerial.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);

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
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

// GPS setup
SoftwareSerial ss(4, 3); // RX, TX
TinyGPS gps;

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

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

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

  lastUpdateTime = millis();
}

void loop() {
  while (ss.available() > 0) {
    gps.encode(ss.read());
  }

  float flat, flon;
  unsigned long age;
  gps.f_get_position(&flat, &flon, &age);

  if (gps.f_speed_kmph() != TinyGPS::GPS_INVALID_F_SPEED) {
    float speedKmH = gps.f_speed_kmph();
    float speedMph = gps.f_speed_mph();

    // 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) {
      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

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

    // Display large speed number
    display.setTextSize(3);
    display.setTextColor(SSD1306_WHITE);
    String speedText = isKmH ? String(int(speedKmH)) : String(int(speedMph));
    int speedTextWidth = speedText.length() * 18; // 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
    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();
  } else {
    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(0, 0);
    display.print("Waiting for GPS signal...");
    display.display();
  }
  
  Serial.println("Looping..."); // Debugging output to Serial Monitor
  delay(1000); // Add a small delay to allow Serial Monitor to catch up
}