#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>
#include "DFRobot_INA219.h"  // Library untuk sensor arus INA219

LiquidCrystal_I2C   lcd(0x27, 20, 4);
DFRobot_INA219_IIC  ina219(&Wire, INA219_I2C_ADDRESS4);  // Objek sensor INA219

// prototype function
void runMotor(int speed, bool reversed = 0);

// pin-pin GPIO
const int reverse   = 2;
const int shutoff   = 3;
const int btnUp     = 4;
const int btnDown   = 5;
const int btnOk     = 6;
const int btnBack   = 7;
const int R_EN_PIN  = 8;  // Pin untuk kontrol enable motor
const int L_EN_PIN  = 9;  // Pin untuk kontrol enable motor
const int RPWM_PIN  = 10;
const int LPWM_PIN  = 11;
const int ENC_A     = 12;
const int ENC_B     = 13;

// kecepatan motor (PWM 0-255)
int speed = 255;

// is reversing state
int isReverse = 0;
long reverseTime = 0;
long reverseDuration = 2000;
int currentMenuIndex = 0;
int selectedUlirIndex = -1;
int selectedCurrentIndex = -1;
bool inHomeScreen = true;
bool inUlirSelection = false;
bool inCurrentSettingsMode = false;
bool inUlirDetailMode = false;
bool inCurrentValueMode = false;
int inOverCurrentMode = false;
bool motorRunning = false;
bool isCurrentMonitoring = false;

unsigned long motorStartTime = 0;  // Waktu motor mulai
const unsigned long delayMonitoring = 200;  // Delay 50 milidetik sebelum monitoring arus

const char *ulirOptions[] = {"M8", "M10", "M33", "M36"};
const int ulirCount = sizeof(ulirOptions) / sizeof(ulirOptions[0]);

int ulirSpeed[4] = {100, 125, 150, 200};
int ulirDelay[4] = {3500, 3000, 2500, 2000};
float maxCurrentValues[4] = {250.0, 300.0, 350.0, 400.0};
float currentCurrent = 0.0;  // Arus yang terukur saat ini

// Alamat EEPROM untuk penyimpanan data arus
const int eepromStartAddress = 0;

void setup() {
  Serial.begin(9600);  // Untuk debugging
  Serial.println("Ulir Tester Serial Running...");
  lcd.init();
  lcd.backlight();
  
  // Inisialisasi INA219
  if (!ina219.begin()) {
    lcd.setCursor(0, 0);
    lcd.print("INA219 not found");
    while (1) { delay(10); }
  }
  else{
    Serial.println("INA219 OK");
  }
  
  lcd.setCursor(6, 0);
  lcd.print("TES ULIR");
  lcd.setCursor(5, 3);
  lcd.print("PT. TOSHIN");
  delay(1500);
  lcd.clear();
  
  // setup pin-pin Button
  pinMode(btnUp, INPUT_PULLUP);
  pinMode(btnDown, INPUT_PULLUP);
  pinMode(btnOk, INPUT_PULLUP);
  pinMode(btnBack, INPUT_PULLUP);

  // setup pin-pin kontrol motor
  pinMode(R_EN_PIN, OUTPUT);
  digitalWrite(R_EN_PIN, HIGH);
  pinMode(L_EN_PIN, OUTPUT);
  digitalWrite(L_EN_PIN, HIGH);
  pinMode(RPWM_PIN, OUTPUT);
  pinMode(LPWM_PIN, OUTPUT);

  // pin kontrol arah/stop motor
  pinMode(reverse, INPUT_PULLUP);
  pinMode(shutoff, OUTPUT);

  // Membaca nilai arus maksimal dari EEPROM
  loadCurrentValuesFromEEPROM();
  loadSpeedValuesFromEEPROM();
  
  displayHomeScreen();
}

