#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

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

LiquidCrystal_I2C lcd(0x27, 20, 4);

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

void setup() {
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Head Light Tester");
  lcd.setCursor(0, 1);

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

  calibrationValue = EEPROM.read(calibrationEEPROMAddress);

  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 < 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("Brightness:");
  lcd.setCursor(0, 1);
  lcd.print("Cd");
  lcd.setCursor(8, 1);
  lcd.print(luxValues[0], 0);
  lcd.setCursor(0, 2);
  lcd.print(beamInfo);
  lcd.setCursor(0, 3);
  lcd.print(deviationInfo);
  delay(1000);
}

void loop() {
  if (digitalRead(2) == LOW) { // Tombol Exit
    currentMenu = 0;
    updateLCD();
    delay(200);
  }
  if (digitalRead(3) == LOW) { // Tombol Enter
    handleMenuSelection();
    delay(200);
  }
  if (digitalRead(4) == LOW) { // Tombol Up
    currentMenu = (currentMenu + 1) % totalMenus;
    updateLCD();
    delay(200);
  }
  if (digitalRead(5) == LOW) { // Tombol Down
    currentMenu = (currentMenu - 1 + totalMenus) % totalMenus;
    updateLCD();
    delay(200);
  }
}

void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Headlight Tester");
  lcd.setCursor(0, 1);
  lcd.print("                 ");
  lcd.setCursor(0, 2);
  lcd.print("                 ");
  lcd.setCursor(0, 3);
  lcd.print("                 ");

  const char* menuNames[] = {"standby", "measure", "laser", "distance", "calibrate"};
  lcd.setCursor(0, 1);
  lcd.print(menuNames[currentMenu]);
}

void handleMenuSelection() {
  switch (currentMenu) {
    case 0:
      // Aksi untuk menu "standby"
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Headlight Tester");
      delay(2000);  // Tampilkan pesan selama 2 detik sebelum kembali ke standby
      currentMenu = 0;
      updateLCD();
      break;
    case 1:
      measureMenuFunction();
      break;
    case 4:
      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) {
    if (digitalRead(2) == LOW) { 
      currentMenu = 0;
      updateLCD();
      delay(200);
      return;
    }
    if (digitalRead(3) == LOW) { 
      EEPROM.write(calibrationEEPROMAddress, calibrationValue);
      delay(200);
    }
    if (digitalRead(4) == LOW) { 
      calibrationValue += 1;
      updateCalibrationLCD();
      delay(200);
    }
    if (digitalRead(5) == LOW) { 
      calibrationValue -= 1;
      updateCalibrationLCD();
      delay(200);
    }
  }
}

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

void measureMenuFunction() {
  bool measuring = true;  // Variabel status untuk menandakan apakah sedang dalam mode pengukuran
  while (true) {
    if (digitalRead(2) == LOW) { 
      currentMenu = 0;
      updateLCD();
      delay(200);
      return;
    }
    if (digitalRead(3) == LOW) { 
      if (measuring) {
        printToThermalPrinter();
        delay(200);
      } else {
        measuring = true;  // Memulai mode pengukuran
        displayLightValues();
        delay(200);
      }
    }
    if (digitalRead(5) == LOW) { 
      if (measuring) {
        // Hanya menampilkan nilai cahaya jika sedang dalam mode pengukuran
        displayLightValues();
        delay(200);
      } else {
        // Jika tidak dalam mode pengukuran, kembali ke menu utama
        currentMenu = 0;
        updateLCD();
        delay(200);
        return;
      }
    }
    if (digitalRead(4) == LOW && measuring) { 
      // Jika tombol down ditekan dan sedang dalam mode pengukuran, ulangi pengukuran
      displayLightValues();
      delay(200);
    }
  }
}




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[0], 0));
  printer.println(" Cd");
  printer.println("Beam Info: " + beamInfo);
  printer.println("Deviation Info: " + deviationInfo);
  printer.feed(3);
  printer.sleep();
}