#include <LiquidCrystal.h>

LiquidCrystal lcd(A5, A4, A3, A2, A1, A0);

#define dirPin 2
#define stepPin 3

#define dirPin2 6
#define stepPin2 7

#define TRIG_1 12
#define ECHO_1 11

#define TRIG_2 9
#define ECHO_2 8

#define PUMP 4

int state = 0;

bool obstacle_detected(uint8_t trig_pin, uint8_t echo_pin) {
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(5);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);
  int duration = pulseIn(echo_pin, HIGH);
  int distance = (duration / 2) / 29.1;

  if (distance < 30)
    return (true);

  return (false);
}

void setup() {
  // Declare pins as Outputs
  lcd.begin(16, 2);
  pinMode(PUMP, OUTPUT);
  pinMode(TRIG_1, OUTPUT);
  pinMode(ECHO_1, INPUT);

  pinMode(TRIG_2, OUTPUT);
  pinMode(ECHO_2, INPUT);

  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
}
void loop() {
  // Set motor direction clockwise
  digitalWrite(dirPin, HIGH);
  digitalWrite(dirPin2, HIGH);

  if (state == 0) {

    digitalWrite(stepPin, HIGH);
    // digitalWrite(stepPin2, HIGH);
    delayMicroseconds(2000);
    digitalWrite(stepPin, LOW);
    // digitalWrite(stepPin2, LOW);
    delayMicroseconds(2000);
    lcd.setCursor(0, 0);
    lcd.print(F("Moving"));

    if (obstacle_detected(TRIG_1, ECHO_1)) {
      state = 1;
      lcd.clear();
    }
  }

  if (state == 1) {
    lcd.setCursor(0, 0);
    lcd.print(F("Pumping"));
    digitalWrite(PUMP, HIGH);
    delay(5000);
    digitalWrite(PUMP, LOW);
    state = 2;
    lcd.clear();
  }

  if (state == 2) {
    lcd.setCursor(0, 0);
    lcd.print(F("Moving"));
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(2000);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(2000);

    if (obstacle_detected(TRIG_2, ECHO_2)) {
      state = 0;
      lcd.clear();
    }
  }
}