void loop() {
  // Monitoring arus saat motor berjalan
  speed = analogRead(A0);
  speed = map(speed, 0, 850, 0, 255);
  if(speed <= 13){
    speed = 13;
  }else if(speed >= 255){
    speed = 255;
  }
  Serial.print("Speed  : ");
  Serial.println(speed);
  Serial.print("Reverse: ");
  Serial.println(digitalRead(reverse));

  //speed = 100;
  if (motorRunning) {
    unsigned long currentTime = millis();
    // Cek apakah sudah melewati waktu delay monitoring
    if (!isCurrentMonitoring && (currentTime - motorStartTime >= delayMonitoring)) {
      isCurrentMonitoring = true;  // Mulai monitoring arus
    }

    // Baca nilai arus jika monitoring aktif
    if (isCurrentMonitoring && !inOverCurrentMode) {
      // tampilkan nilai arus live
      currentCurrent = ina219.getCurrent_mA();  // Konversi mA ke A
      if(currentCurrent == 0.0){
        ina219.begin();
      }
      lcd.setCursor(5, 2);
      lcd.print(currentCurrent, 2);
      lcd.print(" mA  ");

      // jika sensor detect, reverse motor selama x durasi, kembalikan arah putar, delay 500ms agar tdk trip sensor arus
      if(digitalRead(reverse) == 0){
        isReverse = 1;
        reverseTime = millis();
      }
      if(isReverse){
        Serial.println("REVERSING");
        Serial.println();
        lcd.setCursor(18,2);
        lcd.print("<<");
        runMotor(speed, 1);
        currentTime = millis();
        if(currentTime - reverseTime >= reverseDuration){
          reverseTime = currentTime;
          runMotor(speed, 0);
          isReverse = 0;
          delay(80);
        }
      }
      if(!isReverse){
        Serial.println("NOT REVERSING");
        Serial.println();
        lcd.setCursor(18,2);
        lcd.print("  ");
        runMotor(speed, 0);
      }

      // Cek apakah arus melebihi batas maksimal
      if (currentCurrent > maxCurrentValues[selectedUlirIndex] && !isReverse) {
        runMotor(0);
        showOverCurrentWarning();
      }
    }
  }

  if(inUlirDetailMode){
      // tampilkan speed
      lcd.setCursor(0, 3);
      lcd.print("Speed: ");
      int spd = map(speed, 0, 255, 0, 100);
      lcd.print(spd > 100 ? 100 : spd);
      lcd.print("  ");
  }
  
  // Cek input tombol
  if (digitalRead(btnUp) == LOW) {
    navigateMenu(-1);
    delay(200);
  } else if (digitalRead(btnDown) == LOW) {
    navigateMenu(1);
    delay(200);
  } else if (digitalRead(btnOk) == LOW) {
    selectMenu();
    delay(200);
  } else if (digitalRead(btnBack) == LOW) {
    backMenu();
    delay(200);
  }
}

void runMotor(int speed, bool reverse = 0){
  if(speed > 0){
    // variable untuk mencatat status running motor & mengatur kapan monitoring arus
    motorRunning = true;
    //isCurrentMonitoring = false;
    motorStartTime = millis();  // Catat waktu mulai

    if(reverse){
    analogWrite(RPWM_PIN, 0);
    analogWrite(LPWM_PIN, speed);
    }
    else{
      analogWrite(RPWM_PIN, speed);
      analogWrite(LPWM_PIN, 0);
    }
  }
  else{
    motorRunning = false;
    isCurrentMonitoring = false;
    analogWrite(LPWM_PIN, 0);
    analogWrite(RPWM_PIN, 0);
  }
}

void showOverCurrentWarning() {
  lcd.clear();
  inUlirDetailMode    = false;
  inUlirSelection     = false;
  isCurrentMonitoring = false;
  inOverCurrentMode   = true;
  
  lcd.setCursor(0, 0);
  lcd.print("WARNING-OVER CURRENT");
  lcd.setCursor(0, 1);
  lcd.print("Maks: ");
  lcd.print(maxCurrentValues[selectedUlirIndex], 2);
  lcd.print("mA");
  lcd.setCursor(0, 2);
  lcd.print("Aktual: ");
  lcd.print(currentCurrent, 2);
  lcd.print("mA");
  lcd.setCursor(0, 3);
  lcd.print("OK-Revers|BATAL-Back");
}

// Fungsi untuk menyimpan nilai float ke EEPROM
void writeFloatToEEPROM(int address, float value) {
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(float); i++) {
    EEPROM.write(address + i, p[i]);
  }
}

// Fungsi untuk menyimpan nilai integer ke EEPROM
void writeIntToEEPROM(int address, float value) {
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(int); i++) {
    EEPROM.write(address + 4 + i, p[i]);
  }
}

// Fungsi untuk membaca nilai float dari EEPROM
float readFloatFromEEPROM(int address) {
  float value;
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(float); i++) {
    p[i] = EEPROM.read(address + i);
  }
  return value;
}

// Fungsi untuk membaca nilai integer dari EEPROM
float readIntFromEEPROM(int address) {
  float value;
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(int); i++) {
    p[i] = EEPROM.read(address + 4 + i);
  }
  return value;
}

// Menyimpan semua nilai arus maksimal ke EEPROM
void saveCurrentValuesToEEPROM() {
  for (int i = 0; i < ulirCount; i++) {
    writeFloatToEEPROM(eepromStartAddress + (i * sizeof(float)), maxCurrentValues[i]);
  }
}

