#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_INA219.h>

// Inisialisasi INA219
Adafruit_INA219 ina219;

// Inisialisasi LCD I2C (alamat 0x27 dan ukuran 20x4)
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Pin kontrol motor
const int R_EN = 8;
const int L_EN = 9;
const int R_PWM = 10;
const int L_PWM = 11;

// Pin sinyal reverse
const int reverse = 2;

// Pin sinyal shutoff motor
const int shutoff = 3;

// Pin tombol push button
const int btnUp = 4;
const int btnDown = 5;
const int btnOk = 6;
const int btnBack = 7;

// Variabel untuk navigasi menu
int currentMenuIndex = 0;
int selectedMenuIndex = -1;
bool inSelection = false;
bool stopped = false;
bool inMainMenu = true;

// Variabel untuk kontrol motor
bool forward = true;
int speed = 0;

// Variabel untuk Baca Arus dan Threshold Stopper
int currentThreshold = 0;
float currentReading = 0;

// Daftar ulir
const char *ulirOptions[] = { "M6",    "M7", "M10",  "M11",   "M12", "M14"};
int ulirSpeed[]           = {   800,    700 ,   600,    500,    350 ,   250};
float ulirCurrent[]       = {9000.0, 9050.0, 9450.0, 9500.0, 9550.0, 9650.0};
const int ulirCount = sizeof(ulirOptions) / sizeof(ulirOptions[0]);

void setup() {
  Serial.begin(9600);

  // Inisialisasi LCD
  lcd.init();
  lcd.backlight();

  // Inisialisasi INA219
  //if (! ina219.begin()) {
  //  Serial.println("Failed to find INA219 chip");
  //  while (1) { delay(10); }
  //}

  // Menampilkan greeting
  lcd.setCursor(6, 0);
  lcd.print("TES ULIR");
  lcd.setCursor(0, 1);
  lcd.print("");
  lcd.setCursor(0, 2);
  lcd.print("");
  lcd.setCursor(5, 3);
  lcd.print("PT. TOSHIN");
  delay(1500);
  lcd.clear();

  // Inisialisasi tombol
  pinMode(btnUp, INPUT_PULLUP);
  pinMode(btnDown, INPUT_PULLUP);
  pinMode(btnOk, INPUT_PULLUP);
  pinMode(btnBack, INPUT_PULLUP);

  // Inisialisasi Pin Kontrol Motor
  pinMode(R_PWM, OUTPUT);
  pinMode(L_PWM, OUTPUT);
  pinMode(L_EN, OUTPUT);
  pinMode(R_EN, OUTPUT);

  // Inisialisasi pin input sensor
  pinMode(reverse, INPUT_PULLUP);

  // Inisialisasi pin output stopper
  pinMode(shutoff, OUTPUT);

  // Tampilan awal
  mainMenu();
}


int nowTime;


void loop() {
  speed = map(ulirSpeed[currentMenuIndex], 0, 800, 0, 255);
  currentThreshold = ulirCurrent[currentMenuIndex];
  // Cek arus motor
  currentReading = ina219.getCurrent_mA();
  if(millis() - nowTime >= 200){
    nowTime = millis();
    Serial.print("INA219 Current: ");
    Serial.println(currentReading);
  }
  
  //currentReading = 350.0;
  
  if(inSelection && currentReading >= currentThreshold){
    digitalWrite(shutoff, HIGH);
    stopped = 1;
    emergencyStop();
  }
  else{
    digitalWrite(shutoff, LOW);
  }

  if(digitalRead(reverse) == LOW){
    reverseMotor();
  }
  
  // Baca input tombol
  if (digitalRead(btnUp) == LOW) {
    navigateMenu(-1);
    delay(200); // Debounce
  } else if (digitalRead(btnDown) == LOW) {
    navigateMenu(1);
    delay(200); // Debounce
  } else if (digitalRead(btnOk) == LOW) {
    selectMenu();
    delay(200); // Debounce
  } else if (digitalRead(btnBack) == LOW) {
    backMenu();
    delay(200); // Debounce
  }
}

