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

// Pin definitions
const int stepPin = 3;
const int dirPin = 5;
const int loadCellDoutPin = 6;
const int loadCellSckPin = 7;
const int buttonPin = 8;
const int ldrDigitalPin = 11;
const int ldrAnalogPin = A0;
const int trigPin = 9;
const int echoPin = 10;

// Create objects
AccelStepper stepper1(AccelStepper::DRIVER, stepPin, dirPin);
HX711 scale;

int buttonState = 0;
int ldrDigitalValue;
int ldrAnalogValue;
float weight = 0.0;
bool moving = false;
long duration;
int distance;

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

  // Initialize pins
  pinMode(buttonPin, INPUT_PULLUP); // Internal pull-up resistor enabled
  pinMode(ldrDigitalPin, INPUT);
  pinMode(ldrAnalogPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Initialize stepper motor
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(500);

  // Initialize load cell
  scale.begin(loadCellDoutPin, loadCellSckPin);
  scale.set_scale(); // Use the value from calibration
  scale.tare();

  Serial.println("Initialization complete.");
}

void loop() {
  // Read button state
  buttonState = digitalRead(buttonPin);

  // Read LDR sensor values
  ldrDigitalValue = digitalRead(ldrDigitalPin);
  ldrAnalogValue = analogRead(ldrAnalogPin);

  // Read weight from load cell
  weight = scale.get_units(1);

  // Measure distance with ultrasonic sensor
  distance = measureDistance();

  if (buttonState == LOW) { // Button pressed, state is LOW
    // Check weight and decide path
    if (weight > 3.0) {
      Serial.println("Weight too high, not moving.");
      stopRobot();
    } else if (distance < 10) {
      Serial.println("Obstacle detected, stopping.");
      stopRobot();
    } else {
      Serial.print("Weight: ");
      Serial.println(weight);
      followLine();
    }
  } else {
    stopRobot();
  }

  delay(100);
}

void followLine() {
  // Follow line logic
  if (ldrDigitalValue == LOW) {
    Serial.println("Line detected");
    moveForward();
  } else {
    Serial.println("No line detected");
    stopRobot();
  }
}

void moveForward() {
  if (!moving) {
    stepper1.setSpeed(200);
    moving = true;
  }
  stepper1.runSpeed();
}

void stopRobot() {
  if (moving) {
    stepper1.setSpeed(0);
    moving = false;
  }
  stepper1.runSpeed();
}

int measureDistance() {
  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Sets the trigPin on HIGH state for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);

  // Calculating the distance
  distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)

  return distance;
}