#include <Keypad.h>
#include <LiquidCrystal.h>
#include <Arduino.h>
// #include <stdlib.h>
// #include <vector>
#include "vector.h"

#define STATE_FIRST 0
#define STATE_ANS 3
#define STATE_WAIT 4

#define OP_PLUS 0
#define OP_MINUS 1
#define OP_MULTIPLY 2
#define OP_DIVIDE 3

const uint8_t ROWS = 4;
const uint8_t COLS = 4;

struct Lab {
    // State of the MCU
    uint8_t currentState = STATE_FIRST;

    String data = "";

    const char EQUALS = '#';
    const char CLR = 'R';

    // Keypad
    const uint8_t rowPins[COLS] = { 7, 6, 5, 4 }; // Pins connected to R1, R2, R3, R4
    const uint8_t colPins[ROWS] = { 3, 2, 17, 18 }; // Pins connected to C1, C2, C3, C4
    char keys[ROWS][COLS] = {
        { '1', '2', '3', '+' },
        { '4', '5', '6', '-' },
        { '7', '8', '9', '*' },
        { 'R', '0', '#', '/' }
    };
    Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

    // LCD
    LiquidCrystal lcd = LiquidCrystal(14, 15, 16, 12, 11, 10, 9);

} board;

bool isNumber(char key);
bool isOperator(char key);
void updateState1(char key);
void updateAns();
void updateWait();
void writeDisplay(String text);
bool evalExp(int32_t *output, std::vector<String> tokens);
void reset();

void setup() {
    Serial.begin(115200);
    // std::vector<String> tokens;
    // tokens.push_back("20");
    // tokens.push_back("-");
    // tokens.push_back("5");
    // tokens.push_back("-");
    // tokens.push_back("5");
    // bool res = 0;
    // int32_t val = 0;
    // Serial.println(">>");
    // res = evalExp(&val, tokens);
    // Serial.println(">>");
    // Serial.println("Success " + String(res));
    // Serial.println(val);
    // Serial.println("end");
    // return;

    pinMode(7, INPUT);
    pinMode(6, INPUT);
    pinMode(5, INPUT);
    pinMode(4, INPUT);
    pinMode(3, INPUT);
    pinMode(2, INPUT);
    pinMode(1, INPUT);
    pinMode(0, INPUT);

    // set up LCD
	  board.lcd.begin(16, 2); // col, row
}

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

    switch(board.currentState) {
        case STATE_FIRST:
            updateState1(key);
            break;
        case STATE_ANS:
            updateAns();
            break;
        case STATE_WAIT:
            updateWait(key);
            break;
        
    }

    if (key != NO_KEY) {
        Serial.println(key);

    }
}

bool isNumber(char key) {
    return key >= '0' && key <= '9';
}

bool isOperator(char key) {
    return key == '+' || key == '-' || key == '*' || key == '/';
}

uint8_t mapOperator(char op) {
    switch(op) {
        case 'A': return OP_PLUS;
        case 'B': return OP_MINUS;
        case 'C': return OP_MULTIPLY;
        case 'D': return OP_DIVIDE;
        default: return 5;
    }
}

void updateState1(char key){
    if (isNumber(key) || isOperator(key)) {
        board.data += String(key);
        writeDisplay(board.data);
    } else if (key == board.EQUALS) {
        board.currentState = STATE_ANS;
    } else if (key == board.CLR) {
        reset();
    }
}

bool eval(int32_t *output, int32_t first, int32_t second, char op) {
    if (op == '+') {
        *output = first + second;
    } else if (op == '-') {
        *output = first - second;
    } else if (op == '*') {
        *output = first * second;
    } else if (op == '/') {
        if (second == 0) return false;
        *output = first / second;
    }
    return true;
}

bool isDigit(String s) {

    for (int i = 0; i < s.length(); i++) {
        if (i == 0 && s[i] == '-') continue;
        if (s[i] < '0' || s[i] > '9') return false;
    }
    return true;
}

