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


// Define HX711 pins
#define DOUT A1
#define CLK A0

// Define servo pins
#define SERVO_DISPENSE_HIGH 9
#define SERVO_DISPENSE_LOW 10
#define SERVO_BELT 11

// Define weight thresholds
#define MIN_WEIGHT 995
#define MAX_WEIGHT 1005

HX711 scale;
Servo servoDispenseHigh;
Servo servoDispenseLow;
Servo servoBelt;

void setup() {
  // Initialize serial communication for debugging
  Serial.begin(9600);
  
  // Initialize HX711
  scale.begin(DOUT, CLK);

  // Attach servos to pins
  servoDispenseHigh.attach(SERVO_DISPENSE_HIGH);
  servoDispenseLow.attach(SERVO_DISPENSE_LOW);
  servoBelt.attach(SERVO_BELT);
}

void loop() {
  // Move belt forward 1 step
  moveBeltForward();

  // Delay for chocolate to settle
  delay(500);

  // Read weight
  int weight = getWeight();

  // Check weight and take action
  if (weight < MIN_WEIGHT) {
    // Chocolate is too light, dispense to low container
    dispenseLow();
  } else if (weight > MAX_WEIGHT) {
    // Chocolate is too heavy, dispense to high container
    dispenseHigh();
  } else {
    // Chocolate is within acceptable range, continue production
    Serial.println("Chocolate within acceptable range.");
  }

  // Delay for stability before next iteration
  delay(1000);
}

void moveBeltForward() {
  servoBelt.write(180); // Rotate belt servo to move belt forward
  delay(1000); // Delay to allow belt to move
  servoBelt.write(0); // Rotate belt servo to stop
}

int getWeight() {
  // Read weight from load cell
  return scale.read();
}

void dispenseHigh() {
  // Rotate high dispense servo to dispense chocolate to high container
  servoDispenseHigh.write(180);
  delay(1000); // Delay to allow chocolate to be dispensed
  servoDispenseHigh.write(0); // Reset servo position
}

void dispenseLow() {
  // Rotate low dispense servo to dispense chocolate to low container
  servoDispenseLow.write(180);
  delay(1000); // Delay to allow chocolate to be dispensed
  servoDispenseLow.write(0); // Reset servo position
}