/*
PIN
- Ada sort 4 (A, B, C, D)
- 1000 gr maks 5000 gr (1 - 9)
- Reset 
- Deteksi Plastik tanpa tombol (Utras / photodioda)
- Keterangan Beras di penampung A & B


+--------+---------+
| KEYPAD | ARDUINO |
+--------+---------+
| R1     | D9      |
| R2     | D8      |
| R3     | D7      |
| R4     | D6      |
| C1     | D5      |
| C2     | D4      |
| C3     | D3      |
| C4     | D2      |
+--------+---------+
|SERVO A | ARDUINO | // UNTUK SERVO BERAS A
+--------+---------+
| ORANGE | D10     |
| COKLAT | GND     |
| MERAH  | VCC/5V  |
 +--------+---------+
|SERVO B | ARDUINO | // UNTUK SERVO BERAS B
+--------+---------+
| ORANGE | D11     |
| COKLAT | GND     |
| MERAH  | VCC/5V  |
+--------+---------+
|SERVO C | ARDUINO | // UNTUK SERVO BERAS DIPENAMPUNG
+--------+---------+
| ORANGE | D12     |
| COKLAT | GND     |
| MERAH  | VCC/5V  |
+--------+---------+
|ULTRSC1 | ARDUINO | // UNTUK ULTRASONIK BERAS A
+--------+---------+
| TRIGER | D14     |
| ECO    | D15     |
| GND    | GND     |
| VCC    | VCC/5V  |
+--------+---------+
|ULTRSC2 | ARDUINO | // UNTUK ULTRASONIK BERAS B
+--------+---------+
| TRIGER | D16     |
| ECO    | D17     |
| GND    | GND     |
| VCC    | VCC/5V  |
+--------+---------+
|LED A   | ARDUINO | // UNTUK LED INDIKATOR BERAS A
+--------+---------+
|PANJANG | D18     |
+--------+---------+
|LED B   | ARDUINO | // UNTUK LED INDIKATOR BERAS B
+--------+---------+
|PANJANG | D19     |
+--------+---------+
|LCD     | ARDUINO | // UNTUK LCD 20X4
+--------+---------+
|SDA     | D20/SDA |
|SCL     | D21/SCL |
| GND    | GND     |
| VCC    | VCC/5V  |
+--------+---------+
+--------+---------+
|MODUL LC| ARDUINO | // UNTUK MODUL LOADCELL KE ARDUINO
+--------+---------+
| DT     | A0      |
| SCK    | A1      |
| GND    | GND     |
| VCC    | VCC/5V  |
+--------+---------+
|LOADCELL| MODUL   | // UNTUK LOADCELL DAN MODUL
+--------+---------+
| MERAH  | E+      |
| HITAM  | E-      |
| PUTIH  | A-      |
| HIJAU  | A+      |
+--------+---------+
*/

#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <HX711.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

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 Load Cell
HX711 scale;

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

// 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;
const float calibration_factor = 500; // Calibration factor for load cell readings


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


float readLoadCell() {
  scale.set_scale(calibration_factor);
  float GRAM = scale.get_units();
  float grm =419.43 - GRAM*(-1);//float grm =419.43 - GRAM*(-1) +0.80 ;
  return grm * 1000;
}

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,1);
        lcd.print("Beras A Habis");
      } else if (distance_2 > 15 && key == 'B'){
        lcd.setCursor(3,1);
        lcd.print("Beras B Habis");
      } else {
      selectedBeras = key;
      lcd.setCursor(14,0);
      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();

  scale.begin(A0, A1);
  scale.set_scale();
  scale.tare();

  servoA.write(0);
  servoB.write(0);
  servoC.write(0);

}


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);
distance_2 > 15 ? digitalWrite(B_LED, HIGH):digitalWrite(B_LED, LOW);

switch(prev_state) {
	case 'A': 

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

	case 'C':

	lcd.setCursor(6,0);
	lcd.print("Weighing");
	lcd.setCursor(0,1);
	lcd.print("Target Weight : ");
	lcd.print(targetWeight, 0);
  lcd.setCursor(0,2);
  lcd.print("Processing");
	break;

	case 'D':

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

	if (distance_3 < 3 && !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(90);
    else if (selectedBeras == 'B')
      servoB.write(90);

    while(readLoadCell() < targetWeight) {
    Serial.println(readLoadCell());
    lcd.setCursor(10 + i, 2);
    lcd.print(".");
    i++;
    while(millis()%500 == 0); 
    if(i > 9 ) {
      i = 0;
      lcd.setCursor(10, 2);
      lcd.print("          ");
    }  
  }
    servoA.write(0);
    servoB.write(0);
    lcd.clear();
    prev_state = 'D';
  } else if (prev_state == 'D' && key == '#') {
    lcd.clear();
    servoC.write(90);
    while(readLoadCell() > 0) {
      lcd.setCursor(4,0);
      lcd.print("Mohon tunggu");
      lcd.setCursor(6,1);
      lcd.print("Sebentar");
    } // iki diganti nek wes dikalibrasi yo 
    prev_state = 'A';
    servoC.write(0);
    lcd.clear();
    selectedBeras = '\0';
    input = "";
    targetWeight = 0;
  }
}