int findOperator(std::vector<String> tokens) {
    auto f = [&tokens](char param) {
        for (int i = 0; i < tokens.size(); i++) {
            if (tokens[i][0] == param && tokens[i].length() == 1) return i;
        }
        return -1;
    };

    int i;
    if ((i = f('+')) >= 0) return i;
    if ((i = f('-')) >= 0) return i;
    if ((i = f('*')) >= 0) return i;
    if ((i = f('/')) >= 0) return i;
    return -1;
}

void dump(std::vector<String> tokens) {
    for (int i = 0; i < tokens.size(); i++) {
        Serial.print(tokens[i] + ",");
    }
    Serial.println("--");
}

/**
 * Evaluates an expression.
 *
 * The implementation can be visualized as a binary tree with the root node
 * as the operator with the highest precedence. We first read the string, find
 * the root node, then split the left and right into individual expressions
 * and evaluate them recursively. At the end, we will have the root node containing
 * the solution itself.
 * 
 * @param output    the variable to store the output
 * @param tokens    the vector containing tokens
 * @return true if successful, false otherwise
 */
bool evalExp(int32_t *output, std::vector<String> tokens) {
    dump(tokens);
    // Base case
    if (tokens.size() == 0) return false;   // empty input
    if (tokens.size() == 2) return false;   // incomplete input
    if (tokens.size() == 1) {               // single input / base case
        String s = tokens[0];
        if (isDigit(s)) {
            *output = (int32_t) s.toInt();
            return true;
        }
    }
    if (tokens.size() == 3) {
        // assertinos
        if (isDigit(tokens[0]) && isDigit(tokens[2]) &&
            isOperator(tokens[1][0])) {
            return eval(output, tokens[0].toInt(), tokens[2].toInt(), tokens[1][0]);
        }
    }

    // recursive case
    int opIndex = findOperator(tokens);
    char op = tokens[opIndex][0];
    if (op == -1) { // in this case, tokens.size() > 3 but no operators found.
        return false;
    }

    if (op == '-') {
        op = '+';
        if (opIndex + 1 < tokens.size()) {
            tokens[opIndex+1] = "-" + tokens[opIndex+1];

        }
    }

    int32_t leftOutput = 0;
    int32_t rightOutput = 0;
    bool a = evalExp(&leftOutput, tokens.sublist(0, opIndex));
    bool b = evalExp(&rightOutput, tokens.sublist(opIndex+1, tokens.size()));
    bool c = eval(output, leftOutput, rightOutput, op);
    return a & b & c;
}



void updateAns() {

    std::vector<String> tokens;
    // evaluate the expression
    // Tokenization
    String acc = "";
    for (int i = 0; i < board.data.length(); i++) {
        char c = board.data.charAt(i);

        if (isNumber(c)) {
            acc += c;
        } else if (isOperator(c)) {
            if (acc.length() == 0) {
                writeDisplay("Invalid input");
                board.currentState = STATE_WAIT;
                return;
            }
            tokens.push_back(acc);
            tokens.push_back(String(c));
            acc = "";
        }
    }

    tokens.push_back(acc);

    for (int i = 0; i < tokens.size(); i++) {
        Serial.println(tokens[i]);
    }

    // assume 3 tokens
    // int32_t first = tokens[0].toInt();
    // int32_t second = tokens[2].toInt();

    // int32_t result = 0;
    // char b = tokens[1].charAt(0);

    // bool status = eval(&result, first, second, b);
    int32_t result = 0;
    bool status = evalExp(&result, tokens);
    if (status) {
        board.lcd.setCursor(0, 1);
        board.lcd.print("="+String(result));
    } else {
        board.lcd.setCursor(0, 1);
        board.lcd.print("DIV BY 0 FAIL");
    }

    board.currentState = STATE_WAIT;
}

void updateWait(char key) {
    if (key != NO_KEY) {
        reset();

        if (key != board.CLR) {
            board.data += String(key);
        }
        writeDisplay(board.data);
    }
}

void reset() {
    board.currentState = STATE_FIRST;
    board.lcd.clear();
    board.data = "";
}

void writeDisplay(String text){
    board.lcd.clear();
    board.lcd.print(text);
}