// กำหนดพิน Ultrasonic Sensor
const int trigPin = 9;
const int echoPin = 10;

// กำหนดพินของ Stepper Motor ตัวที่ 1
const int motor1Pin1 = 2;
const int motor1Pin2 = 3;
const int motor1Pin3 = 4;
const int motor1Pin4 = 5;

// กำหนดพินของ Stepper Motor ตัวที่ 2
const int motor2Pin1 = 6;
const int motor2Pin2 = 7;
const int motor2Pin3 = 8;
const int motor2Pin4 = 11;

// ระยะตรวจจับเพื่อหยุดมอเตอร์ตัวที่ 1 (เซนติเมตร)
const int distanceThreshold = 2;

// ความเร็วของ Stepper Motor (หน่วงเวลาระหว่างแต่ละสเต็ป)
const int motorDelay = 5;

// Stepper Motor Sequence
int stepSequence[8][4] = {
  {1, 0, 0, 0},
  {1, 1, 0, 0},
  {0, 1, 0, 0},
  {0, 1, 1, 0},
  {0, 0, 1, 0},
  {0, 0, 1, 1},
  {0, 0, 0, 1},
  {1, 0, 0, 1}
};

void setup() {
  // ตั้งค่าพินของ Ultrasonic Sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // ตั้งค่าพินของ Stepper Motor 1
  pinMode(motor1Pin1, OUTPUT);
  pinMode(motor1Pin2, OUTPUT);
  pinMode(motor1Pin3, OUTPUT);
  pinMode(motor1Pin4, OUTPUT);

  // ตั้งค่าพินของ Stepper Motor 2
  pinMode(motor2Pin1, OUTPUT);
  pinMode(motor2Pin2, OUTPUT);
  pinMode(motor2Pin3, OUTPUT);
  pinMode(motor2Pin4, OUTPUT);

  // เริ่ม Serial Monitor
  Serial.begin(9600);
}

void loop() {
  // อ่านค่าระยะห่างจาก Ultrasonic Sensor
  long duration;
  int distance;

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2; // แปลงเวลาเป็นระยะทาง (เซนติเมตร)

  // แสดงระยะใน Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);

  if (distance == distanceThreshold) { 
    // หยุดมอเตอร์ตัวที่ 1
    stopMotor(motor1Pin1, motor1Pin2, motor1Pin3, motor1Pin4);

    // ให้มอเตอร์ตัวที่ 2 ทำงาน 5 วินาที
    unsigned long startTime = millis();
    while (millis() - startTime < 5000) {
      rotateMotor(motor2Pin1, motor2Pin2, motor2Pin3, motor2Pin4, 1);
    }

    // หยุดมอเตอร์ตัวที่ 2 หลังทำงาน 5 วินาที
    stopMotor(motor2Pin1, motor2Pin2, motor2Pin3, motor2Pin4);
  } else {
    // ให้มอเตอร์ตัวที่ 1 ทำงานปกติ
    rotateMotor(motor1Pin1, motor1Pin2, motor1Pin3, motor1Pin4, 1);

    // หยุดมอเตอร์ตัวที่ 2
    stopMotor(motor2Pin1, motor2Pin2, motor2Pin3, motor2Pin4);
  }

  delay(100); // หน่วงเวลาระหว่างการทำงาน
}

// ฟังก์ชันควบคุมการหมุนของ Stepper Motor
void rotateMotor(int pin1, int pin2, int pin3, int pin4, int steps) {
  for (int i = 0; i < steps; i++) {
    for (int step = 0; step < 8; step++) {
      digitalWrite(pin1, stepSequence[step][0]);
      digitalWrite(pin2, stepSequence[step][1]);
      digitalWrite(pin3, stepSequence[step][2]);
      digitalWrite(pin4, stepSequence[step][3]);
      delay(motorDelay);
    }
  }
}

// ฟังก์ชันหยุดมอเตอร์
void stopMotor(int pin1, int pin2, int pin3, int pin4) {
  digitalWrite(pin1, LOW);
  digitalWrite(pin2, LOW);
  digitalWrite(pin3, LOW);
  digitalWrite(pin4, LOW);
}