#include <Wire.h> // Library komunikasi I2C 
#include <LiquidCrystal_I2C.h> // Library modul I2C LCD

uint16_t x_pos, y_pos, z_pos;

long lcd_millis = 0;
LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27, 16, 2);

void adc_init(void){
  ADMUX |= (1 << REFS0);
  ADCSRA |= (1 << ADPS2) | (1 << ADPS1);
  ADCSRA |= (1 << ADEN);
  ADCSRA |= (1 << ADIE);
  sei();
  ADCSRA |= (1 << ADSC);
}

ISR(ADC_vect){
  uint16_t x = ADCW;
  switch(ADMUX){
    case 0x40:
            x_pos = x;
            ADMUX = 0x41;
            break;
    case 0x41:
            y_pos = x;
            ADMUX = 0x42;
            break;
    case 0x42:
            z_pos = x;
            ADMUX = 0x40;
            break;
    default:
            break;
  }
  ADCSRA |= (1 << ADSC);
}

void setup() {
  adc_init();
  lcd.init();
  lcd.backlight();
  lcd.clear();
}

void loop() {
  lcd.setCursor(0, 0); 
  lcd.print("x:");
  lcd.setCursor(2, 0); 
  lcd.print(x_pos);
  lcd.setCursor(0, 1); 
  lcd.print("y:");
  lcd.setCursor(2, 1); 
  lcd.print(y_pos);
  lcd.setCursor(7, 0); 
  lcd.print("z:");
  lcd.setCursor(9, 0); 
  lcd.print(z_pos);

  if(millis()-lcd_millis >= 100){
    lcd.clear();
    lcd_millis = millis();
  }

}