/*
 * ESP32 BASIC Scientific Calculator
 * Version: 1.13
 * Date: 2025-01-31
 * 
 * Features:
 *   - Trig: SIN, COS, TAN, ASIN, ACOS, ATAN, ATN, ATAN2, ATAN2D
 *   - Trig (degrees): SIND, COSD, TAND, ASIND, ACOSD, ATAND
 *   - Reciprocal: SEC, CSC, COT, ASEC, ACSC, ACOT
 *   - Reciprocal (degrees): SECD, CSCD, COTD, ASECD, ACSCD, ACOTD
 *   - Hyperbolic: SINH, COSH, TANH, ASINH, ACOSH, ATANH
 *   - Math: SQRT, SQR, LN, LOG, LOG2, EXP, POW, ABS, SGN
 *   - Rounding: FLOOR, CEIL, ROUND, INT, FIX, FRAC
 *   - Other: FACT, MIN, MAX, HYPOT, RND, NOT
 *   - Operators: + - * / % ^ < > = <> != <= >= AND OR XOR NOT
 *   - Variables: 1-8 chars, Arrays: 1D/2D/3D
 *   - Control: FOR/NEXT, WHILE/WEND, IF/THEN/ELSE, GOTO, GOSUB/RETURN
 *   - I/O: PRINT, INPUT
 *   - Comments: REM, '
 *   - Constants: PI, E
 */

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

// --- Configuration ---
#define STACK_SIZE 100
#define MAX_PROGRAM_LINES 1000
#define MAX_LOOP_DEPTH 32
#define MAX_GOSUB_DEPTH 32

// --- Constants ---
#define PI_CONST 3.141592653589793
#define E_CONST  2.718281828459045

// =================================================================
// Global Variables for BASIC Interpreter
// =================================================================
// Variable System - supports up to 100 variables with 8-char names
// =================================================================
#define MAX_VARIABLES 100
#define MAX_VAR_NAME_LEN 8
#define MAX_DIMENSIONS 3

struct Variable {
  String name;
  double value;
  double* arrayData;
  int dims[MAX_DIMENSIONS];  // Size of each dimension
  int numDims;               // Number of dimensions (0=scalar, 1-3=array)
  int totalSize;             // Total elements
};
Variable variables[MAX_VARIABLES];
int variableCount = 0;

String programLines[MAX_PROGRAM_LINES];
int    lineNumbers[MAX_PROGRAM_LINES];
int    programSize = 0;
enum ProgramState { IDLE, RUNNING, AWAIT_INPUT };
ProgramState programState = IDLE;
int pc = 0;
int awaitingInputForVarIndex = -1;
String awaitingInputVarName = "";
String inputBuffer = "";
int printPrecision = 6;

// GOSUB return stack
int gosubStack[MAX_GOSUB_DEPTH];
int gosubStackPtr = -1;

struct ForLoopState {
  String varName;
  double endValue;
  double stepValue;
  int returnAddress;
};
ForLoopState forLoopStack[MAX_LOOP_DEPTH];
int forLoopStackPtr = -1;

// WHILE loop stack
struct WhileLoopState {
  int conditionAddress;  // Line where WHILE is (to re-evaluate condition)
};
WhileLoopState whileLoopStack[MAX_LOOP_DEPTH];
int whileLoopStackPtr = -1;

// --- Forward Declarations & Helper Functions ---
void newProgram(); void listProgram(); void startProgram(); void executeNextBasicLine(); void processInput(String input); void executeImmediate(String line); void breakProgram();

// Check if string is a valid variable name (starts with letter, alphanumeric, max 8 chars)
bool isValidVarName(String name) {
  if (name.length() == 0 || name.length() > MAX_VAR_NAME_LEN) return false;
  if (!isAlpha(name.charAt(0))) return false;
  for (int i = 1; i < name.length(); i++) {
    if (!isalnum(name.charAt(i))) return false;
  }
  return true;
}

// Find or create variable, returns index
int getOrCreateVariable(String name) {
  name.toUpperCase();
  // Search existing
  for (int i = 0; i < variableCount; i++) {
    if (variables[i].name == name) return i;
  }
  // Create new
  if (variableCount >= MAX_VARIABLES) return -1;
  variables[variableCount].name = name;
  variables[variableCount].value = 0;
  variables[variableCount].arrayData = NULL;
  variables[variableCount].totalSize = 0;
  variables[variableCount].numDims = 0;
  variables[variableCount].dims[0] = 0;
  variables[variableCount].dims[1] = 0;
  variables[variableCount].dims[2] = 0;
  return variableCount++;
}

// Get variable index (returns -1 if not found)
int findVariable(String name) {
  name.toUpperCase();
  for (int i = 0; i < variableCount; i++) {
    if (variables[i].name == name) return i;
  }
  return -1;
}

// Get variable value by name
double getVariableValue(String name) {
  int idx = getOrCreateVariable(name);
  if (idx >= 0) return variables[idx].value;
  return 0;
}

// Set variable value by name
void setVariableValue(String name, double value) {
  int idx = getOrCreateVariable(name);
  if (idx >= 0) variables[idx].value = value;
}

// Calculate linear index from multi-dimensional indices
int calcLinearIndex(int idx, int i1, int i2 = -1, int i3 = -1) {
  if (variables[idx].numDims == 1) {
    return i1;
  } else if (variables[idx].numDims == 2) {
    // Row-major: index = i1 * dim2 + i2
    return i1 * variables[idx].dims[1] + i2;
  } else if (variables[idx].numDims == 3) {
    // Row-major: index = i1 * dim2 * dim3 + i2 * dim3 + i3
    return i1 * variables[idx].dims[1] * variables[idx].dims[2] + i2 * variables[idx].dims[2] + i3;
  }
  return i1;
}

// Get array value (1D) - direct linear index access
double getArrayValue(String name, int i1) {
  int idx = findVariable(name);
  if (idx >= 0 && variables[idx].arrayData != NULL) {
    if (i1 >= 0 && i1 < variables[idx].totalSize) {
      return variables[idx].arrayData[i1];
    }
  }
  return 0;
}

// Get array value (2D)
double getArrayValue2D(String name, int i1, int i2) {
  int idx = findVariable(name);
  if (idx >= 0 && variables[idx].arrayData != NULL && variables[idx].numDims >= 2) {
    int linearIdx = calcLinearIndex(idx, i1, i2);
    if (linearIdx >= 0 && linearIdx < variables[idx].totalSize) {
      return variables[idx].arrayData[linearIdx];
    }
  }
  return 0;
}

// Get array value (3D)
double getArrayValue3D(String name, int i1, int i2, int i3) {
  int idx = findVariable(name);
  if (idx >= 0 && variables[idx].arrayData != NULL && variables[idx].numDims >= 3) {
    int linearIdx = calcLinearIndex(idx, i1, i2, i3);
    if (linearIdx >= 0 && linearIdx < variables[idx].totalSize) {
      return variables[idx].arrayData[linearIdx];
    }
  }
  return 0;
}

// Set array value (1D, auto-grow for 1D only)
void setArrayValue(String name, int i1, double value) {
  if (i1 < 0) return;
  int idx = getOrCreateVariable(name);
  if (idx < 0) return;
  
  // Auto-grow array if needed (only for 1D)
  if (variables[idx].arrayData == NULL || i1 >= variables[idx].totalSize) {
    int newSize = i1 + 1;
    if (newSize < 10) newSize = 10;
    double* newArr = new double[newSize];
    for (int i = 0; i < newSize; i++) newArr[i] = 0;
    if (variables[idx].arrayData != NULL) {
      for (int i = 0; i < variables[idx].totalSize; i++) {
        newArr[i] = variables[idx].arrayData[i];
      }
      delete[] variables[idx].arrayData;
    }
    variables[idx].arrayData = newArr;
    variables[idx].totalSize = newSize;
    variables[idx].numDims = 1;
    variables[idx].dims[0] = newSize;
  }
  variables[idx].arrayData[i1] = value;
}

// Set array value (2D)
void setArrayValue2D(String name, int i1, int i2, double value) {
  int idx = findVariable(name);
  if (idx < 0 || variables[idx].arrayData == NULL || variables[idx].numDims < 2) {
    Serial.println("Array not dimensioned for 2D!");
    return;
  }
  if (i1 < 0 || i1 >= variables[idx].dims[0] || i2 < 0 || i2 >= variables[idx].dims[1]) {
    Serial.println("Array index out of bounds!");
    return;
  }
  int linearIdx = calcLinearIndex(idx, i1, i2);
  variables[idx].arrayData[linearIdx] = value;
}

