#include "lopigaengsel.h"
lopigaengsel putarKanan;
lopigaengsel putarKiri;
lopigaengsel bahuKanan;
lopigaengsel sikuKanan;
lopigaengsel bahuKiri;
lopigaengsel sikuKiri;
lopigaengsel anggukLeher;
lopigaengsel putarLeher;

byte btn_memper, btn_sikapsiap;
bool modeSalam, modeDefault, modeHello;
bool Besar = false;
bool Kecil = false;

const int durasiK = 2500; // 500 milliseconds
bool statusTerakhirK = false;
unsigned long pressedTimeK  = 0;
unsigned long releasedTimeK = 0;

const int durasiB = 2500; // 500 milliseconds
bool statusTerakhirB = false;
unsigned long pressedTimeB  = 0;
unsigned long releasedTimeB = 0;
long timerHello;
bool Hello = false;
long timerHelloLeher;
bool HelloLeher = false;
void setup() {
  Serial.begin(9600);
  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
  putarKanan.pin(3);
  bahuKanan.pin(5);
  sikuKanan.pin(6);
  bahuKiri.pin(9);
  sikuKiri.pin(10);
  putarKiri.pin(11);
  anggukLeher.pin(A2);
  putarLeher.pin(A3);
}
void loop() {
  btn_memper = digitalRead(A0);
  btn_sikapsiap = digitalRead(A1);
  if (btn_memper == LOW) {
    modeHello = true;
    Hello = false;
    HelloLeher = false;
    modeDefault = false;
  }
  if (btn_sikapsiap == LOW) {
    modeHello = false;
    modeDefault = true;
  }
  if (modeDefault == 1) {
    bahuKiri.Servo1(90, 10);
    sikuKiri.Servo2(90, 10);
    putarKiri.Servo3(90, 10);
    bahuKanan.Servo4(90, 10);
    sikuKanan.Servo5(90, 10);
    putarKanan.Servo6(90, 10);
    anggukLeher.Servo7(90, 50);
    putarLeher.Servo8(90, 50);
  }
  if (modeSalam == 1) {

  }
  ///////////////Mode Hello Awal//////////////////
  if (millis() - timerHello > 1000 && modeHello == true) {
    timerHello = millis();
    if (Hello == false) {
      Hello = true;
    } else {
      Hello = false;
    }
  }
  if (millis() - timerHelloLeher > 5000 && modeHello == true) {
    timerHelloLeher = millis();
    if (HelloLeher == false) {
      HelloLeher = true;
    } else {
      HelloLeher = false;
    }
  }
  if (modeHello == true) {
    if (Hello == true) {
      bahuKanan.Servo4(100, 10);
      sikuKanan.Servo5(120, 10);
      putarKanan.Servo6(140, 10);
    }
    else {
      bahuKanan.Servo4(10, 10);
      sikuKanan.Servo5(10, 10);
      putarKanan.Servo6(10, 10);
    }
    if (HelloLeher == true) {
      anggukLeher.Servo7(120, 50);
      putarLeher.Servo8(140, 50);
    }
    else {
      anggukLeher.Servo7(90, 50);
      putarLeher.Servo8(10, 50);
    }
  }
  ///////////////Mode Hello Akhir//////////////////
  /*
    if (statusTerakhirK == false && Kecil == true) {

    pressedTimeK = millis();
    }
    else if (statusTerakhirK == true && Kecil == true) {
    releasedTimeK = millis();
    long pressDurationK = releasedTimeK - pressedTimeK;
    if ( pressDurationK > durasiK ) {
      //kecil();
    }
    }
    statusTerakhirK = Kecil;
    if (statusTerakhirB == false && Besar == true) {

    pressedTimeB = millis();
    }
    else if (statusTerakhirB == true && Besar == true) {
    releasedTimeB = millis();
    long pressDurationB = releasedTimeB - pressedTimeB;
    if ( pressDurationB > durasiB ) {
      // besar();
    }
    }
    statusTerakhirB = Besar;
  */
}