#include <util/atomic.h>
#include <LiquidCrystal_I2C.h>
#define LED_RED 6
#define LED_GREEN 5
#define LED_BLUE 3
#define RED_BUTTON 2
#define ENC1 A2
#define ENC2 A3
#define DEBOUNCING_PERIOD 50

LiquidCrystal_I2C lcd(0x27, 16, 2);

int option = 1;
String option1 = " RED LED";
String option2 = " GREEN LED";
String option3 = " BLUE LED";
unsigned long previousButtonTimestamp = 0UL;
volatile int encoder1 = HIGH;
volatile int encoder2 = HIGH;
volatile unsigned long encoderTimestamp = 0UL;
volatile unsigned long buttonTimestamp = 0UL;

ISR(PCINT1_vect) {
  encoder1 = digitalRead(ENC1);
  encoder2 = digitalRead(ENC2);
  encoderTimestamp = millis();
}

void button_interrupt_action() {
  buttonTimestamp = millis();
}

void manage_diodes() {
  bool red = digitalRead(LED_RED);
  bool green = digitalRead(LED_GREEN);
  bool blue = digitalRead(LED_BLUE);

  if (option == 1) {
    digitalWrite(LED_RED, !red);
  } else if (option == 2) {
    digitalWrite(LED_GREEN, !green);
  } else {
    digitalWrite(LED_BLUE, !blue);
  }


}

void adjust_menu_look() {
  if (option == 3) {
    lcd.setCursor(1, 0);
    lcd.print(option2);
    lcd.print("      ");
    lcd.setCursor(1, 1);
    lcd.print(option3);
    lcd.print("      ");
  } else {
    lcd.setCursor(1, 0);
    lcd.print(option1);
    lcd.print("      ");
    lcd.setCursor(1, 1);
    lcd.print(option2);
    lcd.print("      ");
  }
  manage_arrow();
}

void manage_arrow() {
  if (option == 1) {
    lcd.setCursor(0, 1);
    lcd.print(" ");
    lcd.setCursor(0, 0);
  } else {
    lcd.setCursor(0, 0);
    lcd.print(" ");
    lcd.setCursor(0, 1);
  }
  lcd.write(byte(0));
}

void setup_pin_modes() {
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);
  pinMode(RED_BUTTON, INPUT_PULLUP);
  pinMode(ENC1, INPUT_PULLUP);
  pinMode(ENC2, INPUT_PULLUP);
}

void setup_lcd() {
  byte arrow[] = {
    B00000,
    B00100,
    B11110,
    B11111,
    B11110,
    B00100,
    B00000,
    B00000
  };
  lcd.init();
  lcd.backlight();
  lcd.createChar(0, arrow);
  adjust_menu_look();
}

void setup() {
  setup_lcd();
  setup_pin_modes();
  PCICR |= (1 << PCIE1);
  PCMSK1 |= (1 << PCINT10);
  attachInterrupt(digitalPinToInterrupt(RED_BUTTON), button_interrupt_action, FALLING);
}

int lastEn1 = LOW;
unsigned long lastChangeTimestamp = 0UL;
void loop() {
  int en1;
  int en2;
  unsigned long timestamp;
  unsigned long localButtonTimestamp;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    en1 = encoder1;
    en2 = encoder2;
    timestamp = encoderTimestamp;
    localButtonTimestamp = buttonTimestamp;
  }

  if (localButtonTimestamp != previousButtonTimestamp && millis() > localButtonTimestamp + DEBOUNCING_PERIOD) {
    if (digitalRead(RED_BUTTON) == LOW) {
      manage_diodes();
    }
    previousButtonTimestamp = localButtonTimestamp;
  }



  if (en1 == LOW && timestamp > lastChangeTimestamp + DEBOUNCING_PERIOD) {
    if (en2 == HIGH) {
      if (option < 3) {
        option ++;
      }
    }
    else {
      if (option > 1) {
        option --;
      }
    }
    adjust_menu_look();
    lastChangeTimestamp = timestamp;
  }


}