#include <Servo.h>

#define SERVO_DELAY 20  // Waktu delay antar perubahan sudut servo (milidetik)

// Konstanta
const int MIN_ANGLE = 0;
const int MAX_ANGLE = 180;
const int SERVO_COUNT = 2; // Jumlah servo

// Struktur untuk menyimpan informasi terkait servo (lebih dari satu tipe data)
struct ServoInfo {
  Servo motor;     // Objek servo
  int pin;         // Pin tempat servo terhubung
  String name;     // Nama servo
  int currentAngle;  // Menyimpan sudut saat ini
};

// Deklarasi array struct untuk beberapa motor servo
ServoInfo servos[SERVO_COUNT] = {
  {Servo(), 3, "Servo 1", 0},
  {Servo(), 5, "Servo 2", 180}
};

// Fungsi untuk menggerakkan servo dengan parameter (struct servo, sudut mulai, dan sudut akhir)
void moveServo(ServoInfo &servoInfo, int sudutmulai, int sudutakhir) {
  Serial.print("Menggerakkan ");
  Serial.println(servoInfo.name);

  if (sudutmulai < sudutakhir) {
    // Gerakkan servo dari sudutmulai ke sudutakhir
    for (int angle = sudutmulai; angle <= sudutakhir; angle++) {
      servoInfo.motor.write(angle);
      servoInfo.currentAngle = angle;  // Simpan sudut saat ini
      delay(SERVO_DELAY);
    }
  } else {
    // Gerakkan servo dari sudutmulai ke sudutakhir
    for (int angle = sudutmulai; angle >= sudutakhir; angle--) {
      servoInfo.motor.write(angle);
      servoInfo.currentAngle = angle;  // Simpan sudut saat ini
      delay(SERVO_DELAY);
    }
  }
}

void setup() {
  // Menghubungkan pin servo ke objek motor
  for (int i = 0; i < SERVO_COUNT; i++) {
    servos[i].motor.attach(servos[i].pin);
  }

  // Inisialisasi serial untuk debugging
  Serial.begin(9600);
}

void loop() {
  // Loop melalui setiap servo dan menggerakkannya
  for (int i = 0; i < SERVO_COUNT; i++) {
    // Variabel lokal untuk sudut tujuan setiap servo
    int startAngle = servos[i].currentAngle;
    int endAngle;

    // Tentukan arah gerakan berdasarkan posisi awal
    if (startAngle == MIN_ANGLE) {
      endAngle = MAX_ANGLE;
    } else {
      endAngle = MIN_ANGLE;
    }

    // Panggil fungsi untuk menggerakkan servo
    moveServo(servos[i], startAngle, endAngle);
    delay(1000);  // Jeda 1 detik antar gerakan
  }
}