#include <AccelStepper.h>
#include "HX711.h"

// === PIN SETUP ===
#define STEP_PIN_X 5
#define DIR_PIN_X 4
#define STEP_PIN_Z 3
#define DIR_PIN_Z 2

AccelStepper stepperX(AccelStepper::DRIVER, STEP_PIN_X, DIR_PIN_X);
AccelStepper stepperZ(AccelStepper::DRIVER, STEP_PIN_Z, DIR_PIN_Z);

#define LOADCELL_DOUT_PIN 8
#define LOADCELL_SCK_PIN 9
HX711 scale;

#define RELAY_PIN 11

// === KONFIGURASI ===
const float targetPressure = 3.0;         // Tekanan target (kg)
const float upDistance = 10.0;            // Naik 10 mm setelah welding
const float jarakAntarTitik = 20.0;       // Jarak antar titik (mm)
const int stepsPerMM_X = 80;              // Step per mm sumbu X
const int stepsPerMM_Z = 80;              // Step per mm sumbu Z
const unsigned long weldingTime = 5000;   // Durasi welding (ms)
const float faktorKalibrasi = 84.0;       // Kalibrasi loadcell

// === STATUS ===
int titikAktif = 0;
const int jumlahTitik = 3;
bool tekananTercapai = false;
bool weldingSelesai = false;

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

  stepperX.setMaxSpeed(1500);
  stepperX.setAcceleration(500);
  stepperX.setCurrentPosition(0);

  stepperZ.setMaxSpeed(1500);
  stepperZ.setAcceleration(500);
  stepperZ.setCurrentPosition(0);

  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
  scale.set_scale(faktorKalibrasi);
  scale.tare();

  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);
}

void loop() {
  if (titikAktif < jumlahTitik) {
    // === LANGKAH 1: Pindah ke titik X ===
    long posisiX = titikAktif * jarakAntarTitik * stepsPerMM_X;
    stepperX.moveTo(posisiX);
    while (stepperX.distanceToGo() != 0) {
      stepperX.run();
    }
    Serial.print(">> Pindah ke titik ke-");
    Serial.println(titikAktif + 1);

    // === LANGKAH 2: Reset Z dan mulai turun ===
    tekananTercapai = false;
    weldingSelesai = false;
    stepperZ.setCurrentPosition(0);
    stepperZ.moveTo(-10000); // Turun jauh
    scale.tare(); // Bersihkan pembacaan
    delay(200);   // Stabilkan

    while (!tekananTercapai) {
      float tekanan = scale.get_units(3);
      Serial.print("Tekanan: ");
      Serial.println(tekanan, 2);

      if (tekanan >= targetPressure) {
        stepperZ.setCurrentPosition(0);
        stepperZ.moveTo(0);
        tekananTercapai = true;
        Serial.println(">> Tekanan tercapai.");
        delay(100);
      } else {
        stepperZ.run();
      }
    }

    // === LANGKAH 3: Welding ===
    Serial.println(">> Aktifkan welding.");
    digitalWrite(RELAY_PIN, HIGH);
    delay(weldingTime);
    digitalWrite(RELAY_PIN, LOW);
    Serial.println(">> Welding selesai.");

    // === LANGKAH 4: Naikkan Z ===
    long naikStep = upDistance * stepsPerMM_Z;
    stepperZ.moveTo(stepperZ.currentPosition() + naikStep);
    while (stepperZ.distanceToGo() != 0) {
      stepperZ.run();
    }

    Serial.println(">> Motor Z naik. Titik selesai.\n");
    titikAktif++; // Lanjut ke titik berikutnya
    delay(500);
  } else {
    Serial.println(">> Semua titik selesai.");
    while (1); // Program berhenti
  }
}