#include <Arduino.h>
#include <math.h> // Include for sin, cos, tan, atan, atan2, etc.

// Define PI as a constant using double precision
const double PIii = 3.14159265358979323846264338327950288419716939937510;

// Declaration for infix to postfix conversion and evaluation
class CharStack {
private:
    char* arr;
    int capacity;
    int top;
public:
    CharStack(int size) : capacity(size), arr(new char[size]), top(-1) {}
    ~CharStack() { delete[] arr; }
    void push(char element) { if (top < capacity - 1) arr[++top] = element; }
    char pop() { return top >= 0 ? arr[top--] : '\0'; }
    char peek() { return top >= 0 ? arr[top] : '\0'; }
    bool isEmpty() { return top < 0; }
};

class FloatStack {
private:
    double* arr;
    int top;
public:
    FloatStack(int size) : arr(new double[size]), top(-1) {}
    ~FloatStack() { delete[] arr; }
    void push(double element) { arr[++top] = element; }
    double pop() { return arr[top--]; }
    bool isEmpty() const { return top < 0; }
};

String infixToPostfix(String infix);
int precedence(char op);
bool isOperator(char c);
double evaluatePostfix(const String& postfix);
String addZeroBeforeUnaryMinus(const String& infix);
double convertToRadians(double angle);

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

void setup() {
    Serial.begin(9600);
    while (!Serial); // Wait for Serial to initialize

    // Ask the user if they want to use radians or degrees
    Serial.println("Choose mode: Enter 'r' for Radians or 'd' for Degrees:");
    while (!Serial.available()) {
        // Wait for the user to enter 'r' or 'd'
    }

    char mode = Serial.read();
    useRadians = (mode == 'r'); // Set mode based on input

    Serial.println("Enter an infix expression:");
}

void loop() {
    if (Serial.available() > 0) {
        String infix = Serial.readStringUntil('\n');
        infix.trim();

        // Display the original infix expression
        Serial.print("Infix: ");
        Serial.println(infix);

        String postfix = infixToPostfix(infix);

        // Display the postfix expression
        Serial.print("Postfix: ");
        Serial.println(postfix);

        double result = evaluatePostfix(postfix);
        
        Serial.print("Result: ");
        Serial.println(result, 15); // Print result with 15 decimal places

        Serial.println("Enter another infix expression:");
    }
}

String addZeroBeforeUnaryMinus(const String& infix) {
    String modifiedInfix = "";
    bool lastWasOperatorOrStart = true;

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

        if (ch == '-' && lastWasOperatorOrStart) {
            modifiedInfix += "(0-";
            i++;
            while (i < infix.length() && (isdigit(infix[i]) || infix[i] == '.')) {
                modifiedInfix += infix[i];
                i++;
            }
            i--;
            modifiedInfix += ")";
        } else {
            modifiedInfix += ch;
        }

        lastWasOperatorOrStart = isOperator(ch) || ch == '(';
    }
    return modifiedInfix;
}

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];

        if (infix.substring(i, i + 3) == "sin") {
            operatorStack.push('s');
            i += 2; // Skip past "sin"
            expectOperand = true;
        } else if (infix.substring(i, i + 3) == "cos") {
            operatorStack.push('c');
            i += 2; // Skip past "cos"
            expectOperand = true;
        } else if (infix.substring(i, i + 3) == "tan") {
            operatorStack.push('t');
            i += 2; // Skip past "tan"
            expectOperand = true;
        } else if (infix.substring(i, i + 4) == "atan") {
            operatorStack.push('a');
            i += 3; // Skip past "atan"
            expectOperand = true;
        } else if (infix.substring(i, i + 4) == "atan2") {
            operatorStack.push('z');
            i += 4; // Skip past "atan2"
            expectOperand = true;
        } else if (infix.substring(i, i + 2) == "pi") {
            postfix += String(PIii, 15);
            postfix += ' ';
            expectOperand = false;
            i++;
        } else if (isdigit(currentChar) || (currentChar == '-' && expectOperand)) {
            if (currentChar == '-') {
                postfix += '0';
                postfix += ' ';
            }
            postfix += currentChar;
            while (i + 1 < infix.length() && (isdigit(infix[i + 1]) || infix[i + 1] == '.')) {
                postfix += infix[++i];
            }
            postfix += ' ';
            expectOperand = false;
        } else if (isOperator(currentChar)) {
            while (!operatorStack.isEmpty() && precedence(operatorStack.peek()) >= precedence(currentChar)) {
                postfix += operatorStack.pop();
                postfix += ' ';
            }
            operatorStack.push(currentChar);
            expectOperand = true;
        } else if (currentChar == '(') {
            operatorStack.push(currentChar);
            expectOperand = true;
        } else if (currentChar == ')') {
            while (!operatorStack.isEmpty() && operatorStack.peek() != '(') {
                postfix += operatorStack.pop();
                postfix += ' ';
            }
            operatorStack.pop(); // Remove '(' from the stack
            expectOperand = false;
        }
    }

    while (!operatorStack.isEmpty()) {
        postfix += operatorStack.pop();
        postfix += ' ';
    }
    postfix.trim();
    return postfix;
}

int precedence(char op) {
    switch (op) {
        case '+': case '-': return 1;
        case '*': case '/': case '%': return 2;
        case '^': return 3;
        case 's': case 'c': case 't': case 'a': case 'z': return 4; // Include trig functions
        default: return -1;
    }
}

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

double convertToRadians(double angle) {
    if (useRadians) return angle;
    return angle * (PIii / 180.0); // Convert degrees to radians
}

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

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

        if (isOperator(postfix[i])) {
            double val2 = stack.pop();
            double 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 '/': stack.push(val1 / val2); break;
                case '^': stack.push(pow(val1, val2)); break;
            }
        } else if (isdigit(postfix[i]) || postfix[i] == '.') {
            String numStr = "";
            while (i < postfix.length() && (isdigit(postfix[i]) || postfix[i] == '.')) {
                numStr += postfix[i++];
            }
            stack.push(numStr.toDouble());
            continue;
        } else if (postfix[i] == 's') { // sin
            stack.push(sin(convertToRadians(stack.pop())));
            i++;
        } else if (postfix[i] == 'c') { // cos
            stack.push(cos(convertToRadians(stack.pop())));
            i++;
        } else if (postfix[i] == 't') { // tan
            stack.push(tan(convertToRadians(stack.pop())));
            i++;
        } else if (postfix[i] == 'a') { // atan
            stack.push(atan(stack.pop()));
            i++;
        } else if (postfix[i] == 'z') { // atan2
            double val2 = stack.pop();
            double val1 = stack.pop();
            stack.push(atan2(val1, val2));
            i++;
        }
        i++;
    }

    return stack.pop();
}