#include "lopigaengsel.h"
lopigaengsel putarKanan;
lopigaengsel putarSikuKanan;
lopigaengsel putarKiri;
lopigaengsel putarSikuKiri;
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(26, INPUT_PULLUP);
  pinMode(14, INPUT_PULLUP);
  //Pin Servo yang support ESP32
  //2,4,5,12-19,21-23,25-27,32-33
  putarKanan.pin(2);
  bahuKanan.pin(4);
  sikuKanan.pin(5);
  putarSikuKanan.pin(12);

  putarKiri.pin(19);
  bahuKiri.pin(21);
  sikuKiri.pin(23);
  putarSikuKiri.pin(25);

  anggukLeher.pin(27);
  putarLeher.pin(32);
}
void loop() {
  btn_memper = digitalRead(26);
  btn_sikapsiap = digitalRead(14);
  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) {

  }
  hello_mode();
  ///////////////Mode Hello Awal//////////////////

  ///////////////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;
  */
}

void hello_mode() {
  if (millis() - timerHello > 1000 && modeHello == true) {
    timerHello = millis();
    if (Hello == false) {
      Hello = true;
    } else {
      Hello = false;
    }
  }
  if (millis() - timerHelloLeher > 3000 && modeHello == true) {
    timerHelloLeher = millis();
    if (HelloLeher == false) {
      HelloLeher = true;
    } else {
      HelloLeher = false;
    }
  }
  if (modeHello == true) {
    if (Hello == true) {
      bahuKiri.Servo1(100, 10);
      sikuKiri.Servo2(120, 10);
      putarKiri.Servo3(140, 10);
      putarSikuKiri.Servo4(140, 10);

      bahuKanan.Servo5(100, 10);
      sikuKanan.Servo6(120, 10);
      putarKanan.Servo7(140, 10);
      putarSikuKanan.Servo8(140, 10);
    }
    else {
      bahuKiri.Servo1(10, 10);
      sikuKiri.Servo2(10, 10);
      putarKiri.Servo3(10, 10);
      putarSikuKiri.Servo4(10, 10);

      bahuKanan.Servo5(10, 10);
      sikuKanan.Servo6(10, 10);
      putarKanan.Servo7(10, 10);
      putarSikuKanan.Servo8(10, 10);
    }
    if (HelloLeher == true) {
      anggukLeher.Servo9(120, 20);
      putarLeher.Servo10(140, 20);
    }
    else {
      anggukLeher.Servo9(90, 20);
      putarLeher.Servo10(10, 20);
    }
  }
}