// Set array value (3D)
void setArrayValue3D(String name, int i1, int i2, int i3, double value) {
  int idx = findVariable(name);
  if (idx < 0 || variables[idx].arrayData == NULL || variables[idx].numDims < 3) {
    Serial.println("Array not dimensioned for 3D!");
    return;
  }
  if (i1 < 0 || i1 >= variables[idx].dims[0] || i2 < 0 || i2 >= variables[idx].dims[1] || i3 < 0 || i3 >= variables[idx].dims[2]) {
    Serial.println("Array index out of bounds!");
    return;
  }
  int linearIdx = calcLinearIndex(idx, i1, i2, i3);
  variables[idx].arrayData[linearIdx] = value;
}

// DIM array (1D, 2D, or 3D)
void dimArray(String name, int d1, int d2 = 0, int d3 = 0) {
  if (d1 <= 0) return;
  int idx = getOrCreateVariable(name);
  if (idx < 0) return;
  
  if (variables[idx].arrayData != NULL) {
    delete[] variables[idx].arrayData;
  }
  
  int totalSize;
  int numDims;
  
  if (d3 > 0) {
    // 3D array
    numDims = 3;
    totalSize = d1 * d2 * d3;
    variables[idx].dims[0] = d1;
    variables[idx].dims[1] = d2;
    variables[idx].dims[2] = d3;
  } else if (d2 > 0) {
    // 2D array
    numDims = 2;
    totalSize = d1 * d2;
    variables[idx].dims[0] = d1;
    variables[idx].dims[1] = d2;
    variables[idx].dims[2] = 0;
  } else {
    // 1D array
    numDims = 1;
    totalSize = d1;
    variables[idx].dims[0] = d1;
    variables[idx].dims[1] = 0;
    variables[idx].dims[2] = 0;
  }
  
  variables[idx].arrayData = new double[totalSize];
  for (int i = 0; i < totalSize; i++) variables[idx].arrayData[i] = 0;
  variables[idx].totalSize = totalSize;
  variables[idx].numDims = numDims;
}

// Clear all variables
void clearVariables() {
  for (int i = 0; i < variableCount; i++) {
    if (variables[i].arrayData != NULL) {
      delete[] variables[i].arrayData;
      variables[i].arrayData = NULL;
    }
    variables[i].totalSize = 0;
    variables[i].numDims = 0;
    variables[i].dims[0] = 0;
    variables[i].dims[1] = 0;
    variables[i].dims[2] = 0;
    variables[i].name = "";
    variables[i].value = 0;
  }
  variableCount = 0;
}

bool isProgramLineStart(String s) {
  s.trim();
  if (s.length() == 0) return false;
  int i = 0;
  while (i < s.length() && isDigit(s.charAt(i))) {
    i++;
  }
  if (i == 0) return false;
  if (i < s.length() && s.charAt(i) == '.') return false;
  if (i == s.length()) return true;
  return s.charAt(i) == ' ';
}

int roundToInt(double v) {
  return (int)(v + (v >= 0 ? 0.5 : -0.5));
}

String removeSpaces(String s) {
  String out = "";
  out.reserve(s.length());
  for (int i = 0; i < s.length(); i++) {
    char c = s.charAt(i);
    if (c != ' ') out += c;
  }
  return out;
}


// =================================================================
// Stacks & Calculator Class
// =================================================================
class CharStack {
  public: 
    CharStack(int s = STACK_SIZE) {
      maxSize = s;
      stackArray = new String[maxSize];
      top = -1;
    }
    ~CharStack() {
      delete[] stackArray;
    }
    void push(String v) {
      if (!isFull()) stackArray[++top] = v;
    }
    String pop() {
      return !isEmpty() ? stackArray[top--] : "";
    }
    String peek() {
      return !isEmpty() ? stackArray[top] : "";
    }
    bool isEmpty() {
      return top == -1;
    }
    bool isFull() {
      return top == maxSize - 1;
    }
  private: 
    String* stackArray;
    int top, maxSize;
};

class FloatStack {
  public: 
    FloatStack(int s = STACK_SIZE) {
      maxSize = s;
      stackArray = new double[maxSize];
      top = -1;
    }
    ~FloatStack() {
      delete[] stackArray;
    }
    void push(double v) {
      if (!isFull()) stackArray[++top] = v;
    }
    double pop() {
      return !isEmpty() ? stackArray[top--] : NAN;
    }
  private: 
    double* stackArray;
    int top, maxSize;
    bool isEmpty() {
      return top == -1;
    }
    bool isFull() {
      return top == maxSize - 1;
    }
};

class Calculator {
  public: 
    String toPostfix(String i);
    double evaluatePostfix(String p, bool isRad);
  private: 
    int getPrecedence(String o);
    bool isFunc(String t);
    String handleUnary(String i);
    double power(double b, double e);
};

String Calculator::handleUnary(String i) {
  // Preserve spaces around AND, OR, XOR, NOT by replacing with placeholders
  i.replace(" AND ", "~AND~");
  i.replace(" OR ", "~OR~");
  i.replace(" XOR ", "~XOR~");
  i.replace(" ", "");
  // Restore spaces around logical operators
  i.replace("~AND~", " AND ");
  i.replace("~OR~", " OR ");
  i.replace("~XOR~", " XOR ");
  i.replace("(+", "(");
  i.replace("*+", "*");
  i.replace("/+", "/");
  i.replace("++", "+");
  i.replace("-+", "-");
  i.replace("%+", "%");
  i.replace("^+", "^");
  if (i.startsWith("+")) i = i.substring(1);
  if (i.startsWith("-")) i = "0" + i;
  i.replace("(-", "(0-");
  return i;
}

int Calculator::getPrecedence(String o) {
  if (isFunc(o)) return 6;
  if (o == "^") return 5;
  if (o == "*" || o == "/" || o == "%") return 4;
  if (o == "+" || o == "-") return 3;
  if (o == "<" || o == ">" || o == "<=" || o == ">=" || o == "=" || o == "<>" || o == "!=") return 2;
  if (o == "AND" || o == "OR" || o == "XOR") return 1;
  return 0;
}

bool Calculator::isFunc(String t) {
  const char* f[] = {"SIN", "COS", "TAN", "ASIN", "ACOS", "ATAN", "SIND", "COSD", "TAND", 
                     "ASIND", "ACOSD", "ATAND",
                     "SEC", "CSC", "COT", "ASEC", "ACSC", "ACOT",
                     "SECD", "CSCD", "COTD", "ASECD", "ACSCD", "ACOTD",
                     "SINH", "COSH", "TANH", "ASINH", "ACOSH", "ATANH", "LN", "LOG", "EXP", 
                     "SQRT", "SQR", "RAD", "DEG", "ABS", "FLOOR", "CEIL", "ROUND",
                     "RND", "INT", "SGN", "MIN", "MAX", "NOT", "ATN", "FIX", "FRAC",
                     "FACT", "HYPOT", "ATAN2", "ATAN2D", "LOG2", "POW"};
  for (const char* i : f) {
    if (t.equalsIgnoreCase(i)) return true;
  }
  // Array access functions GET1_, GET2_, GET3_
  if (t.length() > 5 && (t.substring(0, 5).equalsIgnoreCase("GET1_") || 
                          t.substring(0, 5).equalsIgnoreCase("GET2_") ||
                          t.substring(0, 5).equalsIgnoreCase("GET3_"))) return true;
  return false;
}

