#include <Wire.h> // Library for I2C communication
#include <LiquidCrystal_I2C.h> // Library for LCD

//ap_hdr encoder
#define AP_HDR_ROTARY_ENCODER_CLK 2
#define AP_HDR_ROTARY_ENCODER_DT  4

//ap_altitude encoder
#define AP_ALT_ROTARY_ENCODER_CLK 3
#define AP_ALT_ROTARY_ENCODER_DT  5

//LCD Display
LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27, 16, 2);

//initialize variables
volatile int ap_hdg = 0;
volatile int ap_alt = 0;
volatile int ap_hdg_before = 0;
volatile int ap_alt_before = 0;

void setup() {
	Serial.begin(9600);
  
  pinMode(AP_HDR_ROTARY_ENCODER_CLK, INPUT);
  pinMode(AP_HDR_ROTARY_ENCODER_DT, INPUT);
  attachInterrupt(digitalPinToInterrupt(AP_HDR_ROTARY_ENCODER_CLK), read_AP_HDR_Encoder, FALLING);
	
  pinMode(AP_ALT_ROTARY_ENCODER_CLK, INPUT);
  pinMode(AP_ALT_ROTARY_ENCODER_DT, INPUT);
  attachInterrupt(digitalPinToInterrupt(AP_ALT_ROTARY_ENCODER_CLK), read_AP_ALT_Encoder, FALLING);

  lcd.init();
  lcd.backlight();
  updateDisplay();
}

void read_AP_ALT_Encoder() {
  int dtValue = digitalRead(AP_ALT_ROTARY_ENCODER_DT);
  if (dtValue == HIGH) {
    ap_alt += 100;
  } else {
    ap_alt > 0 ? ap_alt -= 100 : ap_alt = 0;
  }
  logAltChanges();
}

void read_AP_HDR_Encoder() {
  int dtValue = digitalRead(AP_HDR_ROTARY_ENCODER_DT);
  if (dtValue == HIGH) {
    ap_hdg < 359 ? ap_hdg++ : ap_hdg = 0;
  } else {
    ap_hdg > 0 ? ap_hdg-- : ap_hdg = 359;
  }
  logHdrChanges();
}

void loop() {
  if (ap_alt_before != ap_alt) {
    updateDisplay();
    ap_alt_before = ap_alt;
  }
  if (ap_hdg_before != ap_hdg) {
    updateDisplay();
    ap_hdg_before = ap_hdg;
  } 
}

void logHdrChanges() {
  if (ap_hdg_before != ap_hdg) {
    Serial.print("HDR: ");
    Serial.println(ap_hdg);
  }
}

void logAltChanges() {
  if (ap_alt_before != ap_alt) {
    Serial.print("ALT: ");
    Serial.println(ap_alt);
  }
}

void updateDisplay() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("ALT:");
  lcd.setCursor(5, 0);
  lcd.print(ap_alt);

  lcd.setCursor(0, 1);
  lcd.print("HDG:");
  lcd.setCursor(5, 1);
  lcd.print(ap_hdg);
}