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

// LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

// HX711 Load Cell
#define DOUT PA2
#define CLK  PA3
HX711 scale;

// Ultrasonik
#define TRIG_PIN PA8
#define ECHO_PIN PA9

// PIR
#define PIR_PIN PA1

// Buzzer
#define BUZZER_PIN PA0

// Servo
Servo conveyorServo;
#define SERVO_PIN PB10

int botolCount = 0;
float beratMin = 100.0; // Gram (threshold bawah)
float beratMax = 200.0; // Gram (threshold atas)

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();

  // Ultrasonik
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // PIR & Buzzer
  pinMode(PIR_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  // Servo
  conveyorServo.attach(SERVO_PIN);
  conveyorServo.write(0); // Posisi awal

  // HX711
  scale.begin(DOUT, CLK);
  scale.set_scale();    
  scale.tare(); // Kalibrasi awal

  lcd.setCursor(0, 0);
  lcd.print("Sistem Siap...");
  delay(1000);
  lcd.clear();
}

void loop() {
  // 1. Deteksi botol dengan ultrasonik
  long duration;
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH);
  float distance = duration * 0.034 / 2;

  if (distance < 10.0) { // Jika botol terdeteksi
    botolCount++;
    lcd.setCursor(0, 0);
    lcd.print("Botol ke-");
    lcd.print(botolCount);

    delay(1000); // waktu jeda botol berhenti di load cell

    // 2. Baca berat
    float berat = scale.get_units(10);
    lcd.setCursor(0, 1);
    lcd.print("Berat: ");
    lcd.print(berat, 1);
    lcd.print("g  ");

    if (berat < beratMin || berat > beratMax) {
      // Berat tidak sesuai → buzzer
      digitalWrite(BUZZER_PIN, HIGH);
      lcd.setCursor(0, 1);
      lcd.print("Berat TIDAK OK");
      delay(2000);
      digitalWrite(BUZZER_PIN, LOW);
    } else {
      // Berat sesuai → cek PIR
      int gerakan = digitalRead(PIR_PIN);
      if (gerakan == HIGH) {
        lcd.setCursor(0, 1);
        lcd.print("Berat OK. Kirim.");
        conveyorServo.write(90); // Gerakkan servo
        delay(1500);
        conveyorServo.write(0);  // Kembali
      } else {
        lcd.setCursor(0, 1);
        lcd.print("Tunggu PIR...   ");
      }
    }
    delay(1000);
    lcd.clear();
  }
}