const int inputA_pins[4] = {33, 32, 2, 4};

const int inputB_pins[4] = {14, 27, 26, 25};

const int opSelect_pin = 23;

const int output_pins[5] = {21, 19, 18, 17, 16};

const int subLED_pin = 22;

void setup() {
  Serial.begin(115200);
  for (int i = 0; i < 4; i++) {
    pinMode(inputA_pins[i], INPUT_PULLDOWN);
  }

  for (int i = 0; i < 4; i++) {
    pinMode(inputB_pins[i], INPUT_PULLDOWN);
  }

  pinMode(opSelect_pin, INPUT_PULLDOWN);

  for (int i = 0; i < 5; i++) {
    pinMode(output_pins[i], OUTPUT);
    digitalWrite(output_pins[i], LOW);
  }

  pinMode(subLED_pin, OUTPUT);
  digitalWrite(subLED_pin, LOW);
}

int readBits(const int* pins, int numBits) {
  int value = 0;
  for (int i = 0; i < numBits; i++) {
    if (digitalRead(pins[i]) == HIGH) {
      value |= (1 << i); 
    }
  }
  return value;
}

void writeBits(const int* pins, int numBits, int value) {
  for (int i = 0; i < numBits; i++) {
    digitalWrite(pins[i], (value >> i) & 1 ? HIGH : LOW);
  }
}

void printBinary(int value, int numBits) {
  for (int i = numBits - 1; i >= 0; i--) {
    Serial.print((value >> i) & 1);
  }
}

void loop() {
  int A  = readBits(inputA_pins, 4);
  int B  = readBits(inputB_pins, 4);
  int op = digitalRead(opSelect_pin);  // LOW=Add, HIGH=Subtract

  digitalWrite(subLED_pin, op == LOW ? HIGH : LOW);

  int result;
  String opSymbol;

  if (op == LOW) {
    result   = A + B;
    opSymbol = "+";
  } else {
    result   = (A - B) & 0x1F;
    opSymbol = "-";
  }

  writeBits(output_pins, 5, result);

  Serial.print("A = ");
  Serial.print(A);
  Serial.print(" (");
  printBinary(A, 4);
  Serial.print(")  ");
  Serial.print(opSymbol);
  Serial.print("  B = ");
  Serial.print(B);
  Serial.print(" (");
  printBinary(B, 4);
  Serial.print(")  =  Result = ");
  Serial.print(result);
  Serial.print(" (");
  printBinary(result, 5);
  Serial.println(")");

  delay(300);
}