#include <math.h>
#include <Wire.h>
#include <hd44780.h>
#include <hd44780ioClass/hd44780_I2Cexp.h>

hd44780_I2Cexp lcd;

//--------------------------------------------------DATA--------------------------------------------------

const int ROW_COUNT = 4;
const int COL_COUNT = 5;
int rowPins[ROW_COUNT] = {2, 3, 4, 5};
int colPins[COL_COUNT] = {8, 9, 10, 11, 12};
bool buttonStates[ROW_COUNT][COL_COUNT] = {false};
bool prevButtonStates[ROW_COUNT][COL_COUNT] = {false};
char buttonVals[ROW_COUNT][COL_COUNT] = {
  {'7', '8', '9', 'D', 'C'},
  {'4', '5', '6', '*', '/'},
  {'1', '2', '3', '+', '-'},
  {'0', '.', 'E', 'a', '='},
};

const int MAX_INPUT_SIZE = 50;
const int MAX_MEMORY_SIZE = 8;

struct Input{
  char data[MAX_INPUT_SIZE];
  int len;
};
Input userInput = {};

struct Token{
  char type;

  union{
    float val;
    char op;
  };
};

enum ErrorCodes{
  SUCCESS,
  MULTIPLE_DECIMAL_POINTS,
};

struct TokenExpression{
  int tokenCount;
  ErrorCodes status;
  Token data[MAX_INPUT_SIZE];
};

struct Stack{
  int top;
  Token data[MAX_INPUT_SIZE];

  void POP(){
    top--;
  };
  void PUSH(Token input){
    top++;
    data[top] = input;
  };
  Token &PEEK(){
    return data[top];
  };
};

struct Memory{
  float ans[MAX_MEMORY_SIZE];
  char expr[MAX_INPUT_SIZE];
};
Memory memory = {};

struct Cursor{
  int x;
  int y;
};
Cursor cursor = {};

//-----------------------------------------------FUNCTIONS------------------------------------------------

bool isOperator(char input){
  const int OPERATOR_COUNT = 4;
  char operatorArray[OPERATOR_COUNT] = {'+', '-', '*', '/'};

  for(int i = 0; i < OPERATOR_COUNT; i++){
    if(input == operatorArray[i]){
      return true;
    }
  }

  return false;
}

TokenExpression tokenize(Input &input){
  TokenExpression infixExpression = {};
  int i = 0;

  while(i < input.len){
    if(isDigit(input.data[i]) || input.data[i] == '.'){
      float num = 0;
      bool seenDecimal = false;
      int digitsAD = 0;

      while(i < input.len && (isDigit(input.data[i]) || input.data[i] == '.')){
        if(input.data[i] == '.'){
          if(seenDecimal){
            infixExpression.status = MULTIPLE_DECIMAL_POINTS;
            return infixExpression;
          };
          seenDecimal = true;
          i++;
          continue;
        };

        if(isDigit(input.data[i]) && !seenDecimal){
          num = num * 10 + (input.data[i] - '0');
          i++;
          continue;
        };
          
        if(isDigit(input.data[i]) && seenDecimal){
          digitsAD++;
          num = num + (input.data[i] - '0') * pow(0.1, digitsAD);
          i++;
          continue;
        };
      }
      
      infixExpression.data[infixExpression.tokenCount].type = 'n';
      infixExpression.data[infixExpression.tokenCount].val = num;
      infixExpression.tokenCount++;
    }

    if(i < input.len && isOperator(input.data[i])){
      infixExpression.data[infixExpression.tokenCount].type = 'o';
      infixExpression.data[infixExpression.tokenCount].op = input.data[i];
      i++;
      infixExpression.tokenCount++;
    }
  }

  infixExpression.status = SUCCESS;
  return infixExpression;
}

int precedence(char op){
  if(op == '+' || op == '-'){
    return 1;
  }else if(op == '*' || op == '/'){
    return 2;
  };
}

