#include <Wire.h>               // Library untuk komunikasi I2C
#include <LiquidCrystal_I2C.h>  // Library untuk LCD I2C
#include <HX711.h>              // Library untuk load cell (HX711)
#include <Servo.h>              // Library untuk servo motor

#define SECOND_SERVO_PIN 11     // Pin servo motor kedua
#define DOUT  3                 // Pin data HX711
#define CLK   2                 // Pin clock HX711
#define RELAY_PIN 4             // Pin untuk relay
#define LDR_PIN A0              // Pin sensor LDR
#define IR_SENSOR_PIN 5         // Pin sensor IR (digital)
#define START_BUTTON_PIN 6      // Pin tombol push untuk memulai program
#define STOP_BUTTON_PIN 7       // Pin tombol push untuk menghentikan program
#define SERVO_PIN 9             // Pin servo motor

HX711 scale;                    // Inisialisasi objek scale
Servo servoMotor;               // Inisialisasi objek servo
Servo secondServoMotor;         // Inisialisasi objek servo kedua
LiquidCrystal_I2C lcd(0x27, 16, 2); // Inisialisasi objek LCD 16x2 dengan alamat I2C 0x27

const int MAX_COUNT = 100;     // Ukuran maksimum array
int relayCountArray[MAX_COUNT]; // Array untuk menyimpan nilai relay count
float weightArray[MAX_COUNT];   // Array untuk menyimpan nilai weight
int ldrArray[MAX_COUNT];
unsigned long servoMoveTimeArray[MAX_COUNT]; // Array untuk menyimpan waktu pergerakan servo
int arrayIndex = 0;             // Indeks array untuk relay count
int weightIndex = 0;            // Indeks array untuk weight
int ldrIndex = 0;               // Indeks array untuk ldr

int relayCycleCount = 0;            // Counter untuk jumlah siklus relay yang telah dijalankan
unsigned long previousRelayMillis = 0; // Waktu sebelumnya untuk mengaktifkan relay
const unsigned long relayInterval = 5000; // Interval waktu untuk mengaktifkan relay (5 detik)
bool programStarted = false;        // Status apakah program sudah dimulai atau belum
bool startMessageDisplayed = false; // Status apakah pesan "Press the red button to start the program" sudah ditampilkan atau belum
bool stopMessageDisplayed = false;  // Status apakah pesan "Program stopped." sudah ditampilkan atau belum

unsigned long previousServoMillis = 0; // Waktu sebelumnya saat servo terakhir bergerak
const unsigned long servoInterval = 100; // Interval waktu untuk menggerakkan servo (0.1 detik)

int servoMoveCount = 0;
int secondservoMoveCount = 0;

void setup() {
  Serial.begin(9600);         // Inisialisasi komunikasi serial
  lcd.begin(16, 2);           // Inisialisasi LCD 16x2
  lcd.backlight();
  scale.begin(DOUT, CLK);     // Inisialisasi HX711
  pinMode(RELAY_PIN, OUTPUT); // Mengatur pin relay sebagai output
  pinMode(LDR_PIN, INPUT);    // Mengatur pin sensor LDR sebagai input
  pinMode(IR_SENSOR_PIN, INPUT); // Mengatur pin sensor IR sebagai input
  pinMode(START_BUTTON_PIN, INPUT_PULLUP); // Mengatur pin tombol push untuk memulai program sebagai input dengan pull-up resistor
  pinMode(STOP_BUTTON_PIN, INPUT_PULLUP);  // Mengatur pin tombol push untuk menghentikan program sebagai input dengan pull-up resistor
  servoMotor.attach(SERVO_PIN); // Mengatur pin servo motor pertama
  secondServoMotor.attach(SECOND_SERVO_PIN); // Mengatur pin servo motor kedua
  servoMotor.write(0);
  secondServoMotor.write(0);
  digitalWrite(RELAY_PIN, HIGH);
  
  lcd.setCursor(0, 0);
  lcd.print("  TOMBOL MERAH ");
  lcd.setCursor(0, 1);
  lcd.print("      START      ");
}