String Calculator::toPostfix(String i) {
  i.replace("π", "pi");
  i = handleUnary(i);
  String p = "";
  CharStack o;
  bool prevWasValueOrRightParen = false;
  
  for (int k = 0; k < i.length(); ) {
    char c = i.charAt(k);
    
    if (isDigit(c) || c == '.') {
      String n = "";
      while (k < i.length() && (isDigit(i.charAt(k)) || i.charAt(k) == '.')) n += i.charAt(k++);
      p += n + " ";
      prevWasValueOrRightParen = true;
      continue;
    }
    
    if (isAlpha(c)) {
      String token = "";
      // Read alphanumeric token (variable names can have digits after first letter)
      while (k < i.length() && (isAlpha(i.charAt(k)) || isDigit(i.charAt(k)))) token += i.charAt(k++);

      String upperToken = token;
      upperToken.toUpperCase();

      // Check for AND, OR, XOR operators
      if (upperToken == "AND" || upperToken == "OR" || upperToken == "XOR") {
        while (!o.isEmpty() && o.peek() != "(" && 
               (getPrecedence(upperToken) <= getPrecedence(o.peek()))) {
          p += o.pop() + " ";
        }
        o.push(upperToken);
        prevWasValueOrRightParen = false;
      }
      else if (isFunc(upperToken)) {
        o.push(upperToken);
      }
      else if (upperToken == "PI") {
        p += "pi ";
        prevWasValueOrRightParen = true;
      }
      else if (upperToken == "E" && (k >= i.length() || !isalnum(i.charAt(k)))) {
        // E constant only if standalone
        p += "e ";
        prevWasValueOrRightParen = true;
      }
      else if (isValidVarName(upperToken)) {
        // Multi-character variable name
        if (k < i.length() && i.charAt(k) == '(') {
          // Array access - count commas to determine dimensions
          int parenDepth = 1;
          int commaCount = 0;
          int startK = k + 1;
          int tempK = startK;
          while (tempK < i.length() && parenDepth > 0) {
            char tc = i.charAt(tempK);
            if (tc == '(') parenDepth++;
            else if (tc == ')') parenDepth--;
            else if (tc == ',' && parenDepth == 1) commaCount++;
            tempK++;
          }
          // GET1_ for 1D, GET2_ for 2D, GET3_ for 3D
          String getTok = String("GET") + String(commaCount + 1) + "_" + upperToken;
          o.push(getTok);
          o.push("(");
          k++;
        } else {
          // Simple variable
          p += "VAR_" + upperToken + " ";
          prevWasValueOrRightParen = true;
        }
      }
      continue;
    }
    
    if (c == '(') {
      o.push("(");
      k++;
      prevWasValueOrRightParen = false;
      continue;
    }
    
    if (c == ')') {
      while (!o.isEmpty() && o.peek() != "(") p += o.pop() + " ";
      o.pop();
      if (!o.isEmpty() && isFunc(o.peek())) p += o.pop() + " ";
      k++;
      prevWasValueOrRightParen = true;
      continue;
    }
    
    // Handle comma (for multi-argument functions)
    if (c == ',') {
      while (!o.isEmpty() && o.peek() != "(") {
        p += o.pop() + " ";
      }
      k++;
      prevWasValueOrRightParen = false;
      continue;
    }
    
    // Check for comparison operators (<=, >=, <>, !=, <, >, =)
    if (c == '<' || c == '>' || c == '!') {
      String o1 = String(c);
      if (k + 1 < i.length()) {
        char next = i.charAt(k + 1);
        if ((c == '<' && (next == '=' || next == '>')) ||
            (c == '>' && next == '=') ||
            (c == '!' && next == '=')) {
          o1 += next;
          k++;
        }
      }
      while (!o.isEmpty() && o.peek() != "(" && 
             (getPrecedence(o1) <= getPrecedence(o.peek()))) {
        p += o.pop() + " ";
      }
      o.push(o1);
      k++;
      prevWasValueOrRightParen = false;
      continue;
    }
    
    // Handle = as comparison (not assignment in expressions)
    if (c == '=') {
      String o1 = "=";
      while (!o.isEmpty() && o.peek() != "(" && 
             (getPrecedence(o1) <= getPrecedence(o.peek()))) {
        p += o.pop() + " ";
      }
      o.push(o1);
      k++;
      prevWasValueOrRightParen = false;
      continue;
    }
    
    if (String("+-*/%^").indexOf(c) != -1) {
      String o1 = String(c);
      if (!prevWasValueOrRightParen && (c == '-' || c == '+')) {
        p += "0 ";
      }
      while (!o.isEmpty() && o.peek() != "(" && 
             ((getPrecedence(o1) < getPrecedence(o.peek())) || 
              (getPrecedence(o1) == getPrecedence(o.peek()) && o1 != "^"))) {
        p += o.pop() + " ";
      }
      o.push(o1);
      k++;
      prevWasValueOrRightParen = false;
      continue;
    }
    k++;
  }
  
  while (!o.isEmpty()) p += o.pop() + " ";
  p.trim();
  return p;
}

