#include <HX711.h>
#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Pin HX711
#define DOUT 2
#define CLK 3
HX711 scale;

// Servo
Servo servo1; // pengangkat
Servo servo2; // box 1: 1-3 kg
Servo servo3; // box 2: 3-5 kg
Servo servo4; // tidak digunakan, tetap ditambahkan

// LCD I2C: Alamat bisa 0x27 atau 0x3F
LiquidCrystal_I2C lcd(0x27, 16, 2); 

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

  // Inisialisasi HX711
  scale.begin(DOUT, CLK);
  delay(500);  // beri waktu HX711 stabil

  // Attach Servo ke pin sesuai gambar
  servo1.attach(11);  // pengangkat awal
  servo2.attach(10);  // box 1
  servo3.attach(9);  // box 2
  servo4.attach(6);  // box 3 (tidak digunakan di logika)

  // Atur posisi awal semua servo ke 90°
  servo1.write(0);
  servo2.write(90);
  servo3.write(90);
  servo4.write(90);

  // Inisialisasi LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Konveyor Ready");
  delay(2000);
  lcd.clear();
}

void loop() {
  if (scale.is_ready()) {
    long nilai_mentah = scale.read();
    float berat = nilai_mentah / 420.0;  // tanpa kalibrasi, kira-kira konversi ke kg

    // Tampilkan berat di Serial dan LCD
    Serial.print("Berat: ");
    Serial.print(berat, 2);
    Serial.println(" kg");

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Berat:");
    lcd.setCursor(7, 0);
    lcd.print(berat, 2);
    lcd.print("kg");

    //if (berat > 0.1) { // jika terdeteksi ada barang
      //servo1.write(0);  // angkat barang
      //delay(1000);
      //servo1.write(90); // kembali posisi semula
      //delay(1000);

      if (berat >= 1 && berat < 3) {
        servo1.write(90);  // angkat barang
        delay(1000);
        servo1.write(0); // kembali posisi semula
        delay(1000);
        servo2.write(0);
        delay(1000);
        servo2.write(90);
      } 
      else if (berat >= 3 && berat < 5) {
        servo1.write(90);  // angkat barang
        delay(1000);
        servo1.write(0); // kembali posisi semula
        delay(1000);
        servo3.write(0);
        delay(1000);
        servo3.write(90);
      }
      else if (berat >= 5) {
        servo1.write(90);  // angkat barang
        delay(1000);
        servo1.write(0); // kembali posisi semula
        delay(1000);
        servo4.write(0);
        delay(1000);
        servo4.write(90);
      }

      delay(2000); // jeda sebelum loop ulang

   // }

  } else {
    Serial.println("Sensor tidak siap");
    lcd.setCursor(0, 1);
    lcd.print("HX711 error");
  }

  delay(500);
}