void loop() {
  if (!startMessageDisplayed && digitalRead(START_BUTTON_PIN) == LOW) {
    startMessageDisplayed = true;
    lcd.clear(); // Membersihkan layar LCD
    programStarted = true;
  }

  if (programStarted && !stopMessageDisplayed && digitalRead(STOP_BUTTON_PIN) == LOW) {
    // Saat tombol stop ditekan, reset variabel yang berhubungan dengan status program
    programStarted = false;
    stopMessageDisplayed = true;
    startMessageDisplayed = false; // Reset pesan tombol merah
    relayCycleCount = 0;
    servoMoveCount = 0;
    secondservoMoveCount = 0;
    digitalWrite(RELAY_PIN, HIGH);
    servoMotor.write(0);
    secondServoMotor.write(0);
    lcd.clear(); // Membersihkan layar LCD
    lcd.setCursor(0, 0);
    lcd.print("    PROGRAM    ");
    lcd.setCursor(0, 1);
    lcd.print("    BERHENTI    ");

    Serial.println("Program stopped.");
    delay(1500);
  lcd.setCursor(0, 0);
  lcd.print("  TOMBOL MERAH ");
  lcd.setCursor(0, 1);
  lcd.print("     RESTART     ");
  }


  if (stopMessageDisplayed && digitalRead(STOP_BUTTON_PIN) == HIGH) {
    stopMessageDisplayed = false;
  }

  if (programStarted) {
    unsigned long currentMillis = millis();
      // Update LCD
      lcd.setCursor(0, 0);
      lcd.print(" COUNTER STATUS "); // Membersihkan baris kedua LCD
      lcd.setCursor(0, 1);
      lcd.print("Wk:");
      lcd.setCursor(6, 1);
      lcd.print(servoMoveCount);
      lcd.print("WB:");
      lcd.setCursor(12, 1);
      lcd.print(secondservoMoveCount);
      lcd.print("WB:");


    if (currentMillis - previousRelayMillis >= relayInterval) {
           // Jika interval tercapai, aktifkan relay, tambahkan counter, dan cetak counter
      digitalWrite(RELAY_PIN, LOW);
      previousRelayMillis = currentMillis;
      relayCycleCount++;
      Serial.print("Relay count: ");
      Serial.println(relayCycleCount);
    }

    // Jika belum mencapai interval, non-aktifkan relay
    else if (currentMillis - previousRelayMillis >= relayInterval - 2000) {
      digitalWrite(RELAY_PIN, HIGH);
    }

    if (currentMillis - previousServoMillis >= servoInterval) {
      static bool servoMovedThisCycle = false;
      static bool secondServoMovedThisCycle = false;
      // Periksa apakah sudah mendekati saat relay dimatikan
      if (currentMillis - previousRelayMillis >= relayInterval - 2000) {
        // Lakukan pergerakan servo hanya saat mendekati relay dimatikan
        for (int i = 0; i < weightIndex; i++) {
          if (weightArray[i] > 60 && relayCycleCount == relayCountArray[i] + 2 &&
              currentMillis - servoMoveTimeArray[i] >= relayInterval && !servoMovedThisCycle) {
            servoMotor.write(180);
            servoMovedThisCycle = true;
            servoMoveCount++;
          } else if (weightArray[i] < 60 && relayCycleCount == relayCountArray[i] + 3 &&
                     currentMillis - servoMoveTimeArray[i] >= relayInterval && !secondServoMovedThisCycle) {
            secondServoMotor.write(180);
            secondServoMovedThisCycle = true;
            secondservoMoveCount++;
          }
        }
      } else {
        // Jika belum mendekati saat relay dimatikan, posisikan servo ke 0
        servoMotor.write(0);
        secondServoMotor.write(0);
        servoMovedThisCycle = false;
        secondServoMovedThisCycle = false;
      }
      previousServoMillis = currentMillis;
    }

    int irValue = digitalRead(IR_SENSOR_PIN);
    if (irValue == LOW) {
      if (arrayIndex < MAX_COUNT) {
        relayCountArray[arrayIndex] = relayCycleCount;
        arrayIndex++;
      }

      float weight = scale.get_units();
      if (weightIndex < MAX_COUNT) {
        weightArray[weightIndex] = weight;
        weightIndex++;
      }

      int ldrValue = analogRead(LDR_PIN);
      if (ldrIndex < MAX_COUNT) {
        ldrArray[ldrIndex] = ldrValue;
        ldrIndex++;
      }

      for (int i = 0; i < ldrIndex; i++) {
        Serial.print("ldr terakhir saat IR aktif: ");
        Serial.print(i);
        Serial.print(": ");
        Serial.println(ldrArray[i]);
      }

      for (int i = 0; i < arrayIndex; i++) {
        Serial.print("Count terakhir saat IR aktif: ");
        Serial.print(i);
        Serial.print(": ");
        Serial.println(relayCountArray[i]);
      }

      //Serial.println ();//("Weight Array:");
      for (int i = 0; i < weightIndex; i++) {
        Serial.print("Weight terakhir saat IR aktif: ");
        Serial.print(i);
        Serial.print(": ");
        Serial.println(weightArray[i]);
      }
    }
  }
 }