void reverseMotor(){
  forward = !forward;
  
  if(forward){
    digitalWrite(L_EN, LOW);
    digitalWrite(L_PWM, LOW);
    digitalWrite(R_EN, HIGH);
    //analogWrite(R_PWM, speed);
    digitalWrite(R_PWM, HIGH);
  }
  else{
    digitalWrite(R_EN, LOW);
    digitalWrite(R_PWM, LOW);
    digitalWrite(L_EN, HIGH);
    //analogWrite(L_PWM, speed);
    digitalWrite(L_PWM, HIGH);
  }
}

void emergencyStop(){
  if(stopped){
    stopped = 0;
    digitalWrite(R_EN, LOW);
    digitalWrite(L_EN, LOW);
    digitalWrite(R_PWM, LOW);
    digitalWrite(L_PWM, LOW);
    lcd.clear();
    lcd.setCursor(5, 0);
    lcd.print("MOTOR STOP");
    lcd.setCursor(5, 1);
    lcd.print("PRODUCT NG");
    lcd.setCursor(0, 2);
    lcd.print("[Back untuk Kembali]");
  }
}

void mainMenu(){
  lcd.clear();

  if(inMainMenu){
    lcd.setCursor(5, 0);
    lcd.print("PILIH OPSI");

    // tampilkan dua opsi
    lcd.setCursor(2, 1);
    lcd.print("1.Tes Ulir");
    lcd.setCursor(2, 2);
    lcd.print("2.Settings");

    // tampilkan cursor
    lcd.setCursor(0, currentMenuIndex);
    lcd.print("  ");
    lcd.setCursor(0, currentMenuIndex+1);
    lcd.print("->");
    lcd.setCursor(0, currentMenuIndex+2);
    lcd.print("  ");
  }
  else{

  }
}

void displayMenu() {
  lcd.clear();

  if(!inMainMenu){
    if (!inSelection) {
      lcd.setCursor(1, 0);
      lcd.print("PILIH UKURAN ULIR");

      // Tampilkan menu dengan highlight pada pilihan saat ini
      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]);
      }
    } else {
      lcd.setCursor(4, 0);
      lcd.print("ULIR DIPILIH");
      lcd.setCursor(9, 1);
      lcd.print(ulirOptions[selectedMenuIndex]);
      lcd.setCursor(0, 2);
      lcd.print("Threshold: ");
      lcd.print(int(ulirCurrent[selectedMenuIndex]));
      lcd.print(" mA");
      lcd.setCursor(1, 3);
      lcd.print("Back untuk Kembali");

      digitalWrite(L_EN, LOW);
      digitalWrite(L_PWM, LOW);
      digitalWrite(R_EN, HIGH);
      digitalWrite(R_PWM, HIGH);
      //analogWrite(R_PWM, speed);
    }
  }
}

void navigateMenu(int direction) {
  if (inMainMenu){
    currentMenuIndex -= direction;
    if (currentMenuIndex < 0){
      currentMenuIndex = 0;
    }
    else if (currentMenuIndex > 1){
      currentMenuIndex = 1;
    }
    mainMenu();
  }
  else if (!inMainMenu && !inSelection) {
    currentMenuIndex += direction;
    if (currentMenuIndex < 0) {
      currentMenuIndex = ulirCount - 1;
    } else if (currentMenuIndex >= ulirCount) {
      currentMenuIndex = 0;
    }
    displayMenu();
  }
}

void selectMenu() {
  if (!inSelection) {
    selectedMenuIndex = currentMenuIndex;
    inSelection = true;
    displayMenu();
  }
}

void backMenu() {
  if (inSelection) {
    inSelection = false;
    if(stopped == 1){
      stopped = false;
    }
    else{
      digitalWrite(R_EN, LOW);
      digitalWrite(L_EN, LOW);
      digitalWrite(R_PWM, LOW);
      digitalWrite(L_PWM, LOW);
    }
    displayMenu();
  }
  if(!inSelection && !inMainMenu){
    inMainMenu = true;
  }
}