double Calculator::evaluatePostfix(String p, bool r) {
  FloatStack v;
  int c = 0;
  
  while (c < p.length()) {
    int n = p.indexOf(' ', c);
    if (n == -1) n = p.length();
    String t = p.substring(c, n);
    c = n + 1;
    if (t.length() == 0) continue;
    
    if (isDigit(t.charAt(0)) || t.charAt(0) == '.' ||
        (t.charAt(0) == '-' && t.length() > 1 && (isDigit(t.charAt(1)) || t.charAt(1) == '.'))) {
      v.push(t.toDouble());
    }
    else if (t.equalsIgnoreCase("pi")) v.push(PI_CONST);
    else if (t.equalsIgnoreCase("e")) v.push(E_CONST);
    else if (t.startsWith("VAR_")) {
      // Multi-character variable
      String varName = t.substring(4);
      v.push(getVariableValue(varName));
    }
    else if (isFunc(t)) {
      String ut = t;
      ut.toUpperCase();
      
      // Handle two-argument functions first
      if (ut == "MIN" || ut == "MAX" || ut == "HYPOT" || ut == "ATAN2" || ut == "ATAN2D" || ut == "POW") {
        double v2 = v.pop();
        double v1 = v.pop();
        if (ut == "MIN") v.push(v1 < v2 ? v1 : v2);
        else if (ut == "MAX") v.push(v1 > v2 ? v1 : v2);
        else if (ut == "HYPOT") v.push(sqrt(v1*v1 + v2*v2));  // Hypotenuse
        else if (ut == "ATAN2") v.push(r ? atan2(v1, v2) : atan2(v1, v2) * 180.0 / PI_CONST);  // atan2(y,x) - mode dependent
        else if (ut == "ATAN2D") v.push(atan2(v1, v2) * 180.0 / PI_CONST);  // atan2(y,x) - always degrees
        else if (ut == "POW") v.push(pow(v1, v2));  // Power function
        continue;
      }
      
      double val = v.pop(), res = 0;
      
      if (ut == "SIN") res = r ? sin(val) : sin(val * PI_CONST / 180.0);
      else if (ut == "COS") res = r ? cos(val) : cos(val * PI_CONST / 180.0);
      else if (ut == "TAN") res = r ? tan(val) : tan(val * PI_CONST / 180.0);
      else if (ut == "ASIN") res = r ? asin(val) : asin(val) * 180.0 / PI_CONST;
      else if (ut == "ACOS") res = r ? acos(val) : acos(val) * 180.0 / PI_CONST;
      else if (ut == "ATAN") res = r ? atan(val) : atan(val) * 180.0 / PI_CONST;
      // Always degrees input
      else if (ut == "SIND") res = sin(val * PI_CONST / 180.0);
      else if (ut == "COSD") res = cos(val * PI_CONST / 180.0);
      else if (ut == "TAND") res = tan(val * PI_CONST / 180.0);
      // Always degrees output
      else if (ut == "ASIND") res = asin(val) * 180.0 / PI_CONST;
      else if (ut == "ACOSD") res = acos(val) * 180.0 / PI_CONST;
      else if (ut == "ATAND") res = atan(val) * 180.0 / PI_CONST;
      // Reciprocal trig (affected by RAD/DEG mode)
      else if (ut == "SEC") {
        double c = r ? cos(val) : cos(val * PI_CONST / 180.0);
        res = (c == 0) ? NAN : 1.0 / c;
      }
      else if (ut == "CSC") {
        double s = r ? sin(val) : sin(val * PI_CONST / 180.0);
        res = (s == 0) ? NAN : 1.0 / s;
      }
      else if (ut == "COT") {
        double t = r ? tan(val) : tan(val * PI_CONST / 180.0);
        res = (t == 0) ? NAN : 1.0 / t;
      }
      // Reciprocal trig - always degrees input
      else if (ut == "SECD") {
        double c = cos(val * PI_CONST / 180.0);
        res = (c == 0) ? NAN : 1.0 / c;
      }
      else if (ut == "CSCD") {
        double s = sin(val * PI_CONST / 180.0);
        res = (s == 0) ? NAN : 1.0 / s;
      }
      else if (ut == "COTD") {
        double t = tan(val * PI_CONST / 180.0);
        res = (t == 0) ? NAN : 1.0 / t;
      }
      // Inverse reciprocal (affected by RAD/DEG mode)
      else if (ut == "ASEC") {
        res = (val == 0) ? NAN : (r ? acos(1.0/val) : acos(1.0/val) * 180.0 / PI_CONST);
      }
      else if (ut == "ACSC") {
        res = (val == 0) ? NAN : (r ? asin(1.0/val) : asin(1.0/val) * 180.0 / PI_CONST);
      }
      else if (ut == "ACOT") {
        res = (val == 0) ? PI_CONST/2 : (r ? atan(1.0/val) : atan(1.0/val) * 180.0 / PI_CONST);
      }
      // Inverse reciprocal - always degrees output
      else if (ut == "ASECD") {
        res = (val == 0) ? NAN : acos(1.0/val) * 180.0 / PI_CONST;
      }
      else if (ut == "ACSCD") {
        res = (val == 0) ? NAN : asin(1.0/val) * 180.0 / PI_CONST;
      }
      else if (ut == "ACOTD") {
        res = (val == 0) ? 90.0 : atan(1.0/val) * 180.0 / PI_CONST;
      }
      // Hyperbolic
      else if (ut == "SINH") res = sinh(val);
      else if (ut == "COSH") res = cosh(val);
      else if (ut == "TANH") res = tanh(val);
      else if (ut == "ASINH") res = asinh(val);
      else if (ut == "ACOSH") res = acosh(val);
      else if (ut == "ATANH") res = atanh(val);
      else if (ut == "LN") res = log(val);
      else if (ut == "LOG") res = log10(val);
      else if (ut == "LOG2") res = log(val) / log(2.0);  // Log base 2
      else if (ut == "EXP") res = exp(val);
      else if (ut == "SQRT") res = sqrt(val);
      else if (ut == "SQR") res = sqrt(val);  // CPC compatible alias
      else if (ut == "ABS") res = fabs(val);
      else if (ut == "FLOOR") res = floor(val);
      else if (ut == "CEIL") res = ceil(val);
      else if (ut == "ROUND") res = round(val);
      else if (ut == "RAD") res = val * PI_CONST / 180.0;
      else if (ut == "DEG") res = val * 180.0 / PI_CONST;
      // New functions
      else if (ut == "RND") res = (double)random(10000) / 10000.0 * val;  // RND(n) returns 0 to n
      else if (ut == "INT") res = (val >= 0) ? floor(val) : ceil(val);
      else if (ut == "FIX") res = trunc(val);  // Truncate toward zero
      else if (ut == "FRAC") res = val - trunc(val);  // Fractional part
      else if (ut == "SGN") res = (val > 0) ? 1 : ((val < 0) ? -1 : 0);
      else if (ut == "NOT") res = (val == 0) ? 1 : 0;
      else if (ut == "ATN") res = r ? atan(val) : atan(val) * 180.0 / PI_CONST;  // CPC compatible alias
      else if (ut == "FACT") {
        // Factorial
        int n = roundToInt(val);
        if (n < 0) res = NAN;
        else if (n > 170) res = INFINITY;  // Overflow
        else {
          res = 1;
          for (int i = 2; i <= n; i++) res *= i;
        }
      }
      else if (ut.startsWith("GET3_")) {
        // 3D array access GET3_VARNAME
        String arrName = ut.substring(5);
        double i3 = val;
        double i2 = v.pop();
        double i1 = v.pop();
        res = getArrayValue3D(arrName, roundToInt(i1), roundToInt(i2), roundToInt(i3));
      }
      else if (ut.startsWith("GET2_")) {
        // 2D array access GET2_VARNAME
        String arrName = ut.substring(5);
        double i2 = val;
        double i1 = v.pop();
        res = getArrayValue2D(arrName, roundToInt(i1), roundToInt(i2));
      }
      else if (ut.startsWith("GET1_")) {
        // 1D array access GET1_VARNAME
        String arrName = ut.substring(5);
        int idx = roundToInt(val);
        res = getArrayValue(arrName, idx);
      }
      v.push(res);
    } 
    else {
      // Binary operators
      double v2 = v.pop(), v1 = v.pop();
      if (t == "+") v.push(v1 + v2);
      else if (t == "-") v.push(v1 - v2);
      else if (t == "*") v.push(v1 * v2);
      else if (t == "/") v.push((v2 == 0.0) ? NAN : (v1 / v2));
      else if (t == "%") v.push((v2 == 0.0) ? NAN : fmod(v1, v2));
      else if (t == "^") v.push(power(v1, v2));
      // Comparison operators
      else if (t == "<") v.push(v1 < v2 ? 1 : 0);
      else if (t == ">") v.push(v1 > v2 ? 1 : 0);
      else if (t == "<=") v.push(v1 <= v2 ? 1 : 0);
      else if (t == ">=") v.push(v1 >= v2 ? 1 : 0);
      else if (t == "=") v.push(fabs(v1 - v2) < 0.0001 ? 1 : 0);
      else if (t == "<>" || t == "!=") v.push(fabs(v1 - v2) >= 0.0001 ? 1 : 0);
      // Logical operators
      else if (t == "AND") v.push((v1 != 0 && v2 != 0) ? 1 : 0);
      else if (t == "OR") v.push((v1 != 0 || v2 != 0) ? 1 : 0);
      else if (t == "XOR") v.push(((v1 != 0) != (v2 != 0)) ? 1 : 0);
    }
  }
  return v.pop();
}

double Calculator::power(double b, double e) {
  if (e == 0) return 1;
  if (b == 0) {
    if (e > 0) return 0;
    if (e == 0) return 1;
    return NAN;
  }
  if (floor(e) == e && e > 0) {
    long i_e = (long)e;
    double r = 1.0;
    while (i_e > 0) {
      if (i_e % 2 == 1) r *= b;
      b *= b;
      i_e /= 2;
    }
    return r;
  }
  return pow(b, e);
}

Calculator calculator;
bool isRadiansMode = true;

// =================================================================
// BASIC Interpreter Implementation
// =================================================================
int findLineIndex(int lineNum) {
  for (int i = 0; i < programSize; i++) {
    if (lineNumbers[i] == lineNum) return i;
  }
  return -1;
}

// MODIFIED: Added echo parameter to control output
void storeLine(String line, bool echo = false) {
  line.trim();
  int spacePos = line.indexOf(' ');
  if (spacePos == -1 && line.toInt() > 0) {
    spacePos = line.length();
  } else if (spacePos == -1) {
    return;
  }
  
  int lineNum = line.substring(0, spacePos).toInt();
  if (lineNum == 0) return;
  
  String code = line.substring(spacePos);
  code.trim();
  
  int existingIndex = findLineIndex(lineNum);
  
  if (existingIndex != -1) {
    if (code.length() == 0) {
      // Delete line
      programSize--;
      for (int i = existingIndex; i < programSize; i++) {
        lineNumbers[i] = lineNumbers[i + 1];
        programLines[i] = programLines[i + 1];
      }
      if (echo) {
        Serial.print(lineNum);
        Serial.println(" deleted");
      }
    } else {
      // Update line
      programLines[existingIndex] = code;
      if (echo) {
        Serial.println(line);
      }
    }
  } else {
    if (code.length() == 0) return;
    if (programSize >= MAX_PROGRAM_LINES) {
      Serial.println("Program memory full!");
      return;
    }
    
    int insertPos = 0;
    while (insertPos < programSize && lineNumbers[insertPos] < lineNum) insertPos++;
    
    for (int i = programSize; i > insertPos; i--) {
      lineNumbers[i] = lineNumbers[i - 1];
      programLines[i] = programLines[i - 1];
    }
    
    lineNumbers[insertPos] = lineNum;
    programLines[insertPos] = code;
    programSize++;
    
    // Echo the stored line
    if (echo) {
      Serial.println(line);
    }
  }
}

void newProgram() {
  programSize = 0;
  clearVariables();
  programState = IDLE;
  forLoopStackPtr = -1;
  gosubStackPtr = -1;
  whileLoopStackPtr = -1;
  Serial.println("OK");
}

void listProgram() {
  for (int i = 0; i < programSize; i++) {
    Serial.print(lineNumbers[i]);
    Serial.print(" ");
    Serial.println(programLines[i]);
  }
}