TokenExpression shuntingYard(TokenExpression &infixExpression){
  TokenExpression RPNExpression = {};
  int RPNLen = 0;
  Stack opStack = {
    .top = -1
  };

  for(int i = 0; i < infixExpression.tokenCount; i++){
    if(infixExpression.data[i].type == 'n'){
      RPNExpression.data[RPNLen].type = 'n';
      RPNExpression.data[RPNLen].val = infixExpression.data[i].val;
      RPNLen++;
    }else if(infixExpression.data[i].type == 'o'){
      RPNExpression.data[RPNLen].type = 'o';
      if(opStack.top > -1){
        while(precedence(opStack.PEEK().op) >= precedence(infixExpression.data[i].op)){
          RPNExpression.data[RPNLen] = opStack.PEEK();
          RPNLen++;
          opStack.POP();
          continue;
        }
      };
      opStack.PUSH(infixExpression.data[i]);
    };
  }
  for(int i = 0; opStack.top > -1; i++){
    RPNExpression.data[RPNLen] = opStack.PEEK();
    opStack.POP();
    RPNLen++;
  }

  RPNExpression.tokenCount = RPNLen;
  RPNExpression.status = SUCCESS;
  return RPNExpression;
}

float evaluateRPN(TokenExpression &RPNExpression){
  Stack numberStack = {
    .top = -1
  };

  for(int i = 0; i < RPNExpression.tokenCount; i++){
    if(RPNExpression.data[i].type == 'n'){
      numberStack.PUSH(RPNExpression.data[i]);
    }else if(RPNExpression.data[i].type == 'o'){
      float num1 = numberStack.PEEK().val;
      numberStack.POP();
      float num2 = numberStack.PEEK().val;
      numberStack.POP();

      Token ans = {
        .type = 'n'
      };

      if(RPNExpression.data[i].op == '+'){
        ans.val = num2 + num1;
      }else if(RPNExpression.data[i].op == '-'){
        ans.val = num2 - num1;
      }else if(RPNExpression.data[i].op == '*'){
        ans.val = num2 * num1;
      }else if(RPNExpression.data[i].op == '/'){
        ans.val = num2 / num1;
      };

      numberStack.PUSH(ans);
    };
  }

  return numberStack.data[numberStack.top].val;
}

float evaluate(Input &input){
  TokenExpression infixExpression = tokenize(input);
  TokenExpression RPNExpression = shuntingYard(infixExpression);
  float ans = evaluateRPN(RPNExpression);
  return ans;
}

void clearInput(){
  for(int i = 0; i < userInput.len; i++){
    userInput.data[i] = '\0';
  }
  userInput.len = 0;

  lcd.clear();
  cursor.x = 0;
  cursor.y = 0;
  lcd.setCursor(cursor.x, cursor.y);
}

void deleteChar(){
  userInput.len--;
  userInput.data[userInput.len] = '\0';

  cursor.x--;
  lcd.setCursor(cursor.x, cursor.y);
  lcd.print(' ');
  lcd.setCursor(cursor.x, cursor.y);
}

void printChar(char character){
  userInput.data[userInput.len] = character;
  userInput.len++;

  lcd.print(character);
  cursor.x++;
  lcd.setCursor(cursor.x, cursor.y);
}

//---------------------------------------------SETUP AND LOOP---------------------------------------------

void setup() {
  for(int row = 0; row < ROW_COUNT; row++){
    pinMode(rowPins[row], OUTPUT);
  }
  for(int col = 0; col < COL_COUNT; col++){
    pinMode(colPins[col], INPUT_PULLUP);
  }

  lcd.begin(20, 4);
  lcd.setCursor(cursor.x, cursor.y);
  lcd.blink();

  Serial.begin(9600);
}

void loop() {
  //SCAN
  for(int currentRow = 0; currentRow < ROW_COUNT; currentRow++){
    for(int row = 0; row < ROW_COUNT; row++){
      digitalWrite(rowPins[row], HIGH);
    }
    digitalWrite(rowPins[currentRow], LOW);

    for(int currentCol = 0; currentCol < COL_COUNT; currentCol++){
      prevButtonStates[currentRow][currentCol] = buttonStates[currentRow][currentCol];
      buttonStates[currentRow][currentCol] = !digitalRead(colPins[currentCol]);
    }
  }

  //OUTPUT
  for(int currentRow = 0; currentRow < ROW_COUNT; currentRow++){
    for(int currentCol = 0; currentCol < COL_COUNT; currentCol++){
      if(buttonStates[currentRow][currentCol] && !prevButtonStates[currentRow][currentCol]){
        char val = buttonVals[currentRow][currentCol];

        if(val == '='){
            float ans = evaluate(userInput);
            cursor.x = 3;
            cursor.y = 3;
            lcd.setCursor(cursor.x, cursor.y);
            lcd.print(ans);
        }else if(val == 'C'){
          clearInput();
        }else if(val == 'D'){
          deleteChar();
        }else{
          printChar(val);
        }
      }
    }
  }

  delay(15);
}