#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>
#include "Adafruit_Thermal.h"
#include "SoftwareSerial.h"

#define TX_PIN 10 // Arduino transmit YELLOW WIRE labeled RX on the printer
#define RX_PIN 11 // Arduino receive GREEN WIRE labeled TX on the printer
#define RELAY_PIN 6 // Pin yang digunakan untuk mengontrol relay

SoftwareSerial mySerial(RX_PIN, TX_PIN);
Adafruit_Thermal printer(&mySerial);

LiquidCrystal_I2C lcd(0x27, 20, 4);

const int LDR_PIN[] = {A0, A1, A2, A3, A6};
int currentMenu = 0;
const int totalMenus = 4;
int calibrationValue = 500;
const int calibrationEEPROMAddress = 0;

unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;

void setup() {
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("  HeadLights Tester");
  lcd.setCursor(7, 2);
   lcd.print("COSBER");

  pinMode(2, INPUT_PULLUP); // Tombol Exit
  pinMode(3, INPUT_PULLUP); // Tombol Enter
  pinMode(4, INPUT_PULLUP); // Tombol Up
  pinMode(5, INPUT_PULLUP); // Tombol Down

  pinMode(RELAY_PIN, OUTPUT); // Konfigurasi pin digital untuk relay
  
  calibrationValue = 500;  // Nilai default jika bacaan dari EEPROM gagal
  EEPROM.get(calibrationEEPROMAddress, calibrationValue);


  printer.begin();
  printer.justify('C');
  printer.setSize('S');
}

float measureLight(int pin) {
  int sensorValue = analogRead(pin);
  float voltage = (sensorValue / 1023.0) * 5.0;
  float resistance = (5.0 - voltage) / voltage;
  return calibrationValue / resistance * 100;
}

void displayLightValues() {
  float luxValues[4];
  for (int i = 0; i < 5; i++) {
    luxValues[i] = measureLight(LDR_PIN[i]);
  }
  float avgLux = (luxValues[0] + luxValues[1] + luxValues[2] + luxValues[3]) / 4.0;
  String beamInfo;
  String deviationInfo;
  if (luxValues[3] > avgLux) {
    beamInfo = "High Beam";
  } else {
    beamInfo = "Low Beam";
  }
  float deviationInMM = abs(luxValues[1] - luxValues[3]) / 1000.0;
  float radiusMM = 1000.0;
  float deviationInDegrees = (deviationInMM / radiusMM) * (180.0 / PI);
  if (luxValues[1] > luxValues[3]) {
    deviationInfo = "Right : " + String(deviationInMM) + " mm";
  } else if (luxValues[3] > luxValues[1]) {
    deviationInfo = "Left  : " + String(-deviationInMM) + " mm";
  } else {
    deviationInfo = "No Deviation";
  }
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Brightness:");
  lcd.setCursor(0, 1);
  lcd.print("Cd");
  lcd.setCursor(8, 1);
  lcd.print(luxValues[5], 0);
  lcd.setCursor(0, 2);
  lcd.print(beamInfo);
  lcd.setCursor(0, 3);
  lcd.print(deviationInfo);
  delay(1000);
}

void loop() {
  int readingExit = digitalRead(2);
  int readingEnter = digitalRead(3);
  int readingUp = digitalRead(4);
  int readingDown = digitalRead(5);

  if (readingExit == LOW && currentMenu != 0 && millis() - lastDebounceTime > debounceDelay) { // Tombol Exit
    currentMenu = 0;
    updateLCD();
    digitalWrite(RELAY_PIN, LOW); // Mematikan relay saat keluar dari menu "laser"
    lastDebounceTime = millis();
  }
  if (readingEnter == LOW && millis() - lastDebounceTime > debounceDelay) { // Tombol Enter
    handleMenuSelection();
    lastDebounceTime = millis();
  }
  if (readingUp == LOW && millis() - lastDebounceTime > debounceDelay) { // Tombol Up
    currentMenu = (currentMenu + 1) % totalMenus;
    updateLCD();
    lastDebounceTime = millis();
  }
  if (readingDown == LOW && millis() - lastDebounceTime > debounceDelay) { // Tombol Down
    currentMenu = (currentMenu - 1 + totalMenus) % totalMenus;
    updateLCD();
    lastDebounceTime = millis();
  }
}

void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("                 ");
  lcd.setCursor(0, 1);
  lcd.setCursor(0, 3);
  //lcd.print("UP & DOWN to select");

  const char* menuNames[] = {"READY", "MEASURE", "LASER", "CALIBRATE", "DETAIL"};  // Tambahkan menu "DETAIL"
  lcd.print(menuNames[currentMenu]);
  lcd.setCursor(0, 0);
  lcd.print("Head Light Tester");
}

void handleMenuSelection() {
  switch (currentMenu) {
    case 0:
      // Aksi untuk menu "standby"
      // Contoh: standbyMenuFunction();
      break;
    case 1:
      measureMenuFunction();
      break;
    case 2:
      laserMenuFunction();
      break;
    case 3:
      calibrateMenuFunction();
      break;
  }
}

