#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <Stack.h>

// Initialize liquid crystal library
const byte ROWS = 4;
const byte COLS = 20;

LiquidCrystal_I2C lcd(0x27, ROWS, COLS);

// Initialize Keyboard
const byte KEYROWS = 4;
const byte KEYCOLS = 4;

char hexaKeys[KEYROWS][KEYCOLS] = {
  { '1', '2', '3', '+' },
  { '4', '5', '6', '-' },
  { '7', '8', '9', 'C' },
  { '*', '0', '/', '=' }
};

byte rowPins[KEYROWS] = { 2, 3, 4, 5 };
byte colPins[KEYCOLS] = { 6, 7, 8, 9 };

Keypad keypad = Keypad(makeKeymap(hexaKeys), rowPins, colPins, KEYROWS, KEYCOLS);

void setup() {
  Serial.begin(9600);
  Serial.println("Calculator v0.1");

  lcd.init();
  lcd.backlight();

  lcd.print("  Calculator v0.1");
  lcd.setCursor(0, 1);
  lcd.print("   Press any key ");

  snakeAnimation(ROWS - 1, COLS, 5, 150);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.blink_on();
}

void snakeAnimation(uint8_t row, uint8_t row_size, uint8_t snake_len, uint8_t delay_ms) {
  lcd.setCursor(0, row);

  uint8_t offset = row_size - snake_len;

  while (keypad.getKey() == 0) {
    for (int i = 0; i < snake_len; ++i) {
      lcd.setCursor(offset + i, row);
      lcd.print(' ');
      lcd.setCursor(i, row);
      lcd.print('*');
      delay(delay_ms);

      if (keypad.getKey() != 0) return;
    }

    for (int i = 0; i < row_size - snake_len; ++i) {
      lcd.setCursor(i, row);
      lcd.print(' ');
      lcd.setCursor(i + snake_len, row);
      lcd.print('*');
      delay(delay_ms);

      if (keypad.getKey() != 0) return;
    }
  }
}

uint8_t num_buffer[COLS + 1];

bool ignore_operator = false;

uint8_t cur_row = 0;
uint8_t cur_col = 0;
char op = 0;

Stack<uint32_t> num_stack;

void nextRow() {
  cur_row++;
  cur_col = 0;

  lcd.setCursor(cur_col, cur_row);
}

void backspace() {
  if (cur_col == 0) return;

  cur_col--;

  lcd.setCursor(cur_col, cur_row);
  lcd.print(' ');
  lcd.setCursor(cur_col, cur_row);
}

void handleNumRead(char key) {
  if (cur_col == COLS - 1) return;

  if ()

  const uint8_t dec = key - '0';
  num_buffer[cur_col] = dec;

  cur_col++;
  lcd.print(key);
}

void completeNumRead() {
  if (cur_col == 0) handleNumRead('0');

  uint32_t result = 0;

  for (int i = 0; i < cur_col; ++i) {
    result *= 10;
    result += num_buffer[i];
  }

  Serial.print("readed num: ");
  Serial.println(result);

  num_stack.push(result);
  nextRow();
}

void handleOperator(char key) {
  if (ignore_operator) return;

  Serial.print("op: ");
  Serial.print(key);
  Serial.print(" : stack size: ");
  Serial.println(num_stack.length());

  if (num_stack.length() == 0) completeNumRead();
  else if (num_stack.length() == 2) num_stack.pop();

  op = key;

  lcd.print(key);
  nextRow();

  ignore_operator = true;
}

void handleEquals() {
  if (num_stack.length() == 0) return;
  if (ignore_operator) completeNumRead();

  const uint32_t rhs = num_stack.pop();
  const uint32_t lhs = num_stack.pop();

  uint32_t result = lhs;

  switch (op) {
    case '+': result += rhs; break;
    case '-': result -= rhs; break;
    case '*': result *= rhs; break;
    case '/': result /= rhs; break;
    default: break;
  }

  Serial.print("equasion: ");
  Serial.print(lhs);
  Serial.print(op);
  Serial.print(rhs);
  Serial.print("=");
  Serial.println(result);

  num_stack.push(result);
  num_stack.push(rhs);

  cur_col = 0; cur_row = 0;
  lcd.clear();
  lcd.setCursor(cur_col, cur_row);

  lcd.print("= ");
  lcd.print(result);

  ignore_operator = false;
}

void loop() {
  char key = keypad.getKey();

  if (key == 0) return;

  switch(key) {
    case 'C':
      backspace();
    break;

    case '+':
    case '-':
    case '*':
    case '/':
      handleOperator(key);
    break;
    
    case '=':
      handleEquals();
    break;

    default:
      handleNumRead(key);
    break;
  }
}