#include <Arduino.h>
#include <math.h>
#include <string.h>

class CharStack {
private:
    int top;
    int maxSize;
    String* stackArray;

public:
    CharStack(int size) {
        maxSize = size;
        stackArray = new String[maxSize];
        top = -1;
    }

    ~CharStack() {
        delete[] stackArray;
    }

    void push(String item) {
        if (top < maxSize - 1) {
            stackArray[++top] = item;
        }
    }

    String pop() {
        if (top >= 0) {
            return stackArray[top--];
        }
        return ""; // return empty string if stack is empty
    }

    String peek() {
        if (top >= 0) {
            return stackArray[top];
        }
        return ""; // return empty string if stack is empty
    }

    bool isEmpty() {
        return top == -1;
    }
};

class FloatStack {
private:
    int top;
    int maxSize;
    float* stackArray;

public:
    FloatStack(int size) {
        maxSize = size;
        stackArray = new float[maxSize];
        top = -1;
    }

    ~FloatStack() {
        delete[] stackArray;
    }

    void push(float item) {
        if (top < maxSize - 1) {
            stackArray[++top] = item;
        }
    }

    float pop() {
        if (top >= 0) {
            return stackArray[top--];
        }
        return 0; // return 0 if stack is empty
    }

    bool isEmpty() {
        return top == -1;
    }
};

// Define the constant PI
const double PIii = 3.14159265358979323846264338327950288419716939937510;

bool useRadians = true; // Global flag for radian or degree mode

String addZeroBeforeUnaryMinus(const String& infix) {
    String result = "";
    for (int i = 0; i < infix.length(); i++) {
        if (infix[i] == '-' && (i == 0 || infix[i - 1] == '(')) {
            result += "0-"; // Add zero before unary minus
        } else {
            result += infix[i];
        }
    }
    return result;
}

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

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

String infixToPostfix(String infix) {
    infix = addZeroBeforeUnaryMinus(infix);
    CharStack operatorStack(100);
    String postfix = "";
    bool expectOperand = true;

    for (int i = 0; i < infix.length(); i++) {
        char currentChar = infix[i];

        // Check for trigonometric functions
        if (infix.substring(i, i + 3) == "sin") {
            operatorStack.push("sin"); // Directly use "sin"
            i += 2; // Skip past "sin"
            expectOperand = true;
        } else if (infix.substring(i, i + 3) == "cos") {
            operatorStack.push("cos"); // Directly use "cos"
            i += 2; // Skip past "cos"
            expectOperand = true;
        } else if (infix.substring(i, i + 3) == "tan") {
            operatorStack.push("tan"); // Directly use "tan"
            i += 2; // Skip past "tan"
            expectOperand = true;
        } else if (infix.substring(i, i + 4) == "atan") {
            operatorStack.push("atan"); // Directly use "atan"
            i += 3; // Skip past "atan"
            expectOperand = true;
        } else if (infix.substring(i, i + 5) == "atan2") {
            operatorStack.push("atan2"); // Directly use "atan2"
            i += 4; // Skip past "atan2"
            expectOperand = true;
        } else if (infix.substring(i, i + 4) == "asin") {
            operatorStack.push("asin"); // Directly use "asin"
            i += 3; // Skip past "asin"
            expectOperand = true;
        } else if (infix.substring(i, i + 4) == "acos") {
            operatorStack.push("acos"); // Directly use "acos"
            i += 3; // Skip past "acos"
            expectOperand = true;
        } else if (isdigit(currentChar) || currentChar == '.') {
            // Handle numbers
            String numStr = "";
            while (i < infix.length() && (isdigit(infix[i]) || infix[i] == '.')) {
                numStr += infix[i++];
            }
            postfix += numStr + " ";
            i--; // Adjust index after the inner while
            expectOperand = false;
        } else if (currentChar == '(') {
            operatorStack.push("(");
            expectOperand = true;
        } else if (currentChar == ')') {
            while (!operatorStack.isEmpty() && operatorStack.peek() != "(") {
                postfix += operatorStack.pop() + " ";
            }
            operatorStack.pop(); // Remove the '(' from the stack
            expectOperand = false;
        } else if (isOperator(currentChar)) {
            while (!operatorStack.isEmpty() && precedence(operatorStack.peek()[0]) >= precedence(currentChar)) {
                postfix += operatorStack.pop() + " ";
            }
            operatorStack.push(String(currentChar));
            expectOperand = true;
        }
    }

    while (!operatorStack.isEmpty()) {
        postfix += operatorStack.pop() + " ";
    }

    return postfix;
}

float evaluatePostfix(const String& postfix) {
    FloatStack stack(50);
    int i = 0;

    while (i < postfix.length()) {
        if (postfix[i] == ' ') {
            i++; // Skip spaces
            continue;
        }

        if (isOperator(postfix[i])) {
            float val2 = stack.pop();
            float val1 = stack.pop();
            switch (postfix[i]) {
                case '+': stack.push(val1 + val2); break;
                case '-': stack.push(val1 - val2); break;
                case '*': stack.push(val1 * val2); break;
                case '/':
                    if (val2 != 0) {
                        stack.push(val1 / val2);
                    } else {
                        // Handle division by zero appropriately
                    }
                    break;
                case '^': stack.push(pow(val1, val2)); break;
            }
        } else if (isdigit(postfix[i]) || postfix[i] == '-') {
            // Handle numbers
            String numStr = "";
            while (i < postfix.length() && (isdigit(postfix[i]) || postfix[i] == '.' || (numStr.length() == 0 && postfix[i] == '-'))) {
                numStr += postfix[i++];
            }
            stack.push(numStr.toFloat());
            continue;
        } else if (postfix.substring(i, i + 3) == "sin") {
            float angle = stack.pop();
            stack.push(sin(angle));
            i += 2; // Skip past "sin"
        } else if (postfix.substring(i, i + 3) == "cos") {
            float angle = stack.pop();
            stack.push(cos(angle));
            i += 2; // Skip past "cos"
        } else if (postfix.substring(i, i + 3) == "tan") {
            float angle = stack.pop();
            stack.push(tan(angle));
            i += 2; // Skip past "tan"
        } else if (postfix.substring(i, i + 4) == "atan") {
            float value = stack.pop();
            stack.push(atan(value));
            i += 3; // Skip past "atan"
        } else if (postfix.substring(i, i + 5) == "atan2") {
            float val2 = stack.pop();
            float val1 = stack.pop();
            stack.push(atan2(val1, val2));
            i += 4; // Skip past "atan2"
        } else if (postfix.substring(i, i + 4) == "asin") {
            float value = stack.pop();
            stack.push(asin(value));
            i += 3; // Skip past "asin"
        } else if (postfix.substring(i, i + 4) == "acos") {
            float value = stack.pop();
            stack.push(acos(value));
            i += 3; // Skip past "acos"
        }
        i++;
    }
    return stack.pop(); // Final result should be the only number left on the stack
}

void setup() {
    Serial.begin(9600);
    // Example usage
    String infix = "sin(0.5) + cos(0.5) * 2 ^ 3";
    String postfix = infixToPostfix(infix);
    Serial.print("Infix: ");
    Serial.println(infix);
    Serial.print("Postfix: ");
    Serial.println(postfix);
    double result = evaluatePostfix(postfix);
    Serial.print("Result: ");
    Serial.println(result);
}

void loop() {
    // Placeholder for main loop
}