const int X_PIN_VALUE = A1;
const int Y_PIN_VALUE = A0;

const int PIN_INH = 7;
const int PIN_A = 4;
const int PIN_B = 5;

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


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

  pinMode(X_PIN_VALUE, INPUT);
  pinMode(Y_PIN_VALUE, INPUT);

  pinMode(PIN_INH, OUTPUT);
  pinMode(PIN_A, OUTPUT);
  pinMode(PIN_B, OUTPUT);

  pinMode(SW_INH, INPUT_PULLUP);
  pinMode(SW_A, INPUT_PULLUP);
  pinMode(SW_B, INPUT_PULLUP);

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

}

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

  setRegisters(inh, a, b);


  Serial.print("X Value");
  Serial.println(analogRead(X_PIN_VALUE));
  Serial.print("Y Value");
  Serial.println(analogRead(Y_PIN_VALUE));
  Serial.println("------");


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

  delay(1000);
}

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