#include <Servo.h>
#include <LiquidCrystal.h>

// Definisikan pin untuk tombol
#define BUTTON1_PIN PA10  // Push button 1 (Start timer)
#define BUTTON2_PIN PA9   // Push button 2 (Stop timer)

// Definisikan pin untuk servo (diubah ke PA8 dan PB6)
#define SERVO1_PIN PA8  // Pin untuk servo 1 (misalnya TIM1_CH1)
#define SERVO2_PIN PB6  // Pin untuk servo 2 (misalnya TIM4_CH1)

// Inisialisasi LCD (RS, E, D4, D5, D6, D7)
// Pinout LCD Anda: RS=PA0, E=PA1, D4=PA4, D5=PA5, D6=PA6, D7=PA7
LiquidCrystal lcd(PA0, PA1, PA4, PA5, PA6, PA7);

const float JARAK_KM = 0.01;  // Jarak asumsi 10 meter = 0.01 km
const float BATAS_KECEPATAN = 30.0; // km/jam
const int MAX_VEHICLES = 5;  // Jumlah kendaraan yang akan dilacak

unsigned long startTime[MAX_VEHICLES];  // Waktu mulai untuk setiap kendaraan
unsigned long endTime[MAX_VEHICLES];    // Waktu selesai untuk setiap kendaraan
bool measuring[MAX_VEHICLES];           // Status pengukuran untuk setiap kendaraan
int queue[MAX_VEHICLES];                // Menyimpan urutan kendaraan yang memulai timer
int queueIndex = 0;                     // Indeks kendaraan dalam antrian

Servo servo1;
Servo servo2;

void setup() {
  // Setup pin untuk tombol dengan pull-up internal
  pinMode(BUTTON1_PIN, INPUT_PULLUP);
  pinMode(BUTTON2_PIN, INPUT_PULLUP);

  // Setup servo
  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  Serial.begin(9600); // Pindahkan Serial.begin lebih awal untuk debugging attach
  Serial.println("Servo 1 attached to PA8");
  Serial.println("Servo 2 attached to PB6");


  // Set posisi awal ke 0 derajat
  servo1.write(0);
  servo2.write(0);
  Serial.println("Servos set to 0 degrees initial position.");

  // Inisialisasi array status untuk kendaraan
  for (int i = 0; i < MAX_VEHICLES; i++) {
    measuring[i] = false;
    queue[i] = -1;  // Tidak ada kendaraan dalam antrian awal
  }

  // Inisialisasi LCD
  lcd.begin(16, 2);  // Initialize the LCD with 16 columns and 2 rows
  lcd.print("Welcome STM32!"); // Menampilkan pesan awal
  delay(1000); // Beri waktu lebih untuk membaca
  lcd.clear();
  lcd.print("System Ready");
  
  Serial.println("Sistem Siap. Tekan tombol 1 untuk mulai, tombol 2 untuk stop.");
}