void startProgram() {
  if (programSize > 0) {
    pc = 0;
    programState = RUNNING;
    forLoopStackPtr = -1;
    gosubStackPtr = -1;
    whileLoopStackPtr = -1;
  }
}

void executeNextBasicLine() {
  if (pc >= programSize) {
    programState = IDLE;
    return;
  }
  
  String line = programLines[pc];
  
  // Handle ' as comment (entire line)
  if (line.startsWith("'")) {
    pc++;
    return;
  }
  
  // Strip inline comments (REM or ' after statement) - but not in quotes
  int remPos = -1;
  int apoPos = -1;
  bool inQuote = false;
  for (int i = 0; i < line.length(); i++) {
    if (line.charAt(i) == '"') inQuote = !inQuote;
    if (!inQuote) {
      if (line.charAt(i) == '\'') {
        apoPos = i;
        break;
      }
      if (i + 3 < line.length()) {
        String sub = line.substring(i, i + 4);
        sub.toUpperCase();
        if (sub == "REM " || sub == " REM") {
          remPos = i;
          break;
        }
      }
    }
  }
  if (apoPos != -1) line = line.substring(0, apoPos);
  else if (remPos != -1) line = line.substring(0, remPos);
  line.trim();
  
  if (line.length() == 0) {
    pc++;
    return;
  }
  
  String command, args;
  int firstSpace = line.indexOf(' ');
  
  if (firstSpace != -1) {
    command = line.substring(0, firstSpace);
    args = line.substring(firstSpace + 1);
  } else {
    command = line;
    args = "";
  }
  
  command.toUpperCase();
  args.trim();
  bool jumped = false;
  
  if (command == "REM") {}
  else if (command == "RAD") {
    isRadiansMode = true;
  }
  else if (command == "DEG") {
    isRadiansMode = false;
  }
  else if (command == "RANDOMIZE") {
    if (args.length() > 0) {
      randomSeed((unsigned long)calculator.evaluatePostfix(calculator.toPostfix(args), isRadiansMode));
    } else {
      randomSeed(analogRead(0) ^ micros());
    }
  }
  else if (command == "END") {
    programState = IDLE;
  }
  else if (command == "PRINT") {
    // Parse PRINT arguments respecting parentheses and quotes
    int i = 0;
    while (i < args.length()) {
      String token = "";
      bool trailingSemicolon = false;
      int parenDepth = 0;
      bool inQuote = false;
      
      // Extract one token
      while (i < args.length()) {
        char c = args.charAt(i);
        
        if (c == '"') {
          inQuote = !inQuote;
          token += c;
          i++;
        }
        else if (inQuote) {
          token += c;
          i++;
        }
        else if (c == '(') {
          parenDepth++;
          token += c;
          i++;
        }
        else if (c == ')') {
          parenDepth--;
          token += c;
          i++;
        }
        else if (c == ';' && parenDepth == 0) {
          trailingSemicolon = true;
          i++;
          break;
        }
        else {
          token += c;
          i++;
        }
      }
      
      token.trim();
      if (token.startsWith("\"") && token.endsWith("\"")) {
        Serial.print(token.substring(1, token.length() - 1));
      } else if (token.length() > 0) {
        Serial.print(calculator.evaluatePostfix(calculator.toPostfix(token), isRadiansMode), printPrecision);
      }
      if (!trailingSemicolon && i >= args.length()) {
        Serial.println();
      }
    }
  }
  else if (command == "INPUT") {
    // Parse INPUT: INPUT var  OR  INPUT "prompt"; var  OR  INPUT "prompt", var
    String prompt = "? ";
    String varPart = args;
    
    // Check for quoted prompt
    if (args.startsWith("\"")) {
      int endQuote = args.indexOf("\"", 1);
      if (endQuote != -1) {
        prompt = args.substring(1, endQuote);
        // Find separator (semicolon or comma)
        int sepPos = endQuote + 1;
        while (sepPos < args.length() && (args.charAt(sepPos) == ' ' || args.charAt(sepPos) == ';' || args.charAt(sepPos) == ',')) {
          sepPos++;
        }
        varPart = args.substring(sepPos);
        varPart.trim();
      }
    }
    
    // Extract variable name (alphanumeric)
    String varName = "";
    for (int i = 0; i < varPart.length() && (isAlpha(varPart.charAt(i)) || (i > 0 && isDigit(varPart.charAt(i)))); i++) {
      varName += varPart.charAt(i);
    }
    varName.toUpperCase();
    
    if (varName.length() > 0) {
      awaitingInputVarName = varName;
      programState = AWAIT_INPUT;
      Serial.print(prompt);
    }
  }
  else if (command == "GOTO") {
    int newPc = findLineIndex(args.toInt());
    if (newPc != -1) {
      pc = newPc;
      jumped = true;
    }
  }
  else if (command == "PREC") {
    int val = args.toInt();
    if (val < 0) val = 0;
    if (val > 15) val = 15;
    printPrecision = val;
  }
  else if (command == "DIM") {
    args.trim();
    int lp = args.indexOf('(');
    int rp = args.lastIndexOf(')');
    if (lp != -1 && rp != -1 && rp > lp + 1) {
      String varName = args.substring(0, lp);
      varName.trim();
      varName.toUpperCase();
      String dimExpr = args.substring(lp + 1, rp);
      
      // Parse dimensions (comma-separated)
      int d1 = 0, d2 = 0, d3 = 0;
      int comma1 = dimExpr.indexOf(',');
      if (comma1 == -1) {
        // 1D array
        d1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(dimExpr), isRadiansMode));
      } else {
        d1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(dimExpr.substring(0, comma1)), isRadiansMode));
        String rest = dimExpr.substring(comma1 + 1);
        int comma2 = rest.indexOf(',');
        if (comma2 == -1) {
          // 2D array
          d2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(rest), isRadiansMode));
        } else {
          // 3D array
          d2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(rest.substring(0, comma2)), isRadiansMode));
          d3 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(rest.substring(comma2 + 1)), isRadiansMode));
        }
      }
      
      if (d1 > 0) {
        dimArray(varName, d1, d2, d3);
      }
    }
  }
  else if (command == "IF") {
    String uArgs = args;
    uArgs.toUpperCase();
    int thenPos = uArgs.indexOf("THEN");
    if (thenPos == -1) return;
    
    String condStr = args.substring(0, thenPos);
    condStr.trim();
    String afterThen = args.substring(thenPos + 4);
    afterThen.trim();
    
    // Find ELSE (but not inside quotes)
    int elsePos = -1;
    bool inQuote = false;
    String uAfterThen = afterThen;
    uAfterThen.toUpperCase();
    for (int i = 0; i < uAfterThen.length() - 3; i++) {
      if (afterThen.charAt(i) == '"') inQuote = !inQuote;
      if (!inQuote && uAfterThen.substring(i, i + 4) == "ELSE") {
        elsePos = i;
        break;
      }
    }
    
    String thenPart, elsePart;
    if (elsePos != -1) {
      thenPart = afterThen.substring(0, elsePos);
      elsePart = afterThen.substring(elsePos + 4);
      thenPart.trim();
      elsePart.trim();
    } else {
      thenPart = afterThen;
      elsePart = "";
    }
    
    // Evaluate condition as expression (supports AND, OR, XOR, comparisons)
    double condResult = calculator.evaluatePostfix(calculator.toPostfix(condStr), isRadiansMode);
    bool conditionMet = (condResult != 0);
    
    // Determine which part to execute
    String execPart = conditionMet ? thenPart : elsePart;
    
    if (execPart.length() > 0) {
      // Check if it's a line number (GOTO)
      bool isLineNum = true;
      for (int i = 0; i < execPart.length(); i++) {
        if (!isDigit(execPart.charAt(i))) {
          isLineNum = false;
          break;
        }
      }
      
      if (isLineNum) {
        // GOTO line number
        int targetLine = execPart.toInt();
        int newPc = findLineIndex(targetLine);
        if (newPc != -1) {
          pc = newPc;
          jumped = true;
        }
      } else {
        // Execute as statement - recursively call executeNextBasicLine logic
        // Store the statement temporarily and process it
        String tempLine = execPart;
        String tempCommand, tempArgs;
        
        // Parse command
        tempLine.trim();
        String uTempLine = tempLine;
        uTempLine.toUpperCase();
        
        int spacePos = tempLine.indexOf(' ');
        if (spacePos != -1) {
          tempCommand = uTempLine.substring(0, spacePos);
          tempArgs = tempLine.substring(spacePos + 1);
        } else {
          tempCommand = uTempLine;
          tempArgs = "";
        }
        
        // Handle common statements
        if (tempCommand == "PRINT") {
          // Execute PRINT
          int i = 0;
          while (i < tempArgs.length()) {
            String token = "";
            bool inQ = false;
            int parenDepth = 0;
            bool trailingSemicolon = false;
            
            while (i < tempArgs.length()) {
              char c = tempArgs.charAt(i);
              if (c == '"') {
                inQ = !inQ;
                token += c;
                i++;
              } else if (inQ) {
                token += c;
                i++;
              } else if (c == '(') {
                parenDepth++;
                token += c;
                i++;
              } else if (c == ')') {
                parenDepth--;
                token += c;
                i++;
              } else if (c == ';' && parenDepth == 0) {
                trailingSemicolon = true;
                i++;
                break;
              } else {
                token += c;
                i++;
              }
            }
            
            token.trim();
            if (token.startsWith("\"") && token.endsWith("\"")) {
              Serial.print(token.substring(1, token.length() - 1));
            } else if (token.length() > 0) {
              Serial.print(calculator.evaluatePostfix(calculator.toPostfix(token), isRadiansMode), printPrecision);
            }
            if (!trailingSemicolon && i >= tempArgs.length()) {
              Serial.println();
            }
          }
        }
        else if (tempCommand == "GOTO") {
          int targetLine = tempArgs.toInt();
          int newPc = findLineIndex(targetLine);
          if (newPc != -1) {
            pc = newPc;
            jumped = true;
          }
        }
        else if (tempCommand == "GOSUB") {
          int targetLine = tempArgs.toInt();
          int targetPc = findLineIndex(targetLine);
          if (targetPc != -1 && gosubStackPtr < MAX_GOSUB_DEPTH - 1) {
            gosubStackPtr++;
            gosubStack[gosubStackPtr] = pc;
            pc = targetPc;
            jumped = true;
          }
        }
        else if (tempCommand == "RETURN") {
          if (gosubStackPtr >= 0) {
            pc = gosubStack[gosubStackPtr];
            gosubStackPtr--;
          }
        }
        else if (tempCommand == "END") {
          programState = IDLE;
        }
        else {
          // Try variable assignment
          int eqPos = tempLine.indexOf('=');
          if (eqPos != -1 && isAlpha(tempLine.charAt(0))) {
            char beforeEq = (eqPos > 0) ? tempLine.charAt(eqPos - 1) : ' ';
            if (beforeEq != '<' && beforeEq != '>' && beforeEq != '!') {
              String lhs = tempLine.substring(0, eqPos);
              lhs.trim();
              String rhs = tempLine.substring(eqPos + 1);
              rhs.trim();
              
              if (lhs.indexOf('(') != -1 && lhs.endsWith(")")) {
                // Array assignment
                int lp = lhs.indexOf('(');
                int rp = lhs.lastIndexOf(')');
                String varName = lhs.substring(0, lp);
                varName.trim();
                varName.toUpperCase();
                String idxExpr = lhs.substring(lp + 1, rp);
                double value = calculator.evaluatePostfix(calculator.toPostfix(rhs), isRadiansMode);
                
                int commaCount = 0;
                for (int ci = 0; ci < idxExpr.length(); ci++) {
                  if (idxExpr.charAt(ci) == ',') commaCount++;
                }
                
                if (commaCount == 0) {
                  int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr), isRadiansMode));
                  setArrayValue(varName, i1, value);
                } else if (commaCount == 1) {
                  int comma = idxExpr.indexOf(',');
                  int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(0, comma)), isRadiansMode));
                  int i2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma + 1)), isRadiansMode));
                  setArrayValue2D(varName, i1, i2, value);
                } else if (commaCount == 2) {
                  int comma1 = idxExpr.indexOf(',');
                  int comma2 = idxExpr.indexOf(',', comma1 + 1);
                  int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(0, comma1)), isRadiansMode));
                  int i2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma1 + 1, comma2)), isRadiansMode));
                  int i3 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma2 + 1)), isRadiansMode));
                  setArrayValue3D(varName, i1, i2, i3, value);
                }
              } else {
                // Simple variable
                String varName = lhs;
                varName.toUpperCase();
                if (isValidVarName(varName)) {
                  setVariableValue(varName, calculator.evaluatePostfix(calculator.toPostfix(rhs), isRadiansMode));
                }
              }
            }
          }
        }
      }
    }
  }
  else if (command == "FOR") {
    String uArgs = args;
    uArgs.toUpperCase();
    int eqPos = args.indexOf('=');
    int toPos = uArgs.indexOf("TO");
    int stepPos = uArgs.indexOf("STEP");
    
    if (eqPos != -1 && toPos != -1) {
      String varName = args.substring(0, eqPos);
      varName.trim();
      varName.toUpperCase();
      String startExpr = args.substring(eqPos + 1, toPos);
      String endExpr, stepExpr;
      
      if (stepPos != -1) {
        endExpr = args.substring(toPos + 2, stepPos);
        stepExpr = args.substring(stepPos + 4);
      } else {
        endExpr = args.substring(toPos + 2);
        stepExpr = "1";
      }
      
      if (forLoopStackPtr >= MAX_LOOP_DEPTH - 1) {
        Serial.println("Loop depth exceeded!");
        programState = IDLE;
      } else {
        forLoopStackPtr++;
        forLoopStack[forLoopStackPtr].varName = varName;
        forLoopStack[forLoopStackPtr].endValue = calculator.evaluatePostfix(calculator.toPostfix(endExpr), isRadiansMode);
        forLoopStack[forLoopStackPtr].stepValue = calculator.evaluatePostfix(calculator.toPostfix(stepExpr), isRadiansMode);
        forLoopStack[forLoopStackPtr].returnAddress = pc;
        setVariableValue(varName, calculator.evaluatePostfix(calculator.toPostfix(startExpr), isRadiansMode));
      }
    }
  }
  else if (command == "NEXT") {
    String varName = args;
    varName.trim();
    varName.toUpperCase();
    if (forLoopStackPtr < 0 || forLoopStack[forLoopStackPtr].varName != varName) {
      Serial.println("NEXT without FOR error!");
      programState = IDLE;
    } else {
      double val = getVariableValue(varName) + forLoopStack[forLoopStackPtr].stepValue;
      setVariableValue(varName, val);
      bool loopContinues = (forLoopStack[forLoopStackPtr].stepValue >= 0) 
        ? (val <= forLoopStack[forLoopStackPtr].endValue + 0.0001) 
        : (val >= forLoopStack[forLoopStackPtr].endValue - 0.0001);
      
      if (loopContinues) {
        pc = forLoopStack[forLoopStackPtr].returnAddress + 1;
        jumped = true;
      } else {
        forLoopStackPtr--;
      }
    }
  }
  else if (command == "WHILE") {
    // Evaluate condition
    double condResult = calculator.evaluatePostfix(calculator.toPostfix(args), isRadiansMode);
    
    if (condResult != 0) {
      // Condition true - push onto stack and continue
      if (whileLoopStackPtr >= MAX_LOOP_DEPTH - 1) {
        Serial.println("WHILE loop depth exceeded!");
        programState = IDLE;
      } else {
        whileLoopStackPtr++;
        whileLoopStack[whileLoopStackPtr].conditionAddress = pc;
      }
    } else {
      // Condition false - skip to matching WEND
      int depth = 1;
      int searchPc = pc + 1;
      while (searchPc < programSize && depth > 0) {
        String searchLine = programLines[searchPc];
        searchLine.trim();
        String uSearchLine = searchLine;
        uSearchLine.toUpperCase();
        if (uSearchLine.startsWith("WHILE ") || uSearchLine == "WHILE") {
          depth++;
        } else if (uSearchLine.startsWith("WEND") || uSearchLine == "WEND") {
          depth--;
        }
        searchPc++;
      }
      if (depth == 0) {
        pc = searchPc - 1;  // Will be incremented at end
      } else {
        Serial.println("WHILE without WEND!");
        programState = IDLE;
      }
    }
  }
  else if (command == "WEND") {
    if (whileLoopStackPtr < 0) {
      Serial.println("WEND without WHILE!");
      programState = IDLE;
    } else {
      // Jump back to WHILE line to re-evaluate condition
      pc = whileLoopStack[whileLoopStackPtr].conditionAddress;
      whileLoopStackPtr--;  // Pop before re-evaluating (WHILE will push again if true)
      jumped = true;
    }
  }
  else if (command == "GOSUB") {
    int targetLine = args.toInt();
    int targetPc = findLineIndex(targetLine);
    if (targetPc != -1) {
      if (gosubStackPtr >= MAX_GOSUB_DEPTH - 1) {
        Serial.println("GOSUB stack overflow!");
        programState = IDLE;
      } else {
        gosubStackPtr++;
        gosubStack[gosubStackPtr] = pc;
        pc = targetPc;
        jumped = true;
      }
    }
  }
  else if (command == "RETURN") {
    if (gosubStackPtr < 0) {
      Serial.println("RETURN without GOSUB!");
      programState = IDLE;
    } else {
      pc = gosubStack[gosubStackPtr];
      gosubStackPtr--;
      // pc will be incremented at the end, so we return to line after GOSUB
    }
  }
  else {
    String uLine = line;
    uLine.toUpperCase();
    if (uLine.startsWith("LET ")) {
      line = line.substring(4);
    }
    
    int eqPos = line.indexOf('=');
    if (eqPos != -1) {
      String lhs = line.substring(0, eqPos);
      lhs.trim();
      String rhs = line.substring(eqPos + 1);
      rhs.trim();
      
      // Check for array assignment: VARNAME(index) = value
      if (lhs.indexOf('(') != -1 && lhs.endsWith(")")) {
        int lp = lhs.indexOf('(');
        int rp = lhs.lastIndexOf(')');
        String varName = lhs.substring(0, lp);
        varName.trim();
        varName.toUpperCase();
        if (lp != -1 && rp != -1 && rp > lp + 1) {
          String idxExpr = lhs.substring(lp + 1, rp);
          double value = calculator.evaluatePostfix(calculator.toPostfix(rhs), isRadiansMode);
          
          // Count commas to determine dimensions
          int commaCount = 0;
          for (int ci = 0; ci < idxExpr.length(); ci++) {
            if (idxExpr.charAt(ci) == ',') commaCount++;
          }
          
          if (commaCount == 0) {
            // 1D
            int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr), isRadiansMode));
            setArrayValue(varName, i1, value);
          } else if (commaCount == 1) {
            // 2D
            int comma = idxExpr.indexOf(',');
            int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(0, comma)), isRadiansMode));
            int i2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma + 1)), isRadiansMode));
            setArrayValue2D(varName, i1, i2, value);
          } else if (commaCount == 2) {
            // 3D
            int comma1 = idxExpr.indexOf(',');
            int comma2 = idxExpr.indexOf(',', comma1 + 1);
            int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(0, comma1)), isRadiansMode));
            int i2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma1 + 1, comma2)), isRadiansMode));
            int i3 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma2 + 1)), isRadiansMode));
            setArrayValue3D(varName, i1, i2, i3, value);
          }
        }
      } else {
        // Simple variable assignment
        String varName = lhs;
        varName.toUpperCase();
        if (isValidVarName(varName)) {
          setVariableValue(varName, calculator.evaluatePostfix(calculator.toPostfix(rhs), isRadiansMode));
        }
      }
    }
  }
  
  if (!jumped && programState == RUNNING) {
    pc++;
  }
}

