#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, A6};
int currentMenu = 0;
const int totalMenus = 5;
int calibrationValue = 500;

const int RELAY_PIN = 6;  // Pin untuk mengontrol relay
bool relayStatus = false; // Status relay, false untuk off, true untuk on

const int RELAY_PIN_2 = 7;    // Pin untuk relay kedua
bool relayStatus_2 = false;   // Status relay kedua, false untuk off, true untuk on


const int calibrationEEPROMAddress = 0;



void setup() {
  mySerial.begin(9600);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(2, 0);
  lcd.print("HEADLIGHT TESTER");
  lcd.setCursor(7, 1);
  lcd.print("COSBER");
  lcd.setCursor(6, 2);
  lcd.print("KSB-600M");
  lcd.setCursor(8, 3);
  lcd.print("2014");

  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);
  digitalWrite(RELAY_PIN, HIGH); // Matikan relay saat pertama kali dijalankan

  pinMode(RELAY_PIN_2, OUTPUT);
  digitalWrite(RELAY_PIN_2, HIGH);  // Matikan relay kedua saat pertama kali dijalankan



  calibrationValue = 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[5];
  for (int i = 0; i < 5; i++) {
    luxValues[i] = MEASURELight(LDR_PIN[i]);
  }
  float avgLux = (luxValues[0] + luxValues[1] + luxValues[3] + luxValues[4]) / 4.0;
  String beamInfo;
  String deviationInfo;
  if (luxValues[0] >= luxValues[3]) {
    beamInfo = "High Beam";
  } else {
    beamInfo = "Low Beam";
  }
  float deviationInMM = abs(luxValues[1] - luxValues[4]) / 1000.0;
  float radiusMM = 1000.0;
  float deviationInDegrees = (deviationInMM / radiusMM) * (180.0 / PI);
  if (luxValues[4] > luxValues[1]) {
    deviationInfo = "Right : " + String(deviationInMM) + " mm";
  } else if (luxValues[1] > luxValues[4]) {
    deviationInfo = "Left  : " + String(-deviationInMM) + " mm";
  } else {
    deviationInfo = "pass";
  }
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Brightness:");
  lcd.setCursor(0, 1);
  lcd.print("Cd");
  lcd.setCursor(8, 1);
  lcd.print(luxValues[2], 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[] = {"READY", "MENU MEASURE", "MENU LASER", "MENU SENSOR", "MENU CALIBRATE"};
  lcd.setCursor(6, 2);
  lcd.print(menuNames[currentMenu]);
}

void handleMenuSelection() {
  switch (currentMenu) {
    case 0:
      // Aksi untuk menu "READY"
      lcd.clear();
      lcd.setCursor(2, 0);
  lcd.print("HEADLIGHT TESTER");
  lcd.setCursor(7, 1);
  lcd.print("COSBER");
  lcd.setCursor(6, 2);
  lcd.print("KSB-600M");
  lcd.setCursor(8, 3);
  lcd.print("2014");
      delay(2000);  // Tampilkan pesan selama 2 detik sebelum kembali ke READY
      currentMenu = 0;
      updateLCD();
      break;
    case 1:
      MEASUREMenuFunction();
      break;
    case 3:
    lcd.clear();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SENSOR Menu");
  lcd.setCursor(0, 2);
  lcd.print("Enter to continue");
  lcd.setCursor(0, 3);
  lcd.print("or Exit to go back");

      SENSORMenuFunction();
      break;
    case 2:
      LASERMenuFunction();
      break;

    case 4:
      CALIBRATEMenuFunction();
      break;
  }
}

void SENSORMenuFunction() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SENSOR Menu");
  lcd.setCursor(0, 2);
  lcd.print("Enter to continue");
  lcd.setCursor(0, 3);
  lcd.print("or Exit to go back");

  while (true) {
    if (digitalRead(2) == LOW) { 
      currentMenu = 0;
      updateLCD();
      delay(200);
      return;
    }
    if (digitalRead(3) == LOW) { 
      lcd.clear();  // Clear the LCD before displaying SENSOR values
      displaySENSORValues();
      delay(200);
      return;  // Exit the function after displaying SENSOR values
    }
  }
}





