// Pin Motor Stepper
const int DIR = 12;
const int STEP = 14;

// Pin Ultrasonik
const int trigpin = 25;
const int echopin = 27;
const int trigpin2 = 26;
const int echopin2 = 33;
const int trigpin3 = 32;
const int echopin3 = 35;

// Pin LED RGB
const int redPin = 4;
const int greenPin = 2;
const int bluePin = 15;

void stepMotor(int steps, bool direction) {
  digitalWrite(DIR, direction);
  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP, HIGH);
    delayMicroseconds(800);
    digitalWrite(STEP, LOW);
    delayMicroseconds(800); 
  }
}

void setup() {
  Serial.begin(9600);
  
  // Motor Stepper
  pinMode(DIR, OUTPUT);
  pinMode(STEP, OUTPUT);
  
  // Sensor Ultrasonik
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
  pinMode(trigpin2, OUTPUT);
  pinMode(echopin2, INPUT);
  pinMode(trigpin3, OUTPUT);
  pinMode(echopin3, INPUT);

  // LED RGB
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
}

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

// Fungsi untuk mengatur warna LED RGB
void setRGBColor(int redValue, int greenValue, int blueValue) {
  analogWrite(redPin, redValue);
  analogWrite(greenPin, greenValue);
  analogWrite(bluePin, blueValue);
}

void loop() {
  // Membaca jarak dari sensor ultrasonik
  long distance1 = readUltrasonicDistance(trigpin, echopin);
  long distance2 = readUltrasonicDistance(trigpin2, echopin2);
  long distance3 = readUltrasonicDistance(trigpin3, echopin3);
  
  // Menampilkan hasil jarak di monitor serial
  Serial.print("Distance 1: ");
  Serial.println(distance1);
  Serial.print("Distance 2: ");
  Serial.println(distance2);
  Serial.print("Distance 3: ");
  Serial.println(distance3);

  // Logika untuk mengubah warna LED RGB berdasarkan jarak
  if (distance1 < 50 || distance2 < 100 || distance3 < 150) {
    // Jika jarak sangat dekat, nyalakan LED merah (0, 255, 0)
    setRGBColor(255, 0, 0);  // Merah terang
    stepMotor(200, true);  // Menggerakkan motor
    Serial.println("Aktif - Motor Bergerak!");
  } else if (distance1 < 100 || distance2 < 150 || distance3 < 200) {
    // Jika jarak sedang, nyalakan LED kuning (kombinasi merah dan hijau)
    setRGBColor(255, 255, 0);
     stepMotor(200, true);  // Kuning
  } else {
    // Jika jarak jauh, nyalakan LED hijau
    setRGBColor(0, 255, 0);
     stepMotor(200, true);  // Hijau terang
  }

  delay(1000);  // Tunggu 1 detik sebelum loop selanjutnya
}