// =================================================================
// Main Setup and Loop
// =================================================================
void setup() {
  Serial.begin(9600);
  while (!Serial) { ; }
  
  // Seed random number generator
  randomSeed(analogRead(0) ^ micros());
  
  newProgram();
  Serial.println("\n--- Arduino BASIC Scientific Calculator ---");
  Serial.println("Commands: RUN, LIST, NEW, BREAK");
  Serial.println("Statements: LET, PRINT, INPUT, GOTO, IF/THEN, FOR/NEXT, DIM, REM, END");
  Serial.println("Functions: SIN, COS, TAN, SQRT, LN, LOG, EXP, ABS, RND, INT, SGN, MIN, MAX");
  Serial.println("Operators: + - * / % ^ AND OR NOT < > <= >= = <>");
  Serial.println("Constants: PI, E");
  Serial.println("Use \\ to paste multiple lines");
  Serial.println("PREC n to set precision (0-15)");
  Serial.println("-------------------------------------");
  isRadiansMode = true;
  Serial.println("Mode: Radians. (Use DEG/RAD to switch)");
  Serial.println("-------------------------------------");
  Serial.print("> ");
}

void breakProgram() {
  programState = IDLE;
  forLoopStackPtr = -1;
  awaitingInputForVarIndex = -1;
  Serial.println("\n*** BREAK ***");
}

