#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <HX711_ADC.h>
#include <Keypad.h>
#include <Ultrasonic.h>

#define TRIGGER_PIN_1 14 // GPIO pin for first sensor's trigger
#define ECHO_PIN_1 15     // GPIO pin for first sensor's echo

#define TRIGGER_PIN_2 16  // GPIO pin for second sensor's trigger
#define ECHO_PIN_2 17     // GPIO pin for second sensor's echo

#define TRIGGER_PIN_3 A2// GPIO pin for second sensor's trigger
#define ECHO_PIN_3 A3 // GPIO pin for second sensor's echo

#define A_LED 18  // GPIO pin for second sensor's trigger
#define B_LED 19     // GPIO pin for second sensor's echo

#if defined(ESP8266)|| defined(ESP32) || defined(AVR)
#include <EEPROM.h>
#endif

Ultrasonic sensor1(TRIGGER_PIN_1, ECHO_PIN_1);
Ultrasonic sensor2(TRIGGER_PIN_2, ECHO_PIN_2);
Ultrasonic sensor3(TRIGGER_PIN_3, ECHO_PIN_3);

// Initialize Servos
Servo servoA;
Servo servoB;
Servo servoC;

// Initialize LCD
LiquidCrystal_I2C lcd(0x27, 20, 4);

//Inisialisasi Loadcell
//pins:
const int HX711_dout = A0; //mcu > HX711 dout pin
const int HX711_sck = A1; //mcu > HX711 sck pin
HX711_ADC LoadCell(HX711_dout, HX711_sck);

const int calVal_calVal_eepromAdress = 0;
unsigned long t = 0;
float i;
// Initialize Keypad
const int ROWS = 4;
const int COLS = 4;
bool lock;
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

byte colPins[COLS] = {5, 4, 3, 2};
byte rowPins[ROWS] = {9, 8, 7, 6};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Other Constants
const int servoAPin = 10;
const int servoBPin = 11;
const int servoCPin = 12;

// Variables
char selectedBeras = '\0';
float targetWeight = 0.0;
boolean isServoMoving = false;
boolean trigOut = false;
long distance_1;
long distance_2;
long distance_3;


// edited 
int shortcut;
int nilai_shortcut[] = {250, 500, 750, 1000};
char prev_state = 'A';
String input;


float readLoadCell() {
  static boolean newDataReady = 0;
  const int serialPrintInterval = 0; //increase value to slow down serial print activity
  // check for new data/start next conversion:
  if (LoadCell.update()) newDataReady = true;
  // get smoothed value from the dataset:
  if (newDataReady) {
    if (millis() > t + serialPrintInterval) {
      i = LoadCell.getData();
      Serial.print("Load_cell output val: ");
      Serial.println(i);
      newDataReady = 0;
      t = millis();
    Serial.print("Jarak C :");
    Serial.println(distance_3);
    }
  }
  return i;
}

void keypadEvent(KeypadEvent key){
  switch (keypad.getState())  {
  case PRESSED:
    switch(prev_state) {
    case 'A':
    if ((key == 'A' || key == 'B') && keypad.getState() == PRESSED) {
      if (distance_1 > 15 && key == 'A') {
        lcd.setCursor(3,2);
        lcd.print("Beras A Habis");
      } else if (distance_2 > 15 && key == 'B'){
        lcd.setCursor(3,2);
        lcd.print("Beras B Habis");
      } else {
      selectedBeras = key;
      lcd.setCursor(9,1);
      lcd.print(key);
      }
    } else if (key == '#' && selectedBeras != '\0') {
      lcd.clear();
      prev_state = 'B'; 
    }
    break;
    
    case 'B':
    if (key == '*') {
      input = "";
      lcd.setCursor(0,1);
      lcd.print("                    ");
      if (lock) {
        lcd.setCursor(0,2);
        lcd.print("                    ");
        lock = false;
      }
    } else if (key == '#') {
      if (input == "") {
        lcd.setCursor(0,1);
        lcd.print("Isi Berat Dahulu!");
        lcd.setCursor(0,2);
        lcd.print("Tekan * Untuk isi");
        lock = true;
      } else {
        lcd.clear();
        targetWeight = input.toInt();
        prev_state = 'C';
      } 
    } else if (key == 'A' && !lock) {
      input = nilai_shortcut[0];
      lcd.setCursor((20 - input.length()) / 2, 1);
      lcd.print(input);
    } else if (key == 'B' && !lock) {
      input = nilai_shortcut[1];
      lcd.setCursor((20 - input.length()) / 2, 1);
      lcd.print(input);
    } else if (key == 'C' && !lock) {
      input = nilai_shortcut[2];
      lcd.setCursor((20 - input.length()) / 2, 1);
      lcd.print(input);
    } else if (key == 'D' && !lock) {
      input = nilai_shortcut[3];
      lcd.setCursor((20 - input.length()) / 2, 1);
      lcd.print(input);
    } else if (!lock) {
      input += key;
      lcd.setCursor((20 - input.length()) / 2, 1);
      lcd.print(input);
    }
    break;

    case 'D':
      if (key == '*' && lock) {
        lock = false;
        lcd.clear();
      } else if (key == '#') {
        lcd.clear();
      }
    break;

    case 'E':
      if (key == '*') {
        input = "";
        lcd.setCursor(0,3);
        lcd.print("                    ");
      } else if (key == '#') {
        nilai_shortcut[shortcut - 1] = input.toInt();
        prev_state = 'A';
        input = "";
        lcd.clear();
      }else if (key != 'A' && key != 'B' && key != 'C' && key != 'D') {
        input += key;
        lcd.setCursor((20 - input.length() )/ 2, 3);
        lcd.print(input);
        }
    break;
    }
    case HOLD:
      if (prev_state == 'A' && keypad.getState() == HOLD && (key == 'A' || key == 'B' || key == 'C' || key == 'D')) {
        prev_state = 'E';
        lcd.clear();
        lcd.setCursor(5,0);
        lcd.print("Ubah Nilai");
        lcd.setCursor(4,1);
        lcd.print("Shortcut : ");
        lcd.print(key);
        lcd.setCursor(2,2);
        lcd.print("Masukkan Nilai : ");
        if (key == 'A')
          shortcut = 1;
        else if (key == 'B')
          shortcut = 2;
        else if (key == 'C')
          shortcut = 3;
        else if (key == 'D')
          shortcut = 4;
        
      }
    break;
  }
}  