// Menyimpan semua nilai speed maksimal ke EEPROM
void saveSpeedValuesToEEPROM() {
  for (int i = 0; i < ulirCount; i++) {
    writeIntToEEPROM(eepromStartAddress + (i * sizeof(int)), maxCurrentValues[i]);
  }
}

// Membaca semua nilai arus maksimal dari EEPROM
void loadCurrentValuesFromEEPROM() {
  // Cek apakah EEPROM sudah pernah diinisialisasi
  float testValue = readFloatFromEEPROM(eepromStartAddress);
  
  // Jika nilai tidak masuk akal, gunakan nilai default
  if (isnan(testValue) || testValue < 0.1 || testValue > 5000.0) {
    // EEPROM belum diinisialisasi, simpan nilai default
    saveCurrentValuesToEEPROM();
  } else {
    // Baca semua nilai dari EEPROM
    for (int i = 0; i < ulirCount; i++) {
      float value = readFloatFromEEPROM(eepromStartAddress + (i * sizeof(float)));
      // Pastikan nilai masuk akal
      if (!isnan(value) && value >= 0.1 && value <= 5000.0) {
        maxCurrentValues[i] = value;
      }
    }
  }
}

// Membaca semua nilai speed maksimal dari EEPROM
void loadSpeedValuesFromEEPROM() {
  // Cek apakah EEPROM sudah pernah diinisialisasi
  float testValue = readIntFromEEPROM(eepromStartAddress);
  
  // Jika nilai tidak masuk akal, gunakan nilai default
  if (isnan(testValue) || testValue < 13 || testValue > 255.0) {
    // EEPROM belum diinisialisasi, simpan nilai default
    saveSpeedValuesToEEPROM();
  } else {
    // Baca semua nilai dari EEPROM
    for (int i = 0; i < ulirCount; i++) {
      float value = readIntFromEEPROM(eepromStartAddress + (i * sizeof(int)));
      // Pastikan nilai masuk akal
      if (!isnan(value) && value >= 13 && value <= 255.0) {
        maxSpeedValues[i] = value;
      }
    }
  }
}

void displayHomeScreen() {
  lcd.clear();
  inHomeScreen = true;
  inUlirSelection = false;
  inCurrentSettingsMode = false;
  inUlirDetailMode = false;
  inCurrentValueMode = false;
  
  lcd.setCursor(2, 0);
  lcd.print("MENU UTAMA");
  
  lcd.setCursor(1, 1);
  lcd.print("1. Pilih Ulir");
  lcd.setCursor(1, 2);
  lcd.print("2. Ubah Nilai Arus");
  lcd.setCursor(1, 3);
  lcd.print("3. Ubah Nilai Speed");
  
  lcd.setCursor(0, 1 + currentMenuIndex);
  lcd.print(">");
}

void displayUlirSelection() {
  lcd.clear();
  inHomeScreen = false;
  inUlirSelection = true;
  inUlirDetailMode = false;
  
  lcd.setCursor(1, 0);
  lcd.print("PILIH UKURAN ULIR");
  
  for (int i = 0; i < 3; i++) {
    int menuIndex = currentMenuIndex + i;
    if (menuIndex >= ulirCount) break;
    
    lcd.setCursor(1, i + 1);
    if (menuIndex == currentMenuIndex) {
      lcd.print("-> ");
    } else {
      lcd.print("   ");
    }
    lcd.print(ulirOptions[menuIndex]);
  }
}

void displayCurrentSettings() {
  lcd.clear();
  inHomeScreen = false;
  inCurrentSettingsMode = true;
  inCurrentValueMode = false;
  
  lcd.setCursor(1, 0);
  lcd.print("PILIH ULIR UNTUK");
  lcd.setCursor(1, 1);
  lcd.print("UBAH ARUS MAKS");
  
  for (int i = 0; i < 2; i++) {
    int menuIndex = currentMenuIndex + i;
    if (menuIndex >= ulirCount) break;
    
    lcd.setCursor(1, i + 2);
    if (menuIndex == currentMenuIndex) {
      lcd.print("-> ");
    } else {
      lcd.print("   ");
    }
    lcd.print(ulirOptions[menuIndex]);
  }
}

void displayCurrentValue() {
  lcd.clear();
  inCurrentValueMode = true;
  inCurrentSettingsMode = false;
  
  lcd.setCursor(1, 0);
  lcd.print("ATUR ARUS MAKS ");
  lcd.print(ulirOptions[selectedCurrentIndex]);
  
  lcd.setCursor(0, 2);
  lcd.print("Arus: ");
  lcd.print(maxCurrentValues[selectedCurrentIndex], 1);
  lcd.print(" mA");
  
  // Menampilkan informasi penyimpanan
  lcd.setCursor(1, 3);
  lcd.print("Tekan OK: Simpan");
}