void executeImmediate(String line) {
  String safeLine = line;
  safeLine.trim();
  if (safeLine.length() == 0) return;
  
  // Handle ' as comment
  if (safeLine.startsWith("'")) return;
  
  // Strip inline comments (but not in quotes)
  int apoPos = -1;
  bool inQuote = false;
  for (int i = 0; i < safeLine.length(); i++) {
    if (safeLine.charAt(i) == '"') inQuote = !inQuote;
    if (!inQuote && safeLine.charAt(i) == '\'') {
      apoPos = i;
      break;
    }
  }
  if (apoPos != -1) {
    safeLine = safeLine.substring(0, apoPos);
    safeLine.trim();
  }
  if (safeLine.length() == 0) return;

  String upperLine = safeLine;
  upperLine.toUpperCase();
  if (upperLine.startsWith("LET ")) {
    safeLine = safeLine.substring(4);
    safeLine.trim();
    upperLine = safeLine;
    upperLine.toUpperCase();
  }

  // Check for variable assignment - but only if line starts with a letter (variable)
  // followed by = or ( for array, not a command like PRINT
  
  // Skip if it's a known command
  bool isCommand = upperLine.startsWith("PRINT ") || upperLine.startsWith("RUN") || 
                   upperLine.startsWith("LIST") || upperLine.startsWith("NEW") ||
                   upperLine.startsWith("RAD") || upperLine.startsWith("DEG") ||
                   upperLine.startsWith("PREC ") || upperLine.startsWith("DIM ") ||
                   upperLine.startsWith("RANDOMIZE") || upperLine.startsWith("REM") ||
                   upperLine.startsWith("FOR ") || upperLine.startsWith("NEXT ") ||
                   upperLine.startsWith("IF ") || upperLine.startsWith("GOTO ") ||
                   upperLine.startsWith("GOSUB ") || upperLine.startsWith("RETURN") ||
                   upperLine.startsWith("INPUT ") || upperLine.startsWith("END");
  
  if (!isCommand && isAlpha(safeLine.charAt(0))) {
    int eqPos = safeLine.indexOf('=');
    // Make sure we're not matching <= or >= or <> or != 
    if (eqPos > 0) {
      // Check character before = to make sure it's not part of <=, >=, <>, !=
      char beforeEq = safeLine.charAt(eqPos - 1);
      if (beforeEq != '<' && beforeEq != '>' && beforeEq != '!') {
        String lhs = safeLine.substring(0, eqPos);
        lhs.trim();
        String rhs = safeLine.substring(eqPos + 1);
        rhs.trim();
        
        // Check for array assignment: VARNAME(index) = value
        if (lhs.indexOf('(') != -1 && lhs.endsWith(")")) {
          int lp = lhs.indexOf('(');
          int rp = lhs.lastIndexOf(')');
          String varName = lhs.substring(0, lp);
          varName.trim();
          varName.toUpperCase();
          if (lp != -1 && rp != -1 && rp > lp + 1) {
            String idxExpr = lhs.substring(lp + 1, rp);
            double value = calculator.evaluatePostfix(calculator.toPostfix(rhs), isRadiansMode);
            
            // Count commas to determine dimensions
            int commaCount = 0;
            for (int ci = 0; ci < idxExpr.length(); ci++) {
              if (idxExpr.charAt(ci) == ',') commaCount++;
            }
            
            if (commaCount == 0) {
              // 1D
              int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr), isRadiansMode));
              setArrayValue(varName, i1, value);
              Serial.print(varName);
              Serial.print("(");
              Serial.print(i1);
              Serial.print(") = ");
              Serial.println(value, printPrecision);
            } else if (commaCount == 1) {
              // 2D
              int comma = idxExpr.indexOf(',');
              int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(0, comma)), isRadiansMode));
              int i2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma + 1)), isRadiansMode));
              setArrayValue2D(varName, i1, i2, value);
              Serial.print(varName);
              Serial.print("(");
              Serial.print(i1);
              Serial.print(",");
              Serial.print(i2);
              Serial.print(") = ");
              Serial.println(value, printPrecision);
            } else if (commaCount == 2) {
              // 3D
              int comma1 = idxExpr.indexOf(',');
              int comma2 = idxExpr.indexOf(',', comma1 + 1);
              int i1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(0, comma1)), isRadiansMode));
              int i2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma1 + 1, comma2)), isRadiansMode));
              int i3 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(idxExpr.substring(comma2 + 1)), isRadiansMode));
              setArrayValue3D(varName, i1, i2, i3, value);
              Serial.print(varName);
              Serial.print("(");
              Serial.print(i1);
              Serial.print(",");
              Serial.print(i2);
              Serial.print(",");
              Serial.print(i3);
              Serial.print(") = ");
              Serial.println(value, printPrecision);
            }
          }
        } else {
          // Simple variable assignment: VARNAME = value
          String varName = lhs;
          varName.toUpperCase();
          if (isValidVarName(varName)) {
            double res = calculator.evaluatePostfix(calculator.toPostfix(rhs), isRadiansMode);
            setVariableValue(varName, res);
            Serial.print(varName);
            Serial.print(" = ");
            Serial.println(res, printPrecision);
          }
        }
        return;
      }
    }
  }

  String command, args;
  int firstSpace = safeLine.indexOf(' ');
  if (firstSpace != -1) {
    command = safeLine.substring(0, firstSpace);
    args = safeLine.substring(firstSpace + 1);
  } else {
    command = safeLine;
    args = "";
  }

  if (command.equalsIgnoreCase("PRINT")) {
    // Parse PRINT arguments respecting parentheses and quotes
    int i = 0;
    while (i < args.length()) {
      String token = "";
      bool trailingSemicolon = false;
      int parenDepth = 0;
      bool inQuote = false;
      
      // Extract one token
      while (i < args.length()) {
        char c = args.charAt(i);
        
        if (c == '"') {
          inQuote = !inQuote;
          token += c;
          i++;
        }
        else if (inQuote) {
          token += c;
          i++;
        }
        else if (c == '(') {
          parenDepth++;
          token += c;
          i++;
        }
        else if (c == ')') {
          parenDepth--;
          token += c;
          i++;
        }
        else if (c == ';' && parenDepth == 0) {
          trailingSemicolon = true;
          i++;
          break;
        }
        else {
          token += c;
          i++;
        }
      }
      
      token.trim();
      if (token.startsWith("\"") && token.endsWith("\"")) {
        Serial.print(token.substring(1, token.length() - 1));
      } else if (token.length() > 0) {
        Serial.print(calculator.evaluatePostfix(calculator.toPostfix(token), isRadiansMode), printPrecision);
      }
      if (!trailingSemicolon && i >= args.length()) {
        Serial.println();
      }
    }
  } else if (command.equalsIgnoreCase("RUN")) {
    startProgram();
  } else if (command.equalsIgnoreCase("LIST")) {
    listProgram();
  } else if (command.equalsIgnoreCase("NEW")) {
    newProgram();
  } else if (command.equalsIgnoreCase("RANDOMIZE")) {
    if (args.length() > 0) {
      randomSeed((unsigned long)calculator.evaluatePostfix(calculator.toPostfix(args), isRadiansMode));
    } else {
      randomSeed(analogRead(0) ^ micros());
    }
    Serial.println("Randomized.");
  } else if (command.equalsIgnoreCase("RAD")) {
    isRadiansMode = true;
    Serial.println("Radian Mode.");
  } else if (command.equalsIgnoreCase("DEG")) {
    isRadiansMode = false;
    Serial.println("Degree Mode.");
  } else if (command.equalsIgnoreCase("PREC")) {
    args.trim();
    int val = args.toInt();
    if (val < 0) val = 0;
    if (val > 15) val = 15;
    printPrecision = val;
    Serial.print("Precision: ");
    Serial.println(printPrecision);
  } else if (command.equalsIgnoreCase("DIM")) {
    args.trim();
    int lp = args.indexOf('(');
    int rp = args.lastIndexOf(')');
    if (lp != -1 && rp != -1 && rp > lp + 1) {
      String varName = args.substring(0, lp);
      varName.trim();
      varName.toUpperCase();
      String dimExpr = args.substring(lp + 1, rp);
      
      // Parse dimensions (comma-separated)
      int d1 = 0, d2 = 0, d3 = 0;
      int comma1 = dimExpr.indexOf(',');
      if (comma1 == -1) {
        // 1D array
        d1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(dimExpr), isRadiansMode));
        dimArray(varName, d1);
        Serial.print("DIM ");
        Serial.print(varName);
        Serial.print("(");
        Serial.print(d1);
        Serial.println(")");
      } else {
        d1 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(dimExpr.substring(0, comma1)), isRadiansMode));
        String rest = dimExpr.substring(comma1 + 1);
        int comma2 = rest.indexOf(',');
        if (comma2 == -1) {
          // 2D array
          d2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(rest), isRadiansMode));
          dimArray(varName, d1, d2);
          Serial.print("DIM ");
          Serial.print(varName);
          Serial.print("(");
          Serial.print(d1);
          Serial.print(",");
          Serial.print(d2);
          Serial.println(")");
        } else {
          // 3D array
          d2 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(rest.substring(0, comma2)), isRadiansMode));
          d3 = roundToInt(calculator.evaluatePostfix(calculator.toPostfix(rest.substring(comma2 + 1)), isRadiansMode));
          dimArray(varName, d1, d2, d3);
          Serial.print("DIM ");
          Serial.print(varName);
          Serial.print("(");
          Serial.print(d1);
          Serial.print(",");
          Serial.print(d2);
          Serial.print(",");
          Serial.print(d3);
          Serial.println(")");
        }
      }
    }
  } else {
    Serial.println(calculator.evaluatePostfix(calculator.toPostfix(safeLine), isRadiansMode), printPrecision);
  }
}

