// NAMA: MUHAMMAD FA'IZ DWI ADHISKI
// KELAS: XI TEK 1

// === PROTOTYPE SOCCER ROBOT ===

const int BTN[] = {32, 14, 33, 12, 13};
// 32 > maju
// 14 > mundur
// 33 > kiri
// 12 > kanan
// 13 > rem

const int LED[] = {19, 5, 16, 0};
// 19 > kiri mundur
// 5  > kiri maju
// 16 > kanan mundur
// 0  > kanan maju

int f,b,r,l,s,br;
char btSignal;

namespace botControl {
  void maju() {
    for(int i: LED) {
      digitalWrite(i, LOW);
    }

    digitalWrite(LED[1], HIGH);
    digitalWrite(LED[3], HIGH);
  }

  void mundur() {
    for(int i: LED) {
      digitalWrite(i, LOW);
    }

    digitalWrite(LED[0], HIGH);
    digitalWrite(LED[2], HIGH);
  }

  void kanan() {
    for(int i: LED) {
      digitalWrite(i, LOW);
    }

    digitalWrite(LED[1], HIGH);
  }

  void kiri() {
    for(int i: LED) {
      digitalWrite(i, LOW);
    }

    digitalWrite(LED[3], HIGH);
  }

  void berhenti() {
    for(int i: LED) {
      digitalWrite(i, LOW);
    }
  }

  void rem() {
    for(int i: LED) {
      digitalWrite(i, HIGH);
    }
  }

  // Main
  void mainButton() {
    f = digitalRead(BTN[0]);
    b = digitalRead(BTN[1]);
    r = digitalRead(BTN[2]);
    l = digitalRead(BTN[3]);
    br = digitalRead(BTN[4]);
    
    if(!f) {
      botControl::maju();
    } else {
      botControl::berhenti();
    }
    
    if(!b) {
      botControl::mundur();
    } else {
      botControl::berhenti();
    }

    if(!r) {
      botControl::kiri();
    } else {
      botControl::berhenti();
    }

    if(!l) {
      botControl::kanan();
    } else {
      botControl::berhenti();
    }

    if(!br) {
      botControl::rem();
    } else {
      botControl::berhenti();
    }
  }

  void mainBluetooth() {
    if(Serial.available() > 0) {
      btSignal = Serial.read();
    }

    if(btSignal == 'w') {
      botControl::maju();
    } else {
      botControl::berhenti();
    }

    if(btSignal == 'a') {
      botControl::kiri();
    } else {
      botControl::berhenti();
    }

    if(btSignal == 's') {
      botControl::mundur();
    } else {
      botControl::berhenti();
    }

    if(btSignal == 'd') {
      botControl::kanan();
    } else {
      botControl::berhenti();
    }

    if(btSignal == 'f') {
      botControl::rem();
    } else {
      botControl::berhenti();
    }
  }
}

void setup() {
  Serial.begin(9600);
  
  for(int i: LED) {
    pinMode(i, OUTPUT);
  }

  for(int i: BTN) {
    pinMode(i, INPUT_PULLUP);
  }
}

void loop() {
  botControl::mainButton();
  botControl::mainBluetooth();
}