#include <Servo.h>

Servo servoA;
Servo servoB;
Servo servoC;
Servo servoD;

int positions[] = {0, 0, 0, 0}; // Posisi awal semua servo
int activeServo = 0; // Servo yang aktif saat ini

void setup() {
  servoA.attach(3);
  servoB.attach(5);
  servoC.attach(6);
  servoD.attach(11);

  for (int i = 0; i < 4; i++) {
    moveServoToPosition(i, positions[i]);
  }

  // Mulai komunikasi serial dengan baud rate 9600
  Serial.begin(9600);
  Serial.println("TEKAN ANGKA");
}

void moveServoToPosition(int servoIndex, int position) {
  Servo* servo;
  int servoPin;

  if (servoIndex == 0) {
    servo = &servoA;
    servoPin = 3;
  } else if (servoIndex == 1) {
    servo = &servoB;
    servoPin = 5;
  } else if (servoIndex == 2) {
    servo = &servoC;
    servoPin = 6;
  } else if (servoIndex == 3) {
    servo = &servoD;
    servoPin = 11;
  }

  servo->write(position);
  Serial.print("Servo ");
  Serial.print(char('A' + servoIndex));
  Serial.print(" moved to ");
  Serial.print(position);
  Serial.println(" degrees.");
}

void loop() {
  if (Serial.available() > 0) {
    int command = Serial.parseInt(); // Membaca perintah dari komunikasi serial

    if (command >= 1 && command <= 4) {
      int servoIndex = command - 1; // Menghitung indeks servo berdasarkan perintah
      if (activeServo == servoIndex) {
        // Jika servo yang sedang aktif diaktifkan lagi, matikan servo tersebut
        moveServoToPosition(servoIndex, 0);
        activeServo = -1; // Tidak ada servo yang aktif
      } else {
        // Matikan servo yang sedang aktif saat ini (jika ada)
        if (activeServo != -1) {
          moveServoToPosition(activeServo, 0);
        }
        // Aktifkan servo yang sesuai dengan perintah
        moveServoToPosition(servoIndex, 90);
        activeServo = servoIndex;
      }
    } else if (command == 5) {
      // Mengembalikan semua servo ke posisi 0
      for (int i = 0; i < 4; i++) {
        moveServoToPosition(i, 0);
      }
      activeServo = -1; // Tidak ada servo yang aktif saat ini
    }
  }
}