void navigateMenu(int direction) {
  if (inHomeScreen) {
    currentMenuIndex += direction;
    if (currentMenuIndex < 0) currentMenuIndex = 2;
    if (currentMenuIndex > 2) currentMenuIndex = 0;
    displayHomeScreen();
  } else if (inUlirSelection) {
    currentMenuIndex += direction;
    if (currentMenuIndex < 0) currentMenuIndex = ulirCount - 1;
    if (currentMenuIndex >= ulirCount) currentMenuIndex = 0;
    displayUlirSelection();
  } else if (inCurrentSettingsMode) {
    currentMenuIndex += direction;
    if (currentMenuIndex < 0) currentMenuIndex = ulirCount - 1;
    if (currentMenuIndex >= ulirCount) currentMenuIndex = 0;
    displayCurrentSettings();
  } else if (inCurrentValueMode) {
    maxCurrentValues[selectedCurrentIndex] += direction * -10.0;
    if (maxCurrentValues[selectedCurrentIndex] < 0.1) maxCurrentValues[selectedCurrentIndex] = 0.1;
    if (maxCurrentValues[selectedCurrentIndex] > 5000.0) maxCurrentValues[selectedCurrentIndex] = 5000.0;
    displayCurrentValue();
  } else if(inOverCurrentMode){
    if(direction > 0){
      runMotor(speed, 1);
      delay(200);
      runMotor(0);
    }
  }
}

void selectMenu() {
  if (inHomeScreen) {
    if (currentMenuIndex == 0) {
      currentMenuIndex = 0;
      selectedUlirIndex = -1;
      displayUlirSelection();
    } else if (currentMenuIndex == 1) {
      currentMenuIndex = 0;
      selectedCurrentIndex = -1;
      displayCurrentSettings();
    }
  } else if (inUlirSelection) {
    selectedUlirIndex = currentMenuIndex;
    displayUlirDetail();
  } else if (inCurrentSettingsMode) {
    selectedCurrentIndex = currentMenuIndex;
    displayCurrentValue();
  } else if (inCurrentValueMode) {
    // Menyimpan nilai arus yang telah diubah
    saveCurrentValuesToEEPROM();
    
    // Menampilkan pesan konfirmasi
    lcd.clear();
    lcd.setCursor(3, 1);
    lcd.print("DATA DISIMPAN!");
    delay(1500);
    
    displayCurrentValue();
  } else if (inUlirDetailMode) {
    // Tombol OK pada layar detail ulir menjalankan/menghentikan motor
    if (!motorRunning) {
      runMotor(speed); // jalankan motor
      displayUlirDetail();
    } else {
      runMotor(0);  // stop motor
      isReverse = 0;
      displayUlirDetail();
    }
  }
  else if(inOverCurrentMode){
    if (!motorRunning){
      runMotor(speed, 1);
      delay(10);
      runMotor(0);
    }
    else{
      runMotor(0);
    }
  }
}

void backMenu() {
  if (motorRunning) {
    runMotor(0); // stop motor
  }
  
  if (inCurrentValueMode) {
    selectedCurrentIndex = -1;
    displayCurrentSettings();
  } else if (inCurrentSettingsMode) {
    currentMenuIndex = 0;
    displayHomeScreen();
  } else if (inUlirDetailMode) {
    currentMenuIndex = selectedUlirIndex;
    displayUlirSelection();
  } else if (inUlirSelection) {
    currentMenuIndex = 0;
    displayHomeScreen();
  }
  else if (inOverCurrentMode){
    currentMenuIndex = selectedUlirIndex;
    displayUlirDetail();
  }
}

void displayUlirDetail() {
  lcd.clear();
  inOverCurrentMode = false;
  inUlirDetailMode = true;
  inUlirSelection = false;
  
  lcd.setCursor(1, 0);
  lcd.print("ULIR ");
  lcd.print(ulirOptions[selectedUlirIndex]);
  lcd.print(" - ");
  if(!motorRunning){
    lcd.print("READY");
  } else {
    lcd.print("RUNNING");
  }
  
  lcd.setCursor(0, 1);
  lcd.print("Arus Maks: ");
  lcd.print(maxCurrentValues[selectedUlirIndex], 1);
  lcd.print("mA");
  
  lcd.setCursor(0, 2);
  lcd.print("Arus:");
  lcd.print(ina219.getCurrent_mA());
  lcd.print("mA");
  

  lcd.setCursor(13, 3);
  lcd.print(motorRunning ? "OK: STP" : " OK: GO");
}