// MODIFIED: processInput now echoes program lines when pasted
void processInput(String input) {
  if (input.length() == 0) return;

  // If program is awaiting input, treat the whole line as a number (or expression)
  if (programState == AWAIT_INPUT && awaitingInputVarName.length() > 0) {
    input.trim();
    if (input.length() > 0) {
      double res = calculator.evaluatePostfix(calculator.toPostfix(input), isRadiansMode);
      setVariableValue(awaitingInputVarName, res);
      awaitingInputVarName = "";
      programState = RUNNING;
      pc++;
    }
    return;
  }

  // If the input contains backslashes, split by them.
  if (input.indexOf('\\') != -1) {
    int lastPos = 0;
    while (lastPos < input.length()) {
      int slashPos = input.indexOf('\\', lastPos);
      if (slashPos == -1) {
        slashPos = input.length();
      }
      
      String segment = input.substring(lastPos, slashPos);
      segment.trim();
      if (segment.length() > 0) {
        if (isProgramLineStart(segment)) {
          storeLine(segment, true);  // Echo enabled for pasted lines
        } else {
          executeImmediate(segment);
        }
      }
      
      lastPos = slashPos + 1;
    }
  } else {
    // Otherwise, treat the whole line as a single command.
    input.trim();
    if (input.length() > 0) {
      if (isProgramLineStart(input)) {
        storeLine(input, true);  // Echo enabled
      } else {
        executeImmediate(input);
      }
    }
  }
}

void loop() {
  if (programState == RUNNING) {
    executeNextBasicLine();
  }

  if (Serial.available() > 0) {
    char c = Serial.read();

    if (c == '\r' || c == '\n') {
      Serial.println();
      
      String upperInput = inputBuffer;
      upperInput.toUpperCase();
      if (upperInput == "BREAK") {
        if (programState == RUNNING) {
          breakProgram();
        }
      } else {
        processInput(inputBuffer);
      }
      
      inputBuffer = "";
      
      if (programState == IDLE) {
        Serial.print("> ");
      }
    } 
    else if (c == '\b' || c == 127) {
      if (inputBuffer.length() > 0) {
        inputBuffer.remove(inputBuffer.length() - 1);
        Serial.print("\b \b");
      }
    }
    else if (isPrintable(c)) {
      inputBuffer += c;
      Serial.print(c);
    }
  }
}