#include <LiquidCrystal_I2C.h>
#include "postfix.h"

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

// Keypad
const int rows = 4;
const int cols = 4;
char keymap[rows][cols] = {
  {'1', '5', '9', '*'},
  {'2', '6', '0', '/'},
  {'3', '7', '+', '('},
  {'4', '8', '-', ')'}
};
byte rPins[rows] = {2, 3, 4, 5};
byte cPins[cols] = {6, 7, 8, 9};

// Stacks and input string
Stiva stv;
Stiva Op;
String s = "";

// Button to trigger evaluation
const int EVAL = 13;

// Function prototypes
void setup();
void loop();
char polling();
void processKeypress(char key);
void displayOnLCD(String message);
void handleEvaluation();

void setup() {
  // Initialize stacks
  Init(stv);
  Init(Op);

  // Initialize LCD
  lcd.init();
  lcd.clear();
  lcd.backlight();
  lcd.setCursor(0, 0);

  // Set EVAL button as INPUT_PULLUP
  pinMode(EVAL, INPUT_PULLUP);

  // Set row pins as INPUT_PULLUP
  for (int i = 0; i < rows; i++) {
    pinMode(rPins[i], INPUT_PULLUP);
  }

  // Set column pins as OUTPUT and HIGH
  for (int i = 0; i < cols; i++) {
    pinMode(cPins[i], OUTPUT);
    digitalWrite(cPins[i], HIGH);
  }
}

void loop() {
  char key = polling();
  if (key != ' ') {
    processKeypress(key);
  }

  if (!digitalRead(EVAL)) {
    handleEvaluation();
  }
}

char polling() {
  char key = ' ';
  for (int i = 0; i < cols; i++) {
    digitalWrite(cPins[i], LOW);

    for (int j = 0; j < rows; j++) {
      if (digitalRead(rPins[j]) == LOW) {
        delay(100);
        if (digitalRead(rPins[j]) == LOW) {
          key = keymap[j][i];
          break;
        }
      }
    }

    digitalWrite(cPins[i], HIGH);

    if (key != ' ') {
      break;
    }
  }

  return key;
}

void processKeypress(char key) {
  s.concat(key);
  displayOnLCD(s);
}

void handleEvaluation() {
  String postfixForm = postfix(stv, s);
  displayOnLCD("Postfix form:");
  displayOnLCD(postfixForm);

  displayOnLCD("Result:");
  s = postfixeval(Op, postfixForm);
  displayOnLCD(s);
}

void displayOnLCD(String message) {
  lcd.clear();
  int len = message.length();

  if (len <= 32) {
    lcd.print(message);
    Serial.println(message);

    if (len >= 16) {
      lcd.setCursor(0, 1);
      lcd.print(message.substring(16));
    }
  }
  else {
    lcd.clear();
    lcd.print("Character limit!");
    delay(400);
    lcd.clear();
    s = s.substring(0, s.length() - 1);
    displayOnLCD(s);
  }

  delay(1000);
}