void calibrateMenuFunction() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Calibrate Menu");
  lcd.setCursor(0, 1);
  lcd.print("Count: " + String(calibrationValue));

  while (true) {
    int readingExit = digitalRead(2);
    int readingEnter = digitalRead(3);
    int readingUp = digitalRead(4);
    int readingDown = digitalRead(5);

    if (readingExit == LOW && millis() - lastDebounceTime > debounceDelay) { 
      currentMenu = 0;
      updateLCD();
      lastDebounceTime = millis();
      return;
    }
    if (readingEnter == LOW && millis() - lastDebounceTime > debounceDelay) { 
      EEPROM.put(calibrationEEPROMAddress, calibrationValue);
      lcd.setCursor(0, 3);
      lcd.print("Calibrated        ");
      delay(1000);
      updateLCD();
      lastDebounceTime = millis();
    }
    if (readingUp == LOW && millis() - lastDebounceTime > debounceDelay) { 
      calibrationValue -= 100;
      updateCalibrationLCD();
      lastDebounceTime = millis();
    }
    if (readingDown == LOW && millis() - lastDebounceTime > debounceDelay) { 
      calibrationValue += 100;
      updateCalibrationLCD();
      lastDebounceTime = millis();
    }
  }
}



void updateCalibrationLCD() {
  lcd.setCursor(7, 1);
  lcd.print("             ");
  lcd.setCursor(7, 1);
  lcd.print(String(calibrationValue));
}

void measureMenuFunction() {
  displayLightValues();

  while (true) {
    int readingExit = digitalRead(2);
    int readingEnter = digitalRead(3);
    int readingUp = digitalRead(4);
    int readingDown = digitalRead(5);

    if (readingExit == LOW && millis() - lastDebounceTime > debounceDelay) { 
      currentMenu = 0;
      updateLCD();
      digitalWrite(RELAY_PIN, LOW); // Mematikan relay saat keluar dari menu "laser"
      lastDebounceTime = millis();
      return;
    }
    if (readingEnter == LOW && millis() - lastDebounceTime > debounceDelay) { 
      printToThermalPrinter();
      lastDebounceTime = millis();
    }
    if (readingDown == LOW && millis() - lastDebounceTime > debounceDelay) { 
      // Tambahkan logika untuk pengukuran ulang di sini
      displayLightValues();
      lastDebounceTime = millis();
    }
  }
}


void printToThermalPrinter() {
  float luxValues[4];
  for (int i = 0; i < 4; i++) {
    luxValues[i] = measureLight(LDR_PIN[i]);
  }
  float avgLux = (luxValues[0] + luxValues[1] + luxValues[2] + luxValues[3]) / 4.0;
  String beamInfo;
  String deviationInfo;
  if (luxValues[3] > avgLux) {
    beamInfo = "High Beam";
  } else {
    beamInfo = "Low Beam";
  }
  float deviationInMM = abs(luxValues[1] - luxValues[3]) / 1000.0;
  float radiusMM = 1000.0;
  float deviationInDegrees = (deviationInMM / radiusMM) * (180.0 / PI);
  if (luxValues[1] > luxValues[3]) {
    deviationInfo = "Right : " + String(deviationInMM) + " mm";
  } else if (luxValues[3] > luxValues[1]) {
    deviationInfo = "Left  : " + String(-deviationInMM) + " mm";
  } else {
    deviationInfo = "No Deviation";
  }
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("PRINTING.....");
  //delay(500);
  //displayLightValues();
  printer.justify('C');
  printer.setSize('S');
  printer.doubleHeightOn();
  printer.print("BLANGKONMUTER");
  printer.doubleHeightOff();
  printer.println();
  printer.println("HEADLIGHT TESTER");
  printer.println();
  printer.underlineOn();
  printer.println("Measurement Data");
  printer.underlineOff();
  printer.println();
  printer.print("Intensity: " + String(luxValues[5], 0));
  printer.println(" Cd");
  printer.println("Beam Info: " + beamInfo);
  printer.println("Deviation Info: " + deviationInfo);
  printer.feed(3);
  printer.sleep();
  displayLightValues();
}

void laserMenuFunction() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Laser Menu");
  lcd.setCursor(0, 1);
  lcd.print("Press Enter");

  bool isLaserOn = false; // Menyimpan status laser (awalnya dimatikan)

  while (true) {
    if (digitalRead(2) == LOW) {
      currentMenu = 0;
      updateLCD();
      digitalWrite(RELAY_PIN, LOW); // Mematikan relay saat keluar dari menu "laser"
      delay(200);
      return;
    }
    if (digitalRead(3) == LOW) {
      // Toggle status relay
      isLaserOn = !isLaserOn; // Ubah status laser
      digitalWrite(RELAY_PIN, isLaserOn); // Aktifkan atau matikan relay sesuai dengan status
      // Tampilkan status laser di LCD
      lcd.setCursor(0, 3);
      if (isLaserOn) {
        lcd.print("Laser On   ");
      } else {
        lcd.print("Laser Off  ");
      }
      delay(200);
    }
  }
}