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

// Define PI as a constant
const float PIii = 3.14159265358979323846;

// 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:
    float* arr;
    int top;
public:
    FloatStack(int size) : arr(new float[size]), top(-1) {}
    ~FloatStack() { delete[] arr; }
    void push(float element) { arr[++top] = element; }
    float pop() { return arr[top--]; }
    bool isEmpty() const { return top < 0; }
};

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

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();
    if (mode == 'd') {
        useRadians = false;
        Serial.println("Mode: Degrees");
    } else {
        useRadians = true;
        Serial.println("Mode: Radians");
    }

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

void loop() {
    // Check if data is available from the Serial Monitor
    if (Serial.available() > 0) {
        // Read the input expression from Serial Monitor
        String infix = Serial.readStringUntil('\n'); // Read the input until newline
        infix.trim(); // Remove any leading/trailing whitespace
        Serial.println("Infix: " + infix);

        // Convert to postfix
        String postfix = infixToPostfix(infix);
        Serial.println("Postfix: " + postfix);

        // Evaluate the postfix expression
        float result = evaluatePostfix(postfix);
        Serial.print("Result: ");
        Serial.println(result, 12); // Print result with 12 decimal places

        // Prompt the user to enter a new expression
        Serial.println("Enter another infix expression:");
    }
}

String addZeroBeforeUnaryMinus(const String& infix) {
    String modifiedInfix = "";
    bool lastWasOperatorOrStart = true; // True at the start or after an operator

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

        if (ch == '-' && lastWasOperatorOrStart) {
            modifiedInfix += "(0-"; // Start unary minus handling
            i++; // Move to the next character (the number or expression after '-')
            while (i < infix.length() && (isdigit(infix[i]) || infix[i] == '.')) {
                modifiedInfix += infix[i]; // Add the number directly after '-'
                i++;
            }
            i--; // Decrement to offset the loop's next increment
            modifiedInfix += ")"; // Close unary minus handling
        } else {
            modifiedInfix += ch; // Just copy the character
        }

        // Update the flag based on current character
        lastWasOperatorOrStart = isOperator(ch) || ch == '(';
    }
    return modifiedInfix;
}

String infixToPostfix(String infix) {
    infix = addZeroBeforeUnaryMinus(infix); // Preprocess the infix expression
    CharStack operatorStack(100); // Sufficient size for the stack
    String postfix = "";
    bool expectOperand = true; // Indicates we expect a number or unary operator next

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

        // Check for functions
        if (infix.substring(i, i + 3) == "sin" || infix.substring(i, i + 3) == "cos" || infix.substring(i, i + 3) == "tan") {
            operatorStack.push(infix[i]); // Push function identifier to the stack
            i += 2; // Skip the next two characters
            expectOperand = true; // Expecting an operand after function
        } else if (infix.substring(i, i + 4) == "atan") {
            if (infix[i + 4] == '2') {
                operatorStack.push('2'); // Use '2' for atan2
                i += 4; // Skip "atan2"
            } else {
                operatorStack.push('a'); // Use 'a' for atan
                i += 3; // Skip "atan"
            }
            expectOperand = true; // Expecting an operand after function
        } else if (infix.substring(i, i + 2) == "pi") {  // Check for pi
            postfix += String(PI); // Append the value of pi to the postfix string
            postfix += ' '; // Ensure there's a space after pi
            expectOperand = false; // After pi, an operator is expected
            i++; // Skip the next character ('i')
        } else if (isdigit(currentChar) || (currentChar == '-' && expectOperand)) {
            // Append the operand (or unary minus) to the postfix string
            if (currentChar == '-') {
                postfix += '0'; // Handle unary minus
                postfix += ' '; // Space before the unary minus operator
            }
            postfix += currentChar;
            while (i + 1 < infix.length() && (isdigit(infix[i + 1]) || infix[i + 1] == '.')) {
                postfix += infix[++i];
            }
            postfix += ' '; // Ensure there's a space after the operand
            expectOperand = false; // After a number, an operator is expected
        } else if (isOperator(currentChar)) {
            while (!operatorStack.isEmpty() && precedence(operatorStack.peek()) >= precedence(currentChar)) {
                postfix += operatorStack.pop();
                postfix += ' '; // Ensure there's a space after popping an operator
            }
            operatorStack.push(currentChar);
            expectOperand = true; // Expecting a number or unary minus after an operator
        } else if (currentChar == '(') {
            operatorStack.push(currentChar);
            expectOperand = true; // Unary minus can follow '('
        } else if (currentChar == ')') {
            while (!operatorStack.isEmpty() && operatorStack.peek() != '(') {
                postfix += operatorStack.pop();
                postfix += ' '; // Ensure there's a space after popping an operator
            }
            operatorStack.pop(); // Remove '(' from the stack
            expectOperand = false; // Flexible expectation after ')'
        }
    }

    // After the while loop that empties the operator stack
    while (!operatorStack.isEmpty()) {
        postfix += operatorStack.pop();
        postfix += ' '; // Ensure there's a space after popping the remaining operators
    }
    postfix.trim(); // Trim any trailing spaces
    return postfix; // Return the trimmed postfix expression
}

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 '2': return 4; // 's' for sin, 'c' for cos, 't' for tan, 'a' for atan, '2' for atan2
        default: return -1;
    }
}

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

float evaluatePostfix(const String& postfix) {
    FloatStack stack(50); // Assume a sufficiently large stack
    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 {
                        Serial.println("Error: Division by zero");
                        return 0.0; // Default value for error
                    }
                    break;
                case '%': stack.push(fmod(val1, val2)); break;
                case '^': stack.push(pow(val1, val2)); break;
            }
            i++;
        } else if (postfix[i] == 's' || postfix[i] == 'c' || postfix[i] == 't' || postfix[i] == 'a' || postfix[i] == '2') {
            if (postfix[i] == '2') {
                float val2 = stack.pop(); // Second argument (y)
                float val1 = stack.pop(); // First argument (x)
                float result = atan2(val2, val1); // atan2(y, x)
                if (!useRadians) {
                    result *= RAD_TO_DEG; // Convert result to degrees if in degree mode
                }
                stack.push(result);
            } else {
                float val = stack.pop();
                float angle = useRadians ? val : val * DEG_TO_RAD; // Convert to radians if in degree mode
                switch (postfix[i]) {
                    case 's': stack.push(sin(angle)); break;
                    case 'c': stack.push(cos(angle)); break;
                    case 't': stack.push(tan(angle)); break;
                    case 'a': stack.push(atan(angle)); break; // atan is arctangent
                }
            }
            i++;
        } else {
            // Parse number, potentially with a negative sign
            String numStr = "";
            while (i < postfix.length() && (isdigit(postfix[i]) || postfix[i] == '.' || (numStr.length() == 0 && postfix[i] == '-'))) {
                numStr += postfix[i++];
            }
            float num = numStr.toFloat();
            stack.push(num);
        }
    }
    return stack.pop(); // Final result should be the only number left on the stack
}