#include <LiquidCrystal_I2C.h>

const int PIN_VALUE = A0;
const int PIN_INH = 7;
const int PIN_A = 4;
const int PIN_B = 5;
const int PIN_C = 6;

const int SW_INH = 8;
const int SW_A = 9;
const int SW_B = 10;
const int SW_C = 11;

LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  Serial.begin(115200);
  Serial.println("Starting Demo");

  pinMode(PIN_VALUE, INPUT);
  pinMode(PIN_INH, OUTPUT);
  pinMode(PIN_A, OUTPUT);
  pinMode(PIN_B, OUTPUT);
  pinMode(PIN_C, OUTPUT);
  pinMode(SW_INH, INPUT);
  pinMode(SW_A, INPUT);
  pinMode(SW_B, INPUT);
  pinMode(SW_C, INPUT);

  setRegisters(
    digitalRead(SW_INH), 
    digitalRead(SW_A), 
    digitalRead(SW_B), 
    digitalRead(SW_C)
  );

  lcd.init();
  lcd.backlight();
  lcd.begin(16,2);
}

void loop() {
  uint8_t inh = digitalRead(SW_INH);
  uint8_t a = digitalRead(SW_A);
  uint8_t b = digitalRead(SW_B);
  uint8_t c = digitalRead(SW_C);

  setRegisters(inh, a, b, c);

  lcd.setCursor(0,0);
  lcd.print("Value: ");
  lcd.print(analogRead(PIN_VALUE));

  byte result = 0b00000000;
  bitWrite(result, 3, inh);
  bitWrite(result, 2, a);
  bitWrite(result, 1, b);
  bitWrite(result, 0, c);

  lcd.setCursor(0,1);
  lcd.print("Set: ");
  lcd.print(result);

  delay(5);
}

void setRegisters(uint8_t inh, uint8_t a, uint8_t b, uint8_t c) {
  digitalWrite(PIN_INH, inh);
  digitalWrite(PIN_A, a);
  digitalWrite(PIN_B, b);
  digitalWrite(PIN_C, c);
}