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

// === Pin definisi ===
#define TRIG_PIN       PA8
#define ECHO_PIN       PA9
#define HX711_DT       PB12
#define HX711_SCK      PB13
#define PIR_PIN        PB5
#define RELAY_MOTOR    PB6   // LED indikator benda terdeteksi
#define RELAY_BUZZER   PB7   // buzzer jika berat tidak sesuai
#define SERVO_PIN      PB10  // servo

HX711 scale;
Servo conveyorServo;

float berat = 0;
int botolCount = 0;

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

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(PIR_PIN, INPUT);
  pinMode(RELAY_MOTOR, OUTPUT);
  pinMode(RELAY_BUZZER, OUTPUT);

  digitalWrite(RELAY_MOTOR, LOW);
  digitalWrite(RELAY_BUZZER, LOW);

  // Inisialisasi Load Cell
  scale.begin(HX711_DT, HX711_SCK);
  scale.set_scale(); // Kalibrasi sesuai kebutuhan
  scale.tare();
  Serial.println("Load cell siap...");

  // Servo
  conveyorServo.attach(SERVO_PIN);
  conveyorServo.write(90); // posisi awal netral
  Serial.println("Servo siap di posisi 90 derajat.");

  Serial.println("Sistem Siap...");
}

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

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

  Serial.print("Jarak terdeteksi: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance < 10.0) {
    botolCount++;
    Serial.print(">> Botol ke-");
    Serial.println(botolCount);

    // Hidupkan LED via relay motor
    digitalWrite(RELAY_MOTOR, HIGH);
    Serial.println("Relay motor AKTIF - benda terdeteksi");

    delay(1000); // jeda saat benda berhenti di atas load cell

    // 2. Baca berat
    berat = scale.get_units(10);
    Serial.print("Berat terdeteksi: ");
    Serial.print(berat);
    Serial.println(" gram");

    // 3. Cek berat
    if (berat < 100 || berat > 200) {
      Serial.println(">> Berat TIDAK SESUAI. Buzzer AKTIF.");
      digitalWrite(RELAY_BUZZER, HIGH);
      delay(1000);
      digitalWrite(RELAY_BUZZER, LOW);
      Serial.println("Buzzer OFF");

      // Tetap cek PIR
      int pir = digitalRead(PIR_PIN);
      Serial.print("Status PIR: ");
      Serial.println(pir == HIGH ? "DETEKSI OBJEK" : "TIDAK TERDETEKSI");

      if (pir == HIGH) {
        Serial.println(">> PIR mendeteksi gerakan. Servo bergerak ke 45 derajat.");
        conveyorServo.write(45);
        delay(1500);
        conveyorServo.write(90);
        Serial.println("Servo kembali ke posisi 90 derajat.");
        delay(500);
      } else {
        Serial.println(">> PIR tidak mendeteksi apa pun setelah buzzer.");
      }

    } else {
      // Berat sesuai → cek PIR
      int pir = digitalRead(PIR_PIN);
      Serial.print("Status PIR: ");
      Serial.println(pir == HIGH ? "DETEKSI OBJEK" : "TIDAK TERDETEKSI");

      if (pir == HIGH) {
        Serial.println(">> Berat SESUAI & PIR AKTIF. Servo bergerak ke 135 derajat.");
        conveyorServo.write(135);
        delay(1500);
        conveyorServo.write(90);
        Serial.println("Servo kembali ke posisi 90 derajat.");
        delay(500);
      } else {
        Serial.println(">> Berat sesuai, tapi PIR tidak mendeteksi objek.");
      }
    }

    // Matikan LED indikator benda terdeteksi
    digitalWrite(RELAY_MOTOR, LOW);
    Serial.println("Relay motor NONAKTIF - proses selesai.");
    Serial.println("-------------------------------");
    delay(2000);
  }

  delay(200);
}