void setup() {
  // Initialize Serial Communication
  Serial.begin(9600);
  keypad.setHoldTime(2000);
  keypad.addEventListener(keypadEvent); // Add an event listener for this keypad
  
  // Attach Servos
  servoA.attach(servoAPin);
  servoB.attach(servoBPin);
  servoC.attach(servoCPin);

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  
  servoA.write(0);
  servoB.write(0);
  servoC.write(0);

  LoadCell.begin();
  float calibrationValue; // calibration value (see example file "Calibration.ino")
  calibrationValue = 0.42; // uncomment this if you want to set the calibration value in the sketch ##########################################
  unsigned long stabilizingtime = 3000; // preciscion right after power-up can be improved by adding a few seconds of stabilizing time
  boolean _tare = true; //set this to false if you don't want tare to be performed in the next step
  LoadCell.start(stabilizingtime, _tare);
  if (LoadCell.getTareTimeoutFlag()) {
    Serial.println("Timeout, check MCU>HX711 wiring and pin designations");
    while (1);
  }
  else {
    LoadCell.setCalFactor(calibrationValue); // set calibration value (float)
    Serial.println("Startup is complete");
  }
}


void loop() {

char key = keypad.getKey();
distance_1 = sensor1.read();
distance_2 = sensor2.read();
distance_3 = sensor3.read();
distance_1 > 15 ? digitalWrite(A_LED, HIGH):digitalWrite(A_LED, LOW);
Serial.print("Jarak A : ");
Serial.println(distance_1);
distance_2 > 15 ? digitalWrite(B_LED, HIGH):digitalWrite(B_LED, LOW);
Serial.print("Jarak B :");
Serial.println(distance_2);

switch(prev_state) {
  case 'A': 

  lcd.setCursor(0,0);
  lcd.print("Pilih Beras (A/B) : ");
  break;
  
  case 'B':
  
  lcd.setCursor(0,0);
  lcd.print("Masukan Berat (gr) : ");
  break;

  case 'C':

  lcd.setCursor(7,0);
  lcd.print("Berat");
  lcd.setCursor(0,1);
  lcd.print("Target Berat : ");
  lcd.print(targetWeight, 0);
  lcd.setCursor(0,2);
  lcd.print("Proses :");
  break;

  case 'D':

  lcd.setCursor(5,0);
  lcd.print("Cek Wadah!");

  if (distance_3 < 5 && !lock) {
    lcd.setCursor(3,1);
    lcd.print("Wadah Terpasang");
    lcd.setCursor(0,2);
    lcd.print("Tekan # untuk Tuang!");
  } else {
    lcd.setCursor(0,1);
    lcd.print("Silakan pasang Wadah");
    lcd.setCursor(0,2);
    lcd.print("Tekan * untuk Lanjut");
    lock = true;
  }
  break;

  // case 'E':
  
  // break;


  }
  if(prev_state == 'C') {
    int i = 0;

    if (selectedBeras == 'A')
      servoA.write(35);
    else if (selectedBeras == 'B')
      servoB.write(35);

    while(readLoadCell() < targetWeight) {
    
    lcd.setCursor(13 + i, 2);
    lcd.print(readLoadCell());
    
  }
    servoA.write(0);
    servoB.write(0);
    lcd.clear();
    prev_state = 'D';
  } else if (prev_state == 'D' && key == '#') {
    lcd.clear();
    servoC.write(45);
    while(readLoadCell() > 0) {
      lcd.setCursor(4,0);
      lcd.print("Mohon tunggu");
      lcd.setCursor(6,1);
      lcd.print("Sebentar");
      lcd.setCursor(4,2);
      lcd.print("Berat :");
      lcd.setCursor(12, 2);
      lcd.print(readLoadCell());
      
    } // iki diganti nek wes dikalibrasi yo 
    prev_state = 'A';
    servoC.write(0);
    lcd.clear();
    selectedBeras = '\0';
    input = "";
    targetWeight=0;
 }
}