void loop() {
  // Cek apakah Button 1 ditekan untuk memulai timer untuk kendaraan yang belum dimulai
  // Hanya proses satu penekanan tombol per loop untuk menghindari multiple entries cepat
  if (digitalRead(BUTTON1_PIN) == LOW) {
    bool startedNew = false;
    for (int i = 0; i < MAX_VEHICLES; i++) {
      if (!measuring[i]) { // Cari slot kendaraan yang belum diukur
        // Cek apakah kendaraan ini sudah ada di queue (seharusnya tidak jika measuring[i] false)
        // Lebih aman mencari slot kosong di measuring array dan startTime
        startTime[i] = millis();
        measuring[i] = true;  // Menandakan timer kendaraan i dimulai
        
        // Tambahkan vehicle index (i) ke queue
        if(queueIndex < MAX_VEHICLES) {
            queue[queueIndex++] = i; 
            Serial.print("Tombol 1 ditekan - Timer kendaraan ID ");
            Serial.print(i); // Mencetak ID kendaraan (0 to MAX_VEHICLES-1)
            Serial.print(" dimulai. Antrian ke: ");
            Serial.println(queueIndex);
        } else {
            Serial.println("Antrian Penuh!");
            measuring[i] = false; // Batalkan pengukuran jika antrian penuh
        }
        startedNew = true;
        break; // Hanya satu kendaraan per penekanan tombol
      }
    }
    if(startedNew) {
        delay(300); // debounce setelah berhasil memulai timer
    } else if (queueIndex >= MAX_VEHICLES) {
        Serial.println("Tombol 1 ditekan, tapi semua slot/antrian pengukuran penuh.");
        delay(300); // debounce
    } else {
        Serial.println("Tombol 1 ditekan, tapi tidak ada slot measuring kosong (seharusnya tidak terjadi jika queue < MAX_VEHICLES).");
        delay(300); // debounce
    }
  }

  // Cek apakah Button 2 ditekan untuk menghentikan timer kendaraan yang aktif
  if (digitalRead(BUTTON2_PIN) == LOW && queueIndex > 0) {
    int vehicleToStopIndex = queue[0];  // Ambil ID kendaraan dari depan antrian FIFO

    endTime[vehicleToStopIndex] = millis();
    measuring[vehicleToStopIndex] = false;  // Timer kendaraan ini selesai, bisa digunakan lagi

    float selisihWaktu = (endTime[vehicleToStopIndex] - startTime[vehicleToStopIndex]) / 1000.0;  // detik
    float waktuJam = selisihWaktu / 3600.0;  // konversi ke jam

    Serial.print("Tombol 2 ditekan - Menghentikan timer kendaraan ID ");
    Serial.println(vehicleToStopIndex);

    if (waktuJam > 0) {
      float kecepatanKmph = JARAK_KM / waktuJam;

      // Menampilkan informasi kecepatan di Serial Monitor
      Serial.print("Kendaraan ID ");
      Serial.print(vehicleToStopIndex);
      Serial.print(" - Waktu: ");
      Serial.print(selisihWaktu, 3);
      Serial.print(" s | Kecepatan: ");
      Serial.print(kecepatanKmph, 2);
      Serial.println(" km/jam");

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("ID:");
      lcd.print(vehicleToStopIndex);
      lcd.print(" Spd:");
      lcd.print(kecepatanKmph,1); // Kecepatan dengan 1 desimal
      lcd.setCursor(0, 1); // Baris kedua
      lcd.print(kecepatanKmph > BATAS_KECEPATAN ? "OVERLIMIT!" : "NORMAL");


      // Logika untuk menggerakkan servo jika kecepatan melebihi batas
      if (kecepatanKmph > BATAS_KECEPATAN) {
        servo1.write(45);  // Gerakkan servo 1 ke 45 derajat
        servo2.write(45);  // Gerakkan servo 2 ke 45 derajat
        Serial.println("Kecepatan melebihi 30 km/jam, servo ke 45 derajat.");
      } else {
        servo1.write(0);   // Kembalikan servo 1 ke 0 derajat
        servo2.write(0);   // Kembalikan servo 2 ke 0 derajat
        Serial.println("Kecepatan normal, servo ke 0 derajat.");
      }
    } else {
      Serial.println("Waktu tidak valid atau terlalu cepat!");
      lcd.clear();
      lcd.print("Error: Time Low");
      servo1.write(0); // Pastikan servo kembali ke posisi default jika ada error
      servo2.write(0);
    }
    
    // Menggeser antrian untuk menghapus kendaraan pertama
    for (int k = 0; k < queueIndex - 1; k++) {
      queue[k] = queue[k + 1];
    }
    queue[queueIndex - 1] = -1;  // Mengosongkan slot terakhir yang digeser
    queueIndex--;                // Mengurangi jumlah kendaraan dalam antrian

    delay(500); // Tahan tampilan LCD dan posisi servo sejenak
    // Tidak perlu `measuring[vehicleToStopIndex] = false;` lagi, sudah di atas
    delay(300);  // debounce untuk tombol 2
  }
}