//LINKS:
//https://randomnerdtutorials.com/esp32-stepper-motor-28byj-48-uln2003/
//https://components101.com/motors/28byj-48-stepper-motor

// PIN DETAILS:
// servo red gnd, brown vin, yellow 13

#include <HX711.h>
#include <Stepper.h>
#include <ESP32Servo.h>
#include <WiFi.h>
#include <BlynkSimpleEsp32.h>

#define BLYNK_TEMPLATE_ID "TMPL6ElwoiqVC"
#define BLYNK_TEMPLATE_NAME "Pensortir Buah"
char auth[] = "6PZ_MmZq6LV3v_mYheWhmYsxBjKUxyd5";
char ssid[] = "PunyaSahi";
char pass[] = "12345678";

const int PIN_LoadCell_DT = 35;
const int PIN_LoadCell_SCK = 18;
const int PIN_Stepper_An = 12;
const int PIN_Stepper_Ap = 14;
const int PIN_Stepper_Bp = 27;
const int PIN_Stepper_Bn = 26;
const int PIN_Servo = 13;

float kalibrasi = 419.8;

HX711 berat;
Stepper roda(200, PIN_Stepper_Bn, PIN_Stepper_Bp, PIN_Stepper_Ap, PIN_Stepper_An);
Servo palang;

void setup() {
    Serial.begin(115200);
    
    Serial.print("Connecting to Wi-Fi...");
    Blynk.begin(auth, ssid, pass);
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("\nConnected to Wi-Fi");

    berat.begin(PIN_LoadCell_DT, PIN_LoadCell_SCK);
    roda.setSpeed(50);
    palang.attach(PIN_Servo);
    palang.write(90);
}

void ensureBlynkConnection() {
    if (WiFi.status() != WL_CONNECTED) {
        Serial.println("Wi-Fi disconnected. Reconnecting...");
        WiFi.begin(ssid, pass);
        while (WiFi.status() != WL_CONNECTED) {
            delay(500);
            Serial.print(".");
        }
        Serial.println("Reconnected to Wi-Fi");
    }
    if (!Blynk.connected()) {
        Serial.println("Blynk disconnected. Reconnecting...");
        Blynk.connect();
    }
}

void loop() {
    ensureBlynkConnection();
    Blynk.run();

    static unsigned long lastActionTime = 0;
    static bool rodaBerjalan = true;
    static int stopState = 0;
    double nilaiBerat = berat.get_units(10) / kalibrasi;
    Blynk.virtualWrite(V0, nilaiBerat);

    unsigned long currentMillis = millis();
    if (rodaBerjalan) {
        roda.step(1);
        if (currentMillis - lastActionTime >= 2000) {
            rodaBerjalan = false;
            lastActionTime = currentMillis;
        }
    } else {
        if (stopState == 0 && currentMillis - lastActionTime >= 1000) {
            Serial.println("-------------------------");
            Serial.print("Berat: ");
            Serial.print(nilaiBerat);
            Serial.println(" gram");
            stopState = 1;
            lastActionTime = currentMillis;
        } else if (stopState == 1 && currentMillis - lastActionTime >= 0) {
            if (nilaiBerat > 5.0) {
                palang.write(180);
                Serial.println("Berat > 5gr, jalur kanan terbuka.");
                Blynk.virtualWrite(V1, "Jalur Kanan Terbuka");
            } else {
                palang.write(0);
                Serial.println("Berat < 5gr, jalur kiri terbuka.");
                Blynk.virtualWrite(V1, "Jalur Kiri Terbuka");
            }
            stopState = 2;
            lastActionTime = currentMillis;
        } else if (stopState == 2 && currentMillis - lastActionTime >= 1000) {
            rodaBerjalan = true;
            stopState = 0;
            lastActionTime = currentMillis;
        }
    }
}