/*
 * LCD Wiring
 * Pin 1: Ground
 * Pin 2: +5V
 * Pin 3: Connect to wiper of potentiometer
 * Pin 4: RS
 * Pin 5: RW (Connect to Ground)
 * Pin 6: EN
 * Pins 7-10: DB0-DB3 (Unused)
 * Pin 11: DB4
 * Pin 12: DB5
 * Pin 13: DB6
 * Pin 14: DB7
 * Pin 15: Backlight (Connect to +5V)
 * Pin 16: Red
 * Pin 17: Green
 * Pin 18: Blue
 */

#include <LiquidCrystal.h>
#define RSPIN 13
#define ENPIN 12
#define DB4PIN 8
#define DB5PIN 7
#define DB6PIN 6
#define DB7PIN 5

#define REDLIGHT 11
#define GREENLIGHT 10
#define BLUELIGHT 9

#include <Keypad.h>
#define ROWS 4
#define COLS 4

/* NOTE: program malfunctions if BUFFER is too big---see shunting function */
#define BUFFER 16
#include "Token.h"

LiquidCrystal lcd(RSPIN, ENPIN, DB4PIN, DB5PIN, DB6PIN, DB7PIN);

char keys[ROWS][COLS] = {{'1','2','3','A'},
                         {'4','5','6','B'},
                         {'7','8','9','C'},
                         {'*','0','#','D'}};
byte rowPins[ROWS] = {0,1,2,3};
byte colPins[COLS] = {A0,A1,A2,A3};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

void setBacklight(int r, int g, int b)
{
        r = map(r, 0, 255, 255, 0);
        g = map(g, 0, 255, 255, 0);
        b = map(b, 0, 255, 255, 0);

        analogWrite(REDLIGHT, r);
        analogWrite(GREENLIGHT, g);
        analogWrite(BLUELIGHT, b);
}

bool isNum(char c)
{
        return c == '1' || c == '2' || c == '3' || c == '4' || c == '5' ||
                 c == '6' || c == '7' || c == '8' || c == '9' || c == '0';
}

bool isOp(char c)
{
        return c == '+' || c == '-' || c == '*' || c == '/';
}

void tokenize(String input, Token *store, int *storeLen)
{
        int currLen = 0;
        for (int i = 0; i < input.length(); i++) {
                /* 
                 * NOTE: When BUFFER is too big, some characters 
                 * are seen as a block, so the if statements fail
                 */
                if (isNum(input[i])) {
                        /*
                         * Scans input until non-number is read, then adds to
                         * token 
                         */
                        String n;
                        for (int j = i; isNum(input[j]); j++) {
                                n += input[j];
                                i = j;
                        }
                        store[currLen++] = Token(n, "Number");
                } else if (isOp(input[i])) {
                        store[currLen++] = Token((String)input[i], "Operator");
                }
        }
        
        *storeLen = currLen;
}

void shunting(Token *tokenInput, Token *rpnStore, int len)
{
        int place = 0;
        Token opStack[BUFFER];
        int opLen = 0;
        
        for (int i = 0; i < len; i++) {
                Token currToken = tokenInput[i];
                if (currToken.getDataType() == "Number") {
                        rpnStore[place++] = currToken;
                } else if (currToken.getDataType() == "Operator") {
                        String currOp = currToken.getData();
                        while (opLen > 0) {
                                String topOp = opStack[opLen-1].getData();
                                // These two if statements could be combined
                                if (currOp == "+" || currOp == "-")
                                        rpnStore[place++] = opStack[--opLen];
                                else if ((currOp == "*" || currOp == "/") &&
                                             (topOp == "*" || topOp == "/"))
                                        rpnStore[place++] = opStack[--opLen];
                                else
                                        break;
                        }
                        opStack[opLen++] = currToken;
                }
        }
        
        while (opLen > 0)
                rpnStore[place++] = opStack[--opLen];
}

void calculate(Token *rpnInput, int len)
{
        float numStack[BUFFER];
        int numStackLen = 0;
        for (int i = 0; i < len; i++) {
                Token t = rpnInput[i];
                if (t.getDataType() == "Number") {
                        numStack[numStackLen++] = t.getData().toFloat();
                } else {
                        float sNum = numStack[--numStackLen];
                        float fNum = numStack[--numStackLen];
                        if (numStackLen < 0) {
                                lcd.print("ERROR");
                                return;
                        }
                        if (t.getData() == "+")
                                numStack[numStackLen++] = fNum + sNum;
                        else if (t.getData() == "-")
                                numStack[numStackLen++] = fNum - sNum;
                        else if (t.getData() == "*")
                                numStack[numStackLen++] = fNum * sNum;
                        else if (t.getData() == "/")
                                numStack[numStackLen++] = fNum / sNum;
                }
        }

        lcd.print(numStack[0]);
}

void getAns(String s)
{
        Token parsedInput[BUFFER];
        Token rpnParsedInput[BUFFER];
        int inputLen;

        tokenize(s, parsedInput, &inputLen);
        shunting(parsedInput, rpnParsedInput, inputLen);
        calculate(rpnParsedInput, inputLen);
}

void setup()
{
        lcd.begin(16, 2);
        pinMode(REDLIGHT, OUTPUT);
        pinMode(GREENLIGHT, OUTPUT);
        pinMode(BLUELIGHT, OUTPUT);
        setBacklight(0, 128, 64);
}

void loop()
{
        static String expr = "";
        
        char keypress = keypad.waitForKey();
        switch (keypress) {
        case '#':
                lcd.setCursor(0,1);
                getAns(expr);
                
                keypad.waitForKey();
        case '*':
                expr = "";
                break;
        case 'A':
                expr += '/';
                break;
        case 'B':
                expr += '*';
                break;
        case 'C':
                expr += '-';
                break;
        case 'D':
                expr += '+';
                break;
        default:
                expr += keypress;
                break;
        }
        lcd.clear();
        lcd.print(expr);
}