const int stepPins[] = {8, 10, 12, 5}; // ขาขับการเคลื่อนของมอเตอร์
const int dirPins[] = {9, 11, 13, 6}; // ขาควบคุมทิศทางการเคลื่อนของมอเตอร์
int steps[] = {0, 0, 0, 0}; // จำนวนขั้นตอนที่มอเตอร์หมุนไปแล้ว
const int maxSteps[] = {1400 , 1000}; // จำนวนขั้นตอนสูงสุดที่มอเตอร์ต้องหมุน
const int delayTime = 500; // เวลาหน่วงระหว่างการเคลื่อนที่ของมอเตอร์ (ในไมโครวินาที)
const int trigPin = 2;
const int echoPin = 3;
const int raspberryPi = 4;

void setup() {
  for(int i = 0; i < 4; i++) {
    pinMode(stepPins[i], OUTPUT);
    pinMode(dirPins[i], OUTPUT);
  }

  pinMode(raspberryPi, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
}

void loop() {

  /////////////////////////// 1 /////////////////////////////

   for(int i = 0; i < maxSteps[0]; i++) {
      moveMotor(stepPins[0], dirPins[0], false, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], false, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], false, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], true, 1, delayTime);
    }
    delay(3000);
    
    for(int i = 0; i < maxSteps[0]; i++) {
      moveMotor(stepPins[0], dirPins[0], true, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], true, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], true, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], false, 1, delayTime);
    }

    delay(3000);

///////////////////////////////// 2 ///////////////////////////

    for(int i = 0; i < maxSteps[0]; i++) {
      moveMotor(stepPins[0], dirPins[0], true, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], false, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], false, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], false, 1, delayTime);
    }

    delay(3000);

    for(int i = 0; i < maxSteps[0]; i++) {
      moveMotor(stepPins[0], dirPins[0], false, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], true, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], true, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], true, 1, delayTime);
    }

    delay(3000);


/////////////////////////////// 3 ////////////////////////////

    for(int i = 0; i < maxSteps[0]; i++) {
      moveMotor(stepPins[0], dirPins[0], false, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], true, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], false, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], false, 1, delayTime);
    }

    delay(3000);

    for(int i = 0; i < maxSteps[0]; i++) {
      moveMotor(stepPins[0], dirPins[0], true, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], false, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], true, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], true, 1, delayTime);
    }

    delay(3000);


/////////////////////////////// 4 ////////////////////////////
    
    for(int i = 0; i < maxSteps[0]; i++) {
      moveMotor(stepPins[0], dirPins[0], false, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], false, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], true, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], false, 1, delayTime);
    }

    delay(3000);

    for(int i = 0; i < maxSteps[0]; i++) {
      moveMotor(stepPins[0], dirPins[0], true, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], true, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], false, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], true, 1, delayTime);
    }

    delay(3000);


  /////////////////////////// 1 /////////////////////////////

    for(int i = 0; i < maxSteps[1]; i++) {
      moveMotor(stepPins[0], dirPins[0], true, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], false, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], false, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], true, 1, delayTime);
    }
    delay(3000);
    
    for(int i = 0; i < maxSteps[1]; i++) {
      moveMotor(stepPins[0], dirPins[0], false, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], true, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], true, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], false, 1, delayTime);
    }

    delay(3000);

    //////////////////////////// 2 //////////////////////////

    for(int i = 0; i < maxSteps[1]; i++) {
      moveMotor(stepPins[0], dirPins[0], false, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], true, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], true, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], false, 1, delayTime);
    }
    delay(3000);
    
    for(int i = 0; i < maxSteps[1]; i++) {
      moveMotor(stepPins[0], dirPins[0], true, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], false, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], false, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], true, 1, delayTime);
    }

    delay(3000);

    //////////////////////////// 3 //////////////////////////

    for(int i = 0; i < maxSteps[1]; i++) {
      moveMotor(stepPins[0], dirPins[0], true, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], true, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], false, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], false, 1, delayTime);
    }
    delay(3000);
    
    for(int i = 0; i < maxSteps[1]; i++) {
      moveMotor(stepPins[0], dirPins[0], false, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], false, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], true, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], true, 1, delayTime);
    }

    delay(3000);

    //////////////////////////// 4 //////////////////////////

    for(int i = 0; i < maxSteps[1]; i++) {
      moveMotor(stepPins[0], dirPins[0], false, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], false, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], true, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], true, 1, delayTime);
    }
    delay(3000);
    
    for(int i = 0; i < maxSteps[1]; i++) {
      moveMotor(stepPins[0], dirPins[0], true, 1, delayTime);
      moveMotor(stepPins[1], dirPins[1], true, 1, delayTime);
      moveMotor(stepPins[2], dirPins[2], false, 1, delayTime);
      moveMotor(stepPins[3], dirPins[3], false, 1, delayTime);
    }

    delay(3000);
}

void moveMotor(int stepPin, int dirPin, bool clockwise, int steps, int delayTime) {
  float distance = readDistanceCM();
  int raspPi = digitalRead(raspberryPi);
  Serial.println(distance);
  Serial.println(raspPi);

  while (distance < 20 || raspPi != HIGH) {
    delay(100);
    distance = readDistanceCM();
    raspPi = digitalRead(raspberryPi);
  }
  
  if (distance >= 20) {
    digitalWrite(dirPin, clockwise ? HIGH : LOW);
    for (int x = 0; x < steps; x++) {
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(delayTime);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(delayTime);
    }
  }
}



float readDistanceCM() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  int duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}