#include <HX711_ADC.h>
#include <EEPROM.h>
#include <Servo.h>

// Pin definisi
const int HX711_dout = 4; // Pin data dari HX711
const int HX711_sck = 5;  // Pin clock dari HX711
const int servoPin1 = 9;  // Pin servo 1
const int servoPin2 = 10; // Pin servo 2
const int servoPin3 = 11; // Pin servo 3

// Objek load cell dan servo
HX711_ADC LoadCell(HX711_dout, HX711_sck);
Servo servo1;
Servo servo2;
Servo servo3;

// Variabel load cell dan servo
const int calVal_eepromAdress = 0;
unsigned long lastDisplayTime = 0;
const unsigned long displayInterval = 2500; // Interval 2.5 detik
float lastWeight = 0; // Menyimpan berat terakhir untuk pengecekan perubahan

void setup() {
  Serial.begin(57600);
  delay(10);
  Serial.println("Starting...");

  LoadCell.begin();
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  
  // Mengatur posisi awal servo
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);

  unsigned long stabilizingtime = 2000; // Waktu stabilisasi
  boolean _tare = true; // Melakukan tare pada awal
  LoadCell.start(stabilizingtime, _tare);
  
  if (LoadCell.getTareTimeoutFlag() || LoadCell.getSignalTimeoutFlag()) {
    Serial.println("Timeout, check MCU>HX711 wiring and pin designations");
    while (1);
  } else {
    LoadCell.setCalFactor(1.0); // Set nilai kalibrasi awal
    Serial.println("Startup is complete");
  }
  while (!LoadCell.update());
  calibrate(); // Memulai prosedur kalibrasi
}

void loop() {
  static boolean newDataReady = 0;

  if (LoadCell.update()) newDataReady = true;

  // Cek jika 2.5 detik telah berlalu untuk menampilkan data berat di Serial Monitor
  if (newDataReady && millis() - lastDisplayTime >= displayInterval) {
    float weight = LoadCell.getData();
    Serial.print("Load_cell output val: ");
    Serial.println(weight);
    lastDisplayTime = millis();

    // Hanya menggerakkan servo jika berat berubah dan sesuai dengan rentang
    if (weight != lastWeight) {
      moveServoBasedOnWeight(weight);
      lastWeight = weight; // Update berat terakhir
    }
  }

  // Mengecek status tare
  if (LoadCell.getTareStatus() == true) {
    Serial.println("Tare complete");
  }
}

void moveServoBasedOnWeight(float weight) {
  if (weight >= 1.0 && weight < 2.0) {
    activateServo(servo1);
  } else if (weight >= 2.0 && weight < 3.0) {
    activateServo(servo2);
  } else if (weight >= 3.0 && weight < 4.0) {
    activateServo(servo3);
  } else {
    // Jika berat tidak sesuai dengan rentang, tidak ada servo yang bergerak
    resetServos();
  }
}

void activateServo(Servo &servo) {
  resetServos();      // Mengembalikan semua servo ke posisi awal
  servo.write(180);   // Menggerakkan servo ke 180 derajat
  delay(2000);        // Menunggu 2 detik
  servo.write(0);     // Mengembalikan servo ke posisi awal (0 derajat)
}

void resetServos() {
  // Mengembalikan semua servo ke posisi awal
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
}

void calibrate() {
  Serial.println("***");
  Serial.println("Start calibration:");
  Serial.println("Place the load cell on a level stable surface.");
  Serial.println("Remove any load applied to the load cell.");
  Serial.println("Send 't' from serial monitor to set the tare offset.");

  boolean _resume = false;
  while (_resume == false) {
    LoadCell.update();
    if (Serial.available() > 0) {
      char inByte = Serial.read();
      if (inByte == 't') LoadCell.tareNoDelay();
    }
    if (LoadCell.getTareStatus() == true) {
      Serial.println("Tare complete");
      _resume = true;
    }
  }

  Serial.println("Now, place your known mass on the loadcell.");
  Serial.println("Then send the weight of this mass (i.e. 100.0) from serial monitor.");

  float known_mass = 0;
  _resume = false;
  while (_resume == false) {
    LoadCell.update();
    if (Serial.available() > 0) {
      known_mass = Serial.parseFloat();
      if (known_mass != 0) {
        Serial.print("Known mass is: ");
        Serial.println(known_mass);
        _resume = true;
      }
    }
  }

  LoadCell.refreshDataSet();
  float newCalibrationValue = LoadCell.getNewCalibration(known_mass);

  Serial.print("New calibration value has been set to: ");
  Serial.print(newCalibrationValue);
  Serial.println(", use this as calibration value (calFactor) in your project sketch.");
  Serial.print("Save this value to EEPROM address ");
  Serial.print(calVal_eepromAdress);
  Serial.println("? y/n");

  _resume = false;
  while (_resume == false) {
    if (Serial.available() > 0) {
      char inByte = Serial.read();
      if (inByte == 'y') {
        EEPROM.put(calVal_eepromAdress, newCalibrationValue);
        EEPROM.get(calVal_eepromAdress, newCalibrationValue);
        Serial.print("Value ");
        Serial.print(newCalibrationValue);
        Serial.print(" saved to EEPROM address: ");
        Serial.println(calVal_eepromAdress);
        _resume = true;
      }
      else if (inByte == 'n') {
        Serial.println("Value not saved to EEPROM");
        _resume = true;
      }
    }
  }

  Serial.println("End calibration");
  Serial.println("***");
  Serial.println("To re-calibrate, send 'r' from serial monitor.");
  Serial.println("For manual edit of the calibration value, send 'c' from serial monitor.");
  Serial.println("***");
}