#include <Servo.h>
byte btn_memper, btn_sikapsiap;
bool hidup, tanda, status, statusSiap, tandaBahuKiri, tandaBahuKanan, tandaSikuKiri, tandaSikuKanan;
class engsel
{
    Servo servo;
    int posBahuKiri = 0;
    int posBahuKanan = 0;
    int posSikuKiri = 0;
    int posSikuKanan = 0;
    int incrementBahuKiri;
    int incrementBahuKanan;
    int incrementSikuKiri;
    int incrementSikuKanan;
    int updateInterval;
    unsigned long lastUpdate;

  public:
    engsel(int interval)
    {
      updateInterval = interval;
      incrementBahuKiri = 1;
      incrementBahuKanan = 1;
      incrementSikuKiri = 1;
      incrementSikuKanan = 1;
    }
    void aktif(int pin)
    {
      servo.attach(pin);
    }
    void nonaktif()
    {
      servo.detach();
    }
    void BahuKiri(int sudut, bool arah)
    {
      if (arah == 1) {
        if (posBahuKiri >= sudut) {
          posBahuKiri = sudut;
        }
      }
      if (arah == 0) {
        if (posBahuKiri <= sudut) {
          posBahuKiri = sudut;
        }
      }
      if ((millis() - lastUpdate) > updateInterval)
      {
        lastUpdate = millis();
        if (tandaBahuKiri == 0) {
          if (arah == 1) {
            posBahuKiri += incrementBahuKiri;
          }
          else {
            posBahuKiri -= incrementBahuKiri;
          }
          servo.write(posBahuKiri);
        }

        if (arah == 1) {
          if (posBahuKiri == sudut ) {
          //  tandaBahuKiri = 1;
            servo.detach();
          }
        }
        if (arah == 0) {
          if (posBahuKiri <= sudut) {
         //   tandaBahuKiri = 1;
            servo.detach();
          }
        }
      }
      Serial.println(posBahuKiri);
    }
    void BahuKanan(int sudut, bool arah)
    {
      if (posBahuKanan >= sudut) {
        posBahuKanan = sudut;
      }
      if (posBahuKanan <= 0) {
        posBahuKanan = 0;
      }
      if ((millis() - lastUpdate) > updateInterval)
      {
        lastUpdate = millis();
        if (tandaBahuKanan == 0) {
          if (arah == 1) {
            posBahuKanan += incrementBahuKanan;
          }
          else {
            posBahuKanan -= incrementBahuKanan;
          }
          servo.write(posBahuKanan);
        }

        if (posBahuKanan >= sudut || posBahuKanan <= 0) {
          tandaBahuKanan = 1;
          servo.detach();
        }
      }
    }
    void SikuKiri(int sudut, bool arah)
    {
      if (posSikuKiri >= sudut) {
        posSikuKiri = sudut;
      }
      if (posSikuKiri <= 0) {
        posSikuKiri = 0;
      }
      if ((millis() - lastUpdate) > updateInterval)
      {
        lastUpdate = millis();
        if (tandaSikuKiri == 0) {
          if (arah == 1) {
            posSikuKiri += incrementSikuKiri;
          }
          else {
            posSikuKiri -= incrementSikuKiri;
          }
          servo.write(posSikuKiri);
        }
        if (posSikuKiri >= sudut || posSikuKiri <= 0) {
          tandaSikuKiri = 1;
          servo.detach();
        }
      }
    }
    void SikuKanan(int sudut, bool arah)
    {
      if (posSikuKanan >= sudut) {
        posSikuKanan = sudut;
      }
      if (posSikuKanan <= 0) {
        posSikuKanan = 0;
      }
      if ((millis() - lastUpdate) > updateInterval)
      {
        lastUpdate = millis();
        if (tandaSikuKanan == 0) {
          if (arah == 1) {
            posSikuKanan += incrementSikuKanan;
          }
          else {
            posSikuKanan -= incrementSikuKanan;
          }
          servo.write(posSikuKanan);
        }
        if (posSikuKanan >= sudut || posSikuKanan <= 0) {
          tandaSikuKanan = 1;
          servo.detach();
        }
      }
    }
};
engsel bahuKiri(10);
engsel sikuKiri(10);
engsel bahuKanan(10);
engsel sikuKanan(10);

void setup() {
  Serial.begin(9600);
  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
}
void loop() {
  btn_memper = digitalRead(A0);
  btn_sikapsiap = digitalRead(A1);
  if (btn_memper == LOW) {
    status = true;
    statusSiap = false;
    tandaSikuKiri = 0;
    tandaSikuKanan = 0;
    tandaBahuKiri = 0;
    tandaBahuKanan = 0;
  }

  if (btn_sikapsiap == LOW) {
    status = false;
    statusSiap = true;
    tandaSikuKiri = 0;
    tandaSikuKanan = 0;
    tandaBahuKiri = 0;
    tandaBahuKanan = 0;
  }
  if (status == 1) {
    bahuKiri.aktif(9);
    bahuKiri.BahuKiri(80, 1);
    sikuKiri.aktif(10);
    sikuKiri.SikuKiri(63, 1);
    bahuKanan.aktif(5);
    bahuKanan.BahuKanan(70, 1);
    sikuKanan.aktif(6);
    sikuKanan.SikuKanan(120, 1);
  }
  if (statusSiap == 1) {
    bahuKiri.aktif(9);
    bahuKiri.BahuKiri(30, 0);
    sikuKiri.aktif(10);
    sikuKiri.SikuKiri(63, 0);
    bahuKanan.aktif(5);
    bahuKanan.BahuKanan(70, 0);
    sikuKanan.aktif(6);
    sikuKanan.SikuKanan(120, 0);
  }

}