#define MOT1_STEP 15
#define MOT1_DIR  16
#define MOT2_STEP 13
#define MOT2_DIR  14
#define MOT3_STEP 11
#define MOT3_DIR  12
#define BTN_A_PLUS  4
#define BTN_A_MINUS 3
#define BTN_B_PLUS  8
#define BTN_B_MINUS 5
#define BTN_C_PLUS  10
#define BTN_C_MINUS 9
#define LED1 0
#define LED2 1
#define LED3 2

#define MOT1_DELAY 1700
#define MOT2_DELAY 1700
#define MOT3_DELAY 1700

void setup() {
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);

  pinMode(BTN_A_MINUS, INPUT_PULLUP);
  pinMode(BTN_A_PLUS, INPUT_PULLUP);
  pinMode(BTN_B_MINUS, INPUT_PULLUP);
  pinMode(BTN_B_PLUS, INPUT_PULLUP);
  pinMode(BTN_C_MINUS, INPUT_PULLUP);
  pinMode(BTN_C_PLUS, INPUT_PULLUP);

  pinMode(MOT1_STEP, OUTPUT);
  pinMode(MOT1_DIR, OUTPUT);
  pinMode(MOT2_STEP, OUTPUT);
  pinMode(MOT2_DIR, OUTPUT);
  pinMode(MOT3_STEP, OUTPUT);
  pinMode(MOT3_DIR, OUTPUT);

  digitalWrite(MOT1_DIR, LOW);
  digitalWrite(MOT1_STEP, LOW);
  digitalWrite(MOT2_DIR, LOW);
  digitalWrite(MOT2_STEP, LOW);
  digitalWrite(MOT3_DIR, LOW);
  digitalWrite(MOT3_STEP, LOW);

  //Serial.begin(9600);
}

byte pollButton() {
  byte buttons = 0b00000000;
  
  if (!digitalRead(BTN_A_MINUS)) {
    buttons |= 0b00000001;
  }
  if (!digitalRead(BTN_A_PLUS)) {
    buttons |= 0b00000010;
  }
  if (!digitalRead(BTN_B_MINUS)) {
    buttons |= 0b00000100;
  }
  if (!digitalRead(BTN_B_PLUS)) {
    buttons |= 0b00001000;
  }
  if (!digitalRead(BTN_C_MINUS)) {
    buttons |= 0b00010000;
  }
  if (!digitalRead(BTN_C_PLUS)) {
    buttons |= 0b00100000;
  }
  
  return buttons;
}

void drive_motor(byte buttons) {
  //Serial.print("drv");
  //Serial.println(buttons);
  if(buttons == 1 || buttons == 5 || buttons == 9 || buttons == 17 || buttons == 33) {
    //Serial.println("MOT1-");
    digitalWrite(MOT1_DIR, LOW);
    
    digitalWrite(MOT1_STEP, HIGH);
    delayMicroseconds(MOT1_DELAY);
    digitalWrite(MOT1_STEP, LOW);
    delayMicroseconds(MOT1_DELAY);
  }
  if(buttons == 2|| buttons == 6 || buttons == 10 || buttons == 18 || buttons == 34) {
    //Serial.println("MOT1+");
    digitalWrite(MOT1_DIR, HIGH);
    
    digitalWrite(MOT1_STEP, HIGH);
    delayMicroseconds(MOT1_DELAY);
    digitalWrite(MOT1_STEP, LOW);
    delayMicroseconds(MOT1_DELAY);
  }
  if(buttons == 4 || buttons == 5 || buttons == 6 || buttons == 20 || buttons == 36) {
    //Serial.println("MOT2-");
    digitalWrite(MOT2_DIR, LOW);
    
    digitalWrite(MOT2_STEP, HIGH);
    delayMicroseconds(MOT2_DELAY);
    digitalWrite(MOT2_STEP, LOW);
    delayMicroseconds(MOT2_DELAY);
  }
  if(buttons == 8 || buttons == 9 || buttons == 10 || buttons == 24 || buttons == 40) {
    //Serial.println("MOT2+");
    digitalWrite(MOT2_DIR, HIGH);
    
    digitalWrite(MOT2_STEP, HIGH);
    delayMicroseconds(MOT2_DELAY);
    digitalWrite(MOT2_STEP, LOW);
    delayMicroseconds(MOT2_DELAY);
  }
  if(buttons == 16 || buttons == 17 || buttons == 18 || buttons == 20 || buttons == 24) {
    //Serial.println("MOT3-");
    digitalWrite(MOT3_DIR, LOW);
    
    digitalWrite(MOT3_STEP, HIGH);
    delayMicroseconds(MOT3_DELAY);
    digitalWrite(MOT3_STEP, LOW);
    delayMicroseconds(MOT3_DELAY);
  }
  if(buttons == 32 || buttons == 33 || buttons == 34 || buttons == 36 || buttons == 40) {
    //Serial.println("MOT3+");
    digitalWrite(MOT3_DIR, HIGH);
    
    digitalWrite(MOT3_STEP, HIGH);
    delayMicroseconds(MOT3_DELAY);
    digitalWrite(MOT3_STEP, LOW);
    delayMicroseconds(MOT3_DELAY);
  }
}

void loop() {
  byte buttons = 0;
  
  buttons = pollButton();
  //Serial.println(buttons);
  drive_motor(buttons);
  //delay(500);
}