void displaySENSORValues() {
  while (true) {
    lcd.clear();
    for (int i = 0; i < 5; i++) {
      lcd.setCursor(0, i);
      lcd.print("S");
      lcd.print(i + 1);
      lcd.print(":");
      lcd.print(MEASURELight(LDR_PIN[i]), 0);
    }
    delay(1000); // Delay antar pembacaan SENSOR (1 detik dalam contoh ini)
  }
}


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

  bool calibrating = true; // Variabel status untuk menandakan apakah sedang dalam mode kalibrasi
  while (calibrating) {
    if (digitalRead(2) == LOW) {
      currentMenu = 0;
      updateLCD();
      delay(200);
      return;
    }
    if (digitalRead(3) == LOW) {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("CALIBRATING");
      lcd.setCursor(0, 2);
      lcd.print("Up/Down to adjust");
      lcd.setCursor(0, 3);
      lcd.print("Enter to save");

      while (true) {
        if (digitalRead(2) == LOW) {
          calibrating = false;
          break;
        }
        if (digitalRead(3) == LOW) {
          EEPROM.put(calibrationEEPROMAddress, calibrationValue);
          calibrating = false;
          updateLCD();
          delay(200);
          break;
        }
        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 LASERMenuFunction() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("LASER Menu");
  
  while (true) {
    if (digitalRead(2) == LOW) { 
      currentMenu = 0;
      updateLCD();
      delay(200);
      return;
    }
    if (digitalRead(3) == LOW) { 
      // Toggle status relay pertama saat tombol Enter ditekan
      relayStatus = !relayStatus;
       relayStatus_2 = !relayStatus_2;
      updateLASERLCD(); // Perbarui tampilan LCD berdasarkan status relay pertama
      controlRelay();   // Kendalikan relay pertama sesuai dengan status
      controlRelay_2();   // Kendalikan relay kedua sesuai dengan status
      delay(200);
    }
   
  }
}



void updateLASERLCD() {
  lcd.setCursor(0, 1);
  lcd.print("LASER 1: ");
  lcd.print(relayStatus ? "OFF " : "ON ");
  lcd.setCursor(0, 2);
  lcd.print("LASER 2: ");
  lcd.print(relayStatus_2 ? "OFF " : "ON ");
}

void controlRelay() {
  digitalWrite(RELAY_PIN, relayStatus ? HIGH : LOW);
}

void controlRelay_2() {
  digitalWrite(RELAY_PIN_2, relayStatus_2 ? HIGH : LOW);
}

void printToThermalPrinter() {
    float luxValues[5];
  for (int i = 0; i < 5; i++) {
    luxValues[i] = MEASURELight(LDR_PIN[i]);
  }
  float avgLux = (luxValues[0] + luxValues[1] + luxValues[3] + luxValues[4]) / 4.0;
  String beamInfo;
  String deviationInfo;
  if (luxValues[0] >= luxValues[3]) {
    beamInfo = "High Beam";
  } else {
    beamInfo = "Low Beam";
  }
  float deviationInMM = abs(luxValues[1] - luxValues[4]) / 10000.0;
  float radiusMM = 10000.0;
  float deviationInDegrees = (deviationInMM / radiusMM) * (180.0 / PI);
  if (luxValues[4] > luxValues[1]) {
    deviationInfo = "Right : " + String(deviationInMM) + " mm";
  } else if (luxValues[1] > luxValues[4]) {
    deviationInfo = "Left  : " + String(-deviationInMM) + " mm";
  } else {
    deviationInfo = "pass";
  }
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("PRINTING.....");
  delay(500);
  displayLightValues();
  printer.justify('C');
  printer.setSize('S');
  printer.doubleHeightOn();
  printer.print("COSBER");
  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();
}