// ================================================================================
// yaZEKA BASIC INTERPRETER v1.19
// "Your Advanced Zero-Error Kernel Architecture"
// ================================================================================
// Version 1.19 - String variables and CPC string functions
// Version 1.18 - Tokenized postfix evaluation, CR/LF line input, optimized IF
// Version 1.17 - Variable index caching for faster variable access
// Version 1.16 - Simple command token lookup (switch instead of if-else)
// Version 1.14 - Expression caching, MILLIS
// ================================================================================

#define YAZEKA_VERSION "1.19"

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

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

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

// =================================================================
// COMMAND TOKENS - for faster string comparison
// =================================================================
#define CMD_UNKNOWN   0
#define CMD_REM       1
#define CMD_RAD       2
#define CMD_DEG       3
#define CMD_RANDOMIZE 4
#define CMD_END       5
#define CMD_PRINT     6
#define CMD_INPUT     7
#define CMD_GOTO      8
#define CMD_PREC      9
#define CMD_DIM       10
#define CMD_IF        11
#define CMD_FOR       12
#define CMD_NEXT      13
#define CMD_WHILE     14
#define CMD_WEND      15
#define CMD_GOSUB     16
#define CMD_RETURN    17
#define CMD_LET       18

// Fast command lookup using first character + length
uint8_t lookupCommand(const String& cmd) {
  int len = cmd.length();
  if (len == 0) return CMD_UNKNOWN;
  char c = cmd.charAt(0);
  
  switch (c) {
    case 'R':
      if (len == 3 && cmd == "REM") return CMD_REM;
      if (len == 3 && cmd == "RAD") return CMD_RAD;
      if (len == 9 && cmd == "RANDOMIZE") return CMD_RANDOMIZE;
      if (len == 6 && cmd == "RETURN") return CMD_RETURN;
      break;
    case 'D':
      if (len == 3 && cmd == "DEG") return CMD_DEG;
      if (len == 3 && cmd == "DIM") return CMD_DIM;
      break;
    case 'E':
      if (len == 3 && cmd == "END") return CMD_END;
      break;
    case 'P':
      if (len == 5 && cmd == "PRINT") return CMD_PRINT;
      if (len == 4 && cmd == "PREC") return CMD_PREC;
      break;
    case 'I':
      if (len == 5 && cmd == "INPUT") return CMD_INPUT;
      if (len == 2 && cmd == "IF") return CMD_IF;
      break;
    case 'G':
      if (len == 4 && cmd == "GOTO") return CMD_GOTO;
      if (len == 5 && cmd == "GOSUB") return CMD_GOSUB;
      break;
    case 'F':
      if (len == 3 && cmd == "FOR") return CMD_FOR;
      break;
    case 'N':
      if (len == 4 && cmd == "NEXT") return CMD_NEXT;
      break;
    case 'W':
      if (len == 5 && cmd == "WHILE") return CMD_WHILE;
      if (len == 4 && cmd == "WEND") return CMD_WEND;
      break;
    case 'L':
      if (len == 3 && cmd == "LET") return CMD_LET;
      break;
  }
  return CMD_UNKNOWN;
}

// =================================================================
// TOKENIZED POSTFIX SYSTEM - No more string parsing!
// =================================================================
// Token types for postfix evaluation
enum PostfixTokenType : uint8_t {
  PT_NUMBER = 1,    // Literal number
  PT_VAR,           // Variable (index in varIndex)
  PT_PI, PT_E,      // Constants
  // Binary operators
  PT_ADD, PT_SUB, PT_MUL, PT_DIV, PT_MOD, PT_POW,
  PT_LT, PT_GT, PT_LE, PT_GE, PT_EQ, PT_NE,
  PT_AND, PT_OR, PT_XOR,
  // Unary functions (one argument)
  PT_SIN, PT_COS, PT_TAN, PT_ASIN, PT_ACOS, PT_ATAN, PT_ATN,
  PT_SIND, PT_COSD, PT_TAND, PT_ASIND, PT_ACOSD, PT_ATAND,
  PT_SEC, PT_CSC, PT_COT, PT_ASEC, PT_ACSC, PT_ACOT,
  PT_SECD, PT_CSCD, PT_COTD, PT_ASECD, PT_ACSCD, PT_ACOTD,
  PT_SINH, PT_COSH, PT_TANH, PT_ASINH, PT_ACOSH, PT_ATANH,
  PT_LN, PT_LOG, PT_LOG2, PT_EXP, PT_SQRT, PT_SQR,
  PT_ABS, PT_FLOOR, PT_CEIL, PT_ROUND, PT_INT, PT_FIX, PT_FRAC,
  PT_SGN, PT_NOT, PT_RND, PT_FACT, PT_MILLIS,
  PT_RAD_CONV, PT_DEG_CONV,  // Conversions
  // Two-argument functions
  PT_MIN, PT_MAX, PT_HYPOT, PT_ATAN2, PT_ATAN2D, PT_POW_FUNC,
  // String functions that return numbers
  PT_LEN, PT_ASC, PT_VAL, PT_INSTR,
  // Array access
  PT_GET1, PT_GET2, PT_GET3,
  PT_END  // End of expression marker
};

// Single postfix token - optimized for size
struct PostfixToken {
  uint8_t type;
  int8_t varIndex;    // For PT_VAR, PT_GET1/2/3 (cached variable/array index)
  float number;       // For PT_NUMBER (use float instead of double to save space)
};

// Compiled expression (tokenized postfix)
#define MAX_POSTFIX_TOKENS 32
struct CompiledExpr {
  PostfixToken tokens[MAX_POSTFIX_TOKENS];
  uint8_t count;
  bool compiled;
};

// Cache of compiled expressions
#define MAX_COMPILED_EXPR 50
CompiledExpr compiledExprs[MAX_COMPILED_EXPR];
int compiledExprCount = 0;

// Forward declarations for tokenized postfix functions
void compilePostfix(const String& postfix, CompiledExpr& ce);
double evalCompiledPostfix(CompiledExpr& ce, bool isRad);
int getCompiledExprIndex(const String& expr);
double evalExprFast(const String& expr, bool isRad);

// Lookup function name to token type
uint8_t funcNameToToken(const String& name) {
  if (name == "SIN") return PT_SIN;
  if (name == "COS") return PT_COS;
  if (name == "TAN") return PT_TAN;
  if (name == "ASIN") return PT_ASIN;
  if (name == "ACOS") return PT_ACOS;
  if (name == "ATAN") return PT_ATAN;
  if (name == "ATN") return PT_ATN;
  if (name == "SIND") return PT_SIND;
  if (name == "COSD") return PT_COSD;
  if (name == "TAND") return PT_TAND;
  if (name == "ASIND") return PT_ASIND;
  if (name == "ACOSD") return PT_ACOSD;
  if (name == "ATAND") return PT_ATAND;
  if (name == "SEC") return PT_SEC;
  if (name == "CSC") return PT_CSC;
  if (name == "COT") return PT_COT;
  if (name == "ASEC") return PT_ASEC;
  if (name == "ACSC") return PT_ACSC;
  if (name == "ACOT") return PT_ACOT;
  if (name == "SECD") return PT_SECD;
  if (name == "CSCD") return PT_CSCD;
  if (name == "COTD") return PT_COTD;
  if (name == "ASECD") return PT_ASECD;
  if (name == "ACSCD") return PT_ACSCD;
  if (name == "ACOTD") return PT_ACOTD;
  if (name == "SINH") return PT_SINH;
  if (name == "COSH") return PT_COSH;
  if (name == "TANH") return PT_TANH;
  if (name == "ASINH") return PT_ASINH;
  if (name == "ACOSH") return PT_ACOSH;
  if (name == "ATANH") return PT_ATANH;
  if (name == "LN") return PT_LN;
  if (name == "LOG") return PT_LOG;
  if (name == "LOG2") return PT_LOG2;
  if (name == "EXP") return PT_EXP;
  if (name == "SQRT") return PT_SQRT;
  if (name == "SQR") return PT_SQR;
  if (name == "ABS") return PT_ABS;
  if (name == "FLOOR") return PT_FLOOR;
  if (name == "CEIL") return PT_CEIL;
  if (name == "ROUND") return PT_ROUND;
  if (name == "INT") return PT_INT;
  if (name == "FIX") return PT_FIX;
  if (name == "FRAC") return PT_FRAC;
  if (name == "SGN") return PT_SGN;
  if (name == "NOT") return PT_NOT;
  if (name == "RND") return PT_RND;
  if (name == "FACT") return PT_FACT;
  if (name == "MILLIS") return PT_MILLIS;
  if (name == "MIN") return PT_MIN;
  if (name == "MAX") return PT_MAX;
  if (name == "HYPOT") return PT_HYPOT;
  if (name == "ATAN2") return PT_ATAN2;
  if (name == "ATAN2D") return PT_ATAN2D;
  if (name == "POW") return PT_POW_FUNC;
  return 0;  // Unknown
}

// =================================================================
// Global Variables for BASIC Interpreter
// =================================================================
// Variable System - supports up to 100 variables with 8-char names
// String variables end with $ (e.g., A$, NAME$)
// =================================================================
#define MAX_VARIABLES 100
#define MAX_VAR_NAME_LEN 8
#define MAX_DIMENSIONS 3
#define MAX_STRING_LEN 255

struct Variable {
  String name;
  double value;
  String strValue;
  double* arrayData;
  String* strArrayData;
  int dims[MAX_DIMENSIONS];
  int numDims;
  int totalSize;
  bool isString;
};
Variable variables[MAX_VARIABLES];
int variableCount = 0;

// String variable storage (separate for efficiency)
#define MAX_STRING_VARS 50
String stringVars[MAX_STRING_VARS];
int stringVarCount = 0;

// =================================================================
// VARIABLE INDEX CACHE - for fast repeated access
// =================================================================
#define VAR_CACHE_SIZE 32
struct VarCacheEntry {
  String name;
  int8_t index;
};
VarCacheEntry varCache[VAR_CACHE_SIZE];
int varCacheCount = 0;

// Fast variable index lookup with caching
int findVariableFast(const String& name) {
  // Check cache first (most recent variables)
  for (int i = 0; i < varCacheCount; i++) {
    if (varCache[i].name == name) {
      return varCache[i].index;
    }
  }
  
  // Not in cache - search main array
  for (int i = 0; i < variableCount; i++) {
    if (variables[i].name == name) {
      // Add to cache
      if (varCacheCount < VAR_CACHE_SIZE) {
        varCache[varCacheCount].name = name;
        varCache[varCacheCount].index = i;
        varCacheCount++;
      } else {
        // Cache full - replace oldest (index 0)
        for (int j = 0; j < VAR_CACHE_SIZE - 1; j++) {
          varCache[j] = varCache[j + 1];
        }
        varCache[VAR_CACHE_SIZE - 1].name = name;
        varCache[VAR_CACHE_SIZE - 1].index = i;
      }
      return i;
    }
  }
  return -1;
}

void clearVarCache() {
  varCacheCount = 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;
bool jumped = false;  // Global flag for jump detection
int awaitingInputForVarIndex = -1;
String awaitingInputVarName = "";
String inputBuffer = "";
int printPrecision = 6;

// Expression cache - stores postfix form to avoid re-parsing
#define MAX_EXPR_CACHE 100
struct ExprCache {
  String infix;
  String postfix;
};
ExprCache exprCache[MAX_EXPR_CACHE];
int exprCacheCount = 0;

// Lookup cached postfix or compute and cache it
String getCachedPostfix(String infix, String (*toPostfixFunc)(String)) {
  // Search cache
  for (int i = 0; i < exprCacheCount; i++) {
    if (exprCache[i].infix == infix) {
      return exprCache[i].postfix;
    }
  }
  // Not found - compute and cache
  String postfix = toPostfixFunc(infix);
  if (exprCacheCount < MAX_EXPR_CACHE) {
    exprCache[exprCacheCount].infix = infix;
    exprCache[exprCacheCount].postfix = postfix;
    exprCacheCount++;
  }
  return postfix;
}

void clearExprCache() {
  exprCacheCount = 0;
}

// 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();
extern bool isRadiansMode;  // Forward declaration

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

// Find or create variable, returns index - uses cache
int getOrCreateVariable(String name) {
  name.toUpperCase();
  
  // Check cache first
  int idx = findVariableFast(name);
  if (idx >= 0) return idx;
  
  // Not found - create new
  if (variableCount >= MAX_VARIABLES) return -1;
  idx = variableCount;
  variables[idx].name = name;
  variables[idx].value = 0;
  variables[idx].strValue = "";
  variables[idx].arrayData = NULL;
  variables[idx].strArrayData = NULL;
  variables[idx].totalSize = 0;
  variables[idx].numDims = 0;
  variables[idx].dims[0] = 0;
  variables[idx].dims[1] = 0;
  variables[idx].dims[2] = 0;
  variables[idx].isString = name.endsWith("$");
  variableCount++;
  
  // Add to cache
  if (varCacheCount < VAR_CACHE_SIZE) {
    varCache[varCacheCount].name = name;
    varCache[varCacheCount].index = idx;
    varCacheCount++;
  }
  
  return idx;
}

// Get variable index (returns -1 if not found) - uses cache
int findVariable(String name) {
  name.toUpperCase();
  return findVariableFast(name);
}

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

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

// Get string variable value
String getStringValue(String name) {
  name.toUpperCase();
  int idx = findVariableFast(name);
  if (idx >= 0) return variables[idx].strValue;
  idx = getOrCreateVariable(name);
  if (idx >= 0) return variables[idx].strValue;
  return "";
}

// Set string variable value
void setStringValue(String name, String value) {
  name.toUpperCase();
  int idx = findVariableFast(name);
  if (idx >= 0) {
    variables[idx].strValue = value;
    variables[idx].isString = true;
    return;
  }
  idx = getOrCreateVariable(name);
  if (idx >= 0) {
    variables[idx].strValue = value;
    variables[idx].isString = true;
  }
}

// =================================================================
// CPC BASIC STRING FUNCTIONS
// =================================================================
// LEFT$(s$, n) - leftmost n characters
String strLEFT(String s, int n) {
  if (n <= 0) return "";
  if (n >= (int)s.length()) return s;
  return s.substring(0, n);
}

// RIGHT$(s$, n) - rightmost n characters
String strRIGHT(String s, int n) {
  if (n <= 0) return "";
  if (n >= (int)s.length()) return s;
  return s.substring(s.length() - n);
}

// MID$(s$, start, len) - substring (1-based index like CPC)
String strMID(String s, int start, int len = -1) {
  if (start < 1) start = 1;
  start--;  // Convert to 0-based
  if (start >= (int)s.length()) return "";
  if (len < 0) return s.substring(start);
  return s.substring(start, start + len);
}

// LEN(s$) - string length
int strLEN(String s) {
  return s.length();
}

// ASC(s$) - ASCII code of first character
int strASC(String s) {
  if (s.length() == 0) return 0;
  return (int)s.charAt(0);
}

// CHR$(n) - character from ASCII code
String strCHR(int n) {
  if (n < 0 || n > 255) return "";
  char c = (char)n;
  return String(c);
}

// VAL(s$) - convert string to number
double strVAL(String s) {
  s.trim();
  return s.toDouble();
}

// STR$(n) - convert number to string
String strSTR(double n) {
  if (n == floor(n) && fabs(n) < 1e9) {
    return String((long)n);
  }
  return String(n, 6);
}

// INSTR(s$, search$) or INSTR(start, s$, search$) - find substring (1-based)
int strINSTR(String s, String search, int start = 1) {
  if (start < 1) start = 1;
  start--;  // Convert to 0-based
  int pos = s.indexOf(search, start);
  return (pos < 0) ? 0 : pos + 1;  // Return 1-based or 0 if not found
}

// UPPER$(s$) - convert to uppercase
String strUPPER(String s) {
  s.toUpperCase();
  return s;
}

// LOWER$(s$) - convert to lowercase
String strLOWER(String s) {
  s.toLowerCase();
  return s;
}

// STRING$(n, char$) or STRING$(n, code) - repeat character
String strSTRING(int n, String c) {
  if (n <= 0) return "";
  String result = "";
  char ch = (c.length() > 0) ? c.charAt(0) : ' ';
  for (int i = 0; i < n; i++) result += ch;
  return result;
}

// SPACE$(n) - n spaces
String strSPACE(int n) {
  if (n <= 0) return "";
  String result = "";
  for (int i = 0; i < n; i++) result += ' ';
  return result;
}

// HEX$(n) - convert to hexadecimal string
String strHEX(int n) {
  return String(n, HEX);
}

// BIN$(n) - convert to binary string
String strBIN(int n) {
  return String(n, BIN);
}

// Parse string expression (handles concatenation with +)
String evalStringExpr(String expr);

// Check if expression is a string expression - FAST version
bool isStringExpr(const String& expr) {
  if (expr.length() == 0) return false;
  char c = expr.charAt(0);
  // Quick check: string literal
  if (c == '"') return true;
  // Quick check: ends with $ (string variable or function)
  if (expr.endsWith("$")) return true;
  // Only check function names if starts with valid letter
  if (c == 'C' || c == 'c' || c == 'L' || c == 'l' || c == 'R' || c == 'r' ||
      c == 'M' || c == 'm' || c == 'S' || c == 's' || c == 'U' || c == 'u' ||
      c == 'H' || c == 'h' || c == 'B' || c == 'b') {
    String uExpr = expr;
    uExpr.toUpperCase();
    if (uExpr.startsWith("CHR$") || uExpr.startsWith("LEFT$") || 
        uExpr.startsWith("RIGHT$") || uExpr.startsWith("MID$") ||
        uExpr.startsWith("STR$") || uExpr.startsWith("UPPER$") ||
        uExpr.startsWith("LOWER$") || uExpr.startsWith("STRING$") ||
        uExpr.startsWith("SPACE$") || uExpr.startsWith("HEX$") ||
        uExpr.startsWith("BIN$")) return true;
  }
  return false;
}

// Extract function arguments from parentheses
String extractArgs(String expr, int start) {
  int depth = 0;
  int argStart = -1;
  for (int i = start; i < (int)expr.length(); i++) {
    if (expr.charAt(i) == '(') {
      if (depth == 0) argStart = i + 1;
      depth++;
    } else if (expr.charAt(i) == ')') {
      depth--;
      if (depth == 0) {
        return expr.substring(argStart, i);
      }
    }
  }
  return "";
}

// Evaluate a single string term (no + operator)
String evalStringTerm(String expr) {
  expr.trim();
  
  // String literal
  if (expr.startsWith("\"") && expr.endsWith("\"")) {
    return expr.substring(1, expr.length() - 1);
  }
  
  String uExpr = expr;
  uExpr.toUpperCase();
  
  // CHR$(n)
  if (uExpr.startsWith("CHR$(")) {
    String args = extractArgs(expr, 4);
    return strCHR((int)evalExprFast(args, isRadiansMode));
  }
  
  // LEFT$(s$, n)
  if (uExpr.startsWith("LEFT$(")) {
    String args = extractArgs(expr, 5);
    int comma = args.indexOf(',');
    if (comma > 0) {
      String s = evalStringExpr(args.substring(0, comma));
      int n = (int)evalExprFast(args.substring(comma + 1), isRadiansMode);
      return strLEFT(s, n);
    }
  }
  
  // RIGHT$(s$, n)
  if (uExpr.startsWith("RIGHT$(")) {
    String args = extractArgs(expr, 6);
    int comma = args.indexOf(',');
    if (comma > 0) {
      String s = evalStringExpr(args.substring(0, comma));
      int n = (int)evalExprFast(args.substring(comma + 1), isRadiansMode);
      return strRIGHT(s, n);
    }
  }
  
  // MID$(s$, start) or MID$(s$, start, len)
  if (uExpr.startsWith("MID$(")) {
    String args = extractArgs(expr, 4);
    int comma1 = args.indexOf(',');
    if (comma1 > 0) {
      String s = evalStringExpr(args.substring(0, comma1));
      String rest = args.substring(comma1 + 1);
      int comma2 = rest.indexOf(',');
      if (comma2 > 0) {
        int start = (int)evalExprFast(rest.substring(0, comma2), isRadiansMode);
        int len = (int)evalExprFast(rest.substring(comma2 + 1), isRadiansMode);
        return strMID(s, start, len);
      } else {
        int start = (int)evalExprFast(rest, isRadiansMode);
        return strMID(s, start);
      }
    }
  }
  
  // STR$(n)
  if (uExpr.startsWith("STR$(")) {
    String args = extractArgs(expr, 4);
    return strSTR(evalExprFast(args, isRadiansMode));
  }
  
  // UPPER$(s$)
  if (uExpr.startsWith("UPPER$(")) {
    String args = extractArgs(expr, 6);
    return strUPPER(evalStringExpr(args));
  }
  
  // LOWER$(s$)
  if (uExpr.startsWith("LOWER$(")) {
    String args = extractArgs(expr, 6);
    return strLOWER(evalStringExpr(args));
  }
  
  // STRING$(n, char$)
  if (uExpr.startsWith("STRING$(")) {
    String args = extractArgs(expr, 7);
    int comma = args.indexOf(',');
    if (comma > 0) {
      int n = (int)evalExprFast(args.substring(0, comma), isRadiansMode);
      String c = evalStringExpr(args.substring(comma + 1));
      return strSTRING(n, c);
    }
  }
  
  // SPACE$(n)
  if (uExpr.startsWith("SPACE$(")) {
    String args = extractArgs(expr, 6);
    return strSPACE((int)evalExprFast(args, isRadiansMode));
  }
  
  // HEX$(n)
  if (uExpr.startsWith("HEX$(")) {
    String args = extractArgs(expr, 4);
    return strHEX((int)evalExprFast(args, isRadiansMode));
  }
  
  // BIN$(n)
  if (uExpr.startsWith("BIN$(")) {
    String args = extractArgs(expr, 4);
    return strBIN((int)evalExprFast(args, isRadiansMode));
  }
  
  // String variable (ends with $)
  if (expr.endsWith("$")) {
    return getStringValue(expr);
  }
  
  return expr;
}

// Evaluate string expression with + concatenation
String evalStringExpr(String expr) {
  expr.trim();
  if (expr.length() == 0) return "";
  
  String result = "";
  int start = 0;
  bool inQuote = false;
  int parenDepth = 0;
  
  for (int i = 0; i <= (int)expr.length(); i++) {
    char c = (i < (int)expr.length()) ? expr.charAt(i) : '+';
    
    if (c == '"') inQuote = !inQuote;
    else if (!inQuote && c == '(') parenDepth++;
    else if (!inQuote && c == ')') parenDepth--;
    else if (!inQuote && parenDepth == 0 && c == '+') {
      String term = expr.substring(start, i);
      term.trim();
      if (term.length() > 0) {
        result += evalStringTerm(term);
      }
      start = i + 1;
    }
  }
  
  return result;
}

// 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;
  clearVarCache();  // Clear variable cache too
}

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);
    double evalCached(String expr, bool isRad);  // Cached evaluation
  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", "MILLIS",
                     "LEN", "ASC", "VAL", "INSTR"};
  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 == "MILLIS") res = (double)millis();  // System time in milliseconds
      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);
}

// Cached expression evaluation - caches postfix form
double Calculator::evalCached(String expr, bool isRad) {
  // Search cache for this expression
  for (int i = 0; i < exprCacheCount; i++) {
    if (exprCache[i].infix == expr) {
      return evaluatePostfix(exprCache[i].postfix, isRad);
    }
  }
  // Not cached - compute postfix and cache it
  String postfix = toPostfix(expr);
  if (exprCacheCount < MAX_EXPR_CACHE) {
    exprCache[exprCacheCount].infix = expr;
    exprCache[exprCacheCount].postfix = postfix;
    exprCacheCount++;
  }
  return evaluatePostfix(postfix, isRad);
}

Calculator calculator;
bool isRadiansMode = true;

// =================================================================
// FAST TOKENIZED POSTFIX EVALUATOR
// =================================================================
// Compile string postfix to tokenized form
void compilePostfix(const String& postfix, CompiledExpr& ce) {
  ce.count = 0;
  ce.compiled = true;
  
  int c = 0;
  while (c < (int)postfix.length() && ce.count < MAX_POSTFIX_TOKENS - 1) {
    int n = postfix.indexOf(' ', c);
    if (n == -1) n = postfix.length();
    String t = postfix.substring(c, n);
    c = n + 1;
    if (t.length() == 0) continue;
    
    PostfixToken& tok = ce.tokens[ce.count];
    
    // Check for number
    if (isDigit(t.charAt(0)) || t.charAt(0) == '.' ||
        (t.charAt(0) == '-' && t.length() > 1 && (isDigit(t.charAt(1)) || t.charAt(1) == '.'))) {
      tok.type = PT_NUMBER;
      tok.number = t.toDouble();
    }
    // Constants
    else if (t.equalsIgnoreCase("PI")) { tok.type = PT_PI; }
    else if (t.equalsIgnoreCase("E") && t.length() == 1) { tok.type = PT_E; }
    // Variable
    else if (t.startsWith("VAR_")) {
      tok.type = PT_VAR;
      String varName = t.substring(4);
      tok.varIndex = getOrCreateVariable(varName);
    }
    // Operators
    else if (t == "+") { tok.type = PT_ADD; }
    else if (t == "-") { tok.type = PT_SUB; }
    else if (t == "*") { tok.type = PT_MUL; }
    else if (t == "/") { tok.type = PT_DIV; }
    else if (t == "%") { tok.type = PT_MOD; }
    else if (t == "^") { tok.type = PT_POW; }
    else if (t == "<") { tok.type = PT_LT; }
    else if (t == ">") { tok.type = PT_GT; }
    else if (t == "<=") { tok.type = PT_LE; }
    else if (t == ">=") { tok.type = PT_GE; }
    else if (t == "=" || t == "==") { tok.type = PT_EQ; }
    else if (t == "<>" || t == "!=") { tok.type = PT_NE; }
    else if (t == "AND") { tok.type = PT_AND; }
    else if (t == "OR") { tok.type = PT_OR; }
    else if (t == "XOR") { tok.type = PT_XOR; }
    // Array access
    else if (t.startsWith("GET1_")) {
      tok.type = PT_GET1;
      String arrName = t.substring(5);
      tok.varIndex = findVariable(arrName);
    }
    else if (t.startsWith("GET2_")) {
      tok.type = PT_GET2;
      String arrName = t.substring(5);
      tok.varIndex = findVariable(arrName);
    }
    else if (t.startsWith("GET3_")) {
      tok.type = PT_GET3;
      String arrName = t.substring(5);
      tok.varIndex = findVariable(arrName);
    }
    // Functions
    else {
      String ut = t;
      ut.toUpperCase();
      uint8_t ft = funcNameToToken(ut);
      if (ft != 0) {
        tok.type = ft;
      } else {
        // Unknown - treat as 0
        tok.type = PT_NUMBER;
        tok.number = 0;
      }
    }
    ce.count++;
  }
  ce.tokens[ce.count].type = PT_END;
}

// Fast evaluation stack
#define EVAL_STACK_SIZE 32
double evalStack[EVAL_STACK_SIZE];
int evalStackPtr = 0;

inline void epush(double v) { if (evalStackPtr < EVAL_STACK_SIZE) evalStack[evalStackPtr++] = v; }
inline double epop() { return (evalStackPtr > 0) ? evalStack[--evalStackPtr] : 0; }

// Evaluate compiled postfix - FAST!
double evalCompiledPostfix(CompiledExpr& ce, bool isRad) {
  evalStackPtr = 0;
  double v1, v2, res;
  
  for (int i = 0; i < ce.count; i++) {
    PostfixToken& t = ce.tokens[i];
    
    switch (t.type) {
      case PT_NUMBER: epush(t.number); break;
      case PT_PI: epush(PI_CONST); break;
      case PT_E: epush(E_CONST); break;
      case PT_VAR: epush(variables[t.varIndex].value); break;
      
      // Binary operators
      case PT_ADD: v2 = epop(); v1 = epop(); epush(v1 + v2); break;
      case PT_SUB: v2 = epop(); v1 = epop(); epush(v1 - v2); break;
      case PT_MUL: v2 = epop(); v1 = epop(); epush(v1 * v2); break;
      case PT_DIV: v2 = epop(); v1 = epop(); epush(v2 != 0 ? v1 / v2 : NAN); break;
      case PT_MOD: v2 = epop(); v1 = epop(); epush(v2 != 0 ? fmod(v1, v2) : NAN); break;
      case PT_POW: v2 = epop(); v1 = epop(); epush(pow(v1, v2)); break;
      
      // Comparisons
      case PT_LT: v2 = epop(); v1 = epop(); epush(v1 < v2 ? 1 : 0); break;
      case PT_GT: v2 = epop(); v1 = epop(); epush(v1 > v2 ? 1 : 0); break;
      case PT_LE: v2 = epop(); v1 = epop(); epush(v1 <= v2 ? 1 : 0); break;
      case PT_GE: v2 = epop(); v1 = epop(); epush(v1 >= v2 ? 1 : 0); break;
      case PT_EQ: v2 = epop(); v1 = epop(); epush(fabs(v1 - v2) < 0.0001 ? 1 : 0); break;
      case PT_NE: v2 = epop(); v1 = epop(); epush(fabs(v1 - v2) >= 0.0001 ? 1 : 0); break;
      
      // Logical
      case PT_AND: v2 = epop(); v1 = epop(); epush((v1 != 0 && v2 != 0) ? 1 : 0); break;
      case PT_OR: v2 = epop(); v1 = epop(); epush((v1 != 0 || v2 != 0) ? 1 : 0); break;
      case PT_XOR: v2 = epop(); v1 = epop(); epush(((v1 != 0) != (v2 != 0)) ? 1 : 0); break;
      
      // Trig (mode dependent)
      case PT_SIN: v1 = epop(); epush(isRad ? sin(v1) : sin(v1 * PI_CONST / 180.0)); break;
      case PT_COS: v1 = epop(); epush(isRad ? cos(v1) : cos(v1 * PI_CONST / 180.0)); break;
      case PT_TAN: v1 = epop(); epush(isRad ? tan(v1) : tan(v1 * PI_CONST / 180.0)); break;
      case PT_ASIN: v1 = epop(); epush(isRad ? asin(v1) : asin(v1) * 180.0 / PI_CONST); break;
      case PT_ACOS: v1 = epop(); epush(isRad ? acos(v1) : acos(v1) * 180.0 / PI_CONST); break;
      case PT_ATAN: case PT_ATN: v1 = epop(); epush(isRad ? atan(v1) : atan(v1) * 180.0 / PI_CONST); break;
      
      // Trig (always degrees)
      case PT_SIND: v1 = epop(); epush(sin(v1 * PI_CONST / 180.0)); break;
      case PT_COSD: v1 = epop(); epush(cos(v1 * PI_CONST / 180.0)); break;
      case PT_TAND: v1 = epop(); epush(tan(v1 * PI_CONST / 180.0)); break;
      case PT_ASIND: v1 = epop(); epush(asin(v1) * 180.0 / PI_CONST); break;
      case PT_ACOSD: v1 = epop(); epush(acos(v1) * 180.0 / PI_CONST); break;
      case PT_ATAND: v1 = epop(); epush(atan(v1) * 180.0 / PI_CONST); break;
      
      // Reciprocal trig
      case PT_SEC: v1 = epop(); res = isRad ? cos(v1) : cos(v1 * PI_CONST / 180.0); epush(res == 0 ? NAN : 1.0 / res); break;
      case PT_CSC: v1 = epop(); res = isRad ? sin(v1) : sin(v1 * PI_CONST / 180.0); epush(res == 0 ? NAN : 1.0 / res); break;
      case PT_COT: v1 = epop(); res = isRad ? tan(v1) : tan(v1 * PI_CONST / 180.0); epush(res == 0 ? NAN : 1.0 / res); break;
      case PT_SECD: v1 = epop(); res = cos(v1 * PI_CONST / 180.0); epush(res == 0 ? NAN : 1.0 / res); break;
      case PT_CSCD: v1 = epop(); res = sin(v1 * PI_CONST / 180.0); epush(res == 0 ? NAN : 1.0 / res); break;
      case PT_COTD: v1 = epop(); res = tan(v1 * PI_CONST / 180.0); epush(res == 0 ? NAN : 1.0 / res); break;
      
      // Inverse reciprocal
      case PT_ASEC: v1 = epop(); epush(v1 == 0 ? NAN : (isRad ? acos(1.0/v1) : acos(1.0/v1) * 180.0 / PI_CONST)); break;
      case PT_ACSC: v1 = epop(); epush(v1 == 0 ? NAN : (isRad ? asin(1.0/v1) : asin(1.0/v1) * 180.0 / PI_CONST)); break;
      case PT_ACOT: v1 = epop(); epush(v1 == 0 ? PI_CONST/2 : (isRad ? atan(1.0/v1) : atan(1.0/v1) * 180.0 / PI_CONST)); break;
      case PT_ASECD: v1 = epop(); epush(v1 == 0 ? NAN : acos(1.0/v1) * 180.0 / PI_CONST); break;
      case PT_ACSCD: v1 = epop(); epush(v1 == 0 ? NAN : asin(1.0/v1) * 180.0 / PI_CONST); break;
      case PT_ACOTD: v1 = epop(); epush(v1 == 0 ? 90.0 : atan(1.0/v1) * 180.0 / PI_CONST); break;
      
      // Hyperbolic
      case PT_SINH: v1 = epop(); epush(sinh(v1)); break;
      case PT_COSH: v1 = epop(); epush(cosh(v1)); break;
      case PT_TANH: v1 = epop(); epush(tanh(v1)); break;
      case PT_ASINH: v1 = epop(); epush(asinh(v1)); break;
      case PT_ACOSH: v1 = epop(); epush(acosh(v1)); break;
      case PT_ATANH: v1 = epop(); epush(atanh(v1)); break;
      
      // Math
      case PT_LN: v1 = epop(); epush(log(v1)); break;
      case PT_LOG: v1 = epop(); epush(log10(v1)); break;
      case PT_LOG2: v1 = epop(); epush(log(v1) / log(2.0)); break;
      case PT_EXP: v1 = epop(); epush(exp(v1)); break;
      case PT_SQRT: case PT_SQR: v1 = epop(); epush(sqrt(v1)); break;
      case PT_ABS: v1 = epop(); epush(fabs(v1)); break;
      case PT_FLOOR: v1 = epop(); epush(floor(v1)); break;
      case PT_CEIL: v1 = epop(); epush(ceil(v1)); break;
      case PT_ROUND: v1 = epop(); epush(round(v1)); break;
      case PT_INT: v1 = epop(); epush(v1 >= 0 ? floor(v1) : ceil(v1)); break;
      case PT_FIX: v1 = epop(); epush(trunc(v1)); break;
      case PT_FRAC: v1 = epop(); epush(v1 - trunc(v1)); break;
      case PT_SGN: v1 = epop(); epush(v1 > 0 ? 1 : (v1 < 0 ? -1 : 0)); break;
      case PT_NOT: v1 = epop(); epush(v1 == 0 ? 1 : 0); break;
      case PT_RND: v1 = epop(); epush((double)random(10000) / 10000.0 * v1); break;
      case PT_MILLIS: epop(); epush((double)millis()); break;
      case PT_FACT: {
        int n = (int)epop();
        if (n < 0) epush(NAN);
        else if (n > 170) epush(INFINITY);
        else { res = 1; for (int j = 2; j <= n; j++) res *= j; epush(res); }
        break;
      }
      
      // Two-argument functions
      case PT_MIN: v2 = epop(); v1 = epop(); epush(v1 < v2 ? v1 : v2); break;
      case PT_MAX: v2 = epop(); v1 = epop(); epush(v1 > v2 ? v1 : v2); break;
      case PT_HYPOT: v2 = epop(); v1 = epop(); epush(sqrt(v1*v1 + v2*v2)); break;
      case PT_ATAN2: v2 = epop(); v1 = epop(); epush(isRad ? atan2(v1, v2) : atan2(v1, v2) * 180.0 / PI_CONST); break;
      case PT_ATAN2D: v2 = epop(); v1 = epop(); epush(atan2(v1, v2) * 180.0 / PI_CONST); break;
      case PT_POW_FUNC: v2 = epop(); v1 = epop(); epush(pow(v1, v2)); break;
      
      // Array access
      case PT_GET1: {
        int idx = (int)epop();
        if (t.varIndex >= 0 && variables[t.varIndex].arrayData && idx >= 0 && idx < variables[t.varIndex].totalSize)
          epush(variables[t.varIndex].arrayData[idx]);
        else epush(0);
        break;
      }
      case PT_GET2: {
        int i2 = (int)epop(); int i1 = (int)epop();
        if (t.varIndex >= 0 && variables[t.varIndex].arrayData && variables[t.varIndex].numDims >= 2) {
          int li = i1 * variables[t.varIndex].dims[1] + i2;
          if (li >= 0 && li < variables[t.varIndex].totalSize) epush(variables[t.varIndex].arrayData[li]);
          else epush(0);
        } else epush(0);
        break;
      }
      case PT_GET3: {
        int i3 = (int)epop(); int i2 = (int)epop(); int i1 = (int)epop();
        if (t.varIndex >= 0 && variables[t.varIndex].arrayData && variables[t.varIndex].numDims >= 3) {
          int li = i1 * variables[t.varIndex].dims[1] * variables[t.varIndex].dims[2] + i2 * variables[t.varIndex].dims[2] + i3;
          if (li >= 0 && li < variables[t.varIndex].totalSize) epush(variables[t.varIndex].arrayData[li]);
          else epush(0);
        } else epush(0);
        break;
      }
      
      case PT_END:
      default:
        break;
    }
  }
  
  return epop();
}

// Get or compile expression, return index
int getCompiledExprIndex(const String& expr) {
  // Search existing
  for (int i = 0; i < exprCacheCount; i++) {
    if (exprCache[i].infix == expr) {
      // Found in string cache - check if compiled
      if (i < compiledExprCount && compiledExprs[i].compiled) {
        return i;
      }
      // Need to compile
      if (i < MAX_COMPILED_EXPR) {
        compilePostfix(exprCache[i].postfix, compiledExprs[i]);
        if (i >= compiledExprCount) compiledExprCount = i + 1;
        return i;
      }
      return -1;  // Can't compile, use string version
    }
  }
  
  // Not found - add to cache and compile
  if (exprCacheCount < MAX_EXPR_CACHE && exprCacheCount < MAX_COMPILED_EXPR) {
    String postfix = calculator.toPostfix(expr);
    int idx = exprCacheCount;
    exprCache[idx].infix = expr;
    exprCache[idx].postfix = postfix;
    exprCacheCount++;
    compilePostfix(postfix, compiledExprs[idx]);
    compiledExprCount = exprCacheCount;
    return idx;
  }
  return -1;
}

// Fast expression evaluation (uses compiled if available)
double evalExprFast(const String& expr, bool isRad) {
  // Quick check: only process string functions if expression might contain them
  // Check first char - string functions start with L, A, V, I
  char firstChar = expr.charAt(0);
  if (firstChar == 'L' || firstChar == 'l' || 
      firstChar == 'A' || firstChar == 'a' ||
      firstChar == 'V' || firstChar == 'v' ||
      firstChar == 'I' || firstChar == 'i') {
    String ue = expr;
    ue.toUpperCase();
    
    if (ue.startsWith("LEN(")) {
      String args = extractArgs(expr, 3);
      String s = evalStringExpr(args);
      return (double)s.length();
    }
    if (ue.startsWith("ASC(")) {
      String args = extractArgs(expr, 3);
      String s = evalStringExpr(args);
      return (s.length() > 0) ? (double)s.charAt(0) : 0;
    }
    if (ue.startsWith("VAL(")) {
      String args = extractArgs(expr, 3);
      String s = evalStringExpr(args);
      s.trim();
      return s.toDouble();
    }
    if (ue.startsWith("INSTR(")) {
      String args = extractArgs(expr, 5);
      int comma1 = -1;
      int depth = 0;
      bool inQ = false;
      for (int i = 0; i < (int)args.length(); i++) {
        char c = args.charAt(i);
        if (c == '"') inQ = !inQ;
        else if (!inQ && c == '(') depth++;
        else if (!inQ && c == ')') depth--;
        else if (!inQ && depth == 0 && c == ',') {
          comma1 = i;
          break;
        }
      }
      if (comma1 > 0) {
        String s = evalStringExpr(args.substring(0, comma1));
        String search = evalStringExpr(args.substring(comma1 + 1));
        int pos = s.indexOf(search);
        return (pos < 0) ? 0 : pos + 1;
      }
      return 0;
    }
  }
  
  int idx = getCompiledExprIndex(expr);
  if (idx >= 0) {
    return evalCompiledPostfix(compiledExprs[idx], isRad);
  }
  return calculator.evalCached(expr, isRad);
}

// =================================================================
// 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;
  exprCacheCount = 0;  // Clear expression cache
  compiledExprCount = 0;  // Clear compiled expressions
  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;
  }
}

// Execute a single statement (no colon parsing)
void executeSingleStatement(String line);

void executeNextBasicLine() {
  if (pc >= programSize) {
    programState = IDLE;
    return;
  }
  
  jumped = false;
  
  String line = programLines[pc];
  
  if (line.startsWith("'")) {
    pc++;
    return;
  }
  
  // Strip inline comments - but not in quotes
  int apoPos = -1;
  bool inQuote = false;
  for (int i = 0; i < (int)line.length(); i++) {
    if (line.charAt(i) == '"') inQuote = !inQuote;
    if (!inQuote && line.charAt(i) == '\'') {
      apoPos = i;
      break;
    }
  }
  if (apoPos != -1) {
    line = line.substring(0, apoPos);
    line.trim();
  }
  
  if (line.length() == 0) {
    pc++;
    return;
  }
  
  // Quick checks for special handling
  bool hasColon = (line.indexOf(':') >= 0);
  char fc = line.charAt(0);
  
  // IF statements with colons need special handling
  if (hasColon && (fc == 'I' || fc == 'i') && line.length() > 2 &&
      (line.charAt(1) == 'F' || line.charAt(1) == 'f') &&
      (line.charAt(2) == ' ' || line.charAt(2) == '(')) {
    executeSingleStatement(line);
    if (programState == RUNNING && !jumped) pc++;
    return;
  }
  
  // Lines with colons need parsing
  if (hasColon) {
    int start = 0;
    while (start < (int)line.length() && programState == RUNNING && !jumped) {
      int colonPos = -1;
      inQuote = false;
      for (int i = start; i < (int)line.length(); i++) {
        if (line.charAt(i) == '"') inQuote = !inQuote;
        if (!inQuote && line.charAt(i) == ':') { colonPos = i; break; }
      }
      String stmt = (colonPos >= 0) ? line.substring(start, colonPos) : line.substring(start);
      start = (colonPos >= 0) ? colonPos + 1 : line.length();
      stmt.trim();
      if (stmt.length() > 0) executeSingleStatement(stmt);
    }
    if (programState == RUNNING && !jumped) pc++;
    return;
  }
  
  // Simple line - execute directly (fast path like v1.18)
  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();
  
  uint8_t cmd = lookupCommand(command);
  
  switch (cmd) {
    case CMD_REM: break;
    case CMD_RAD: isRadiansMode = true; break;
    case CMD_DEG: isRadiansMode = false; break;
    
    case CMD_RANDOMIZE:
      if (args.length() > 0) randomSeed((unsigned long)evalExprFast(args, isRadiansMode));
      else randomSeed(analogRead(0) ^ micros());
      break;
      
    case CMD_END: programState = IDLE; break;
    
    case CMD_PRINT: {
      int i = 0;
      while (i < (int)args.length()) {
        String token = "";
        bool trailingSemicolon = false;
        int parenDepth = 0;
        inQuote = false;
        while (i < (int)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.length() > 0) {
          if (token.charAt(0) == '"' || token.indexOf('$') >= 0) {
            Serial.print(evalStringExpr(token));
          } else {
            Serial.print(evalExprFast(token, isRadiansMode), printPrecision);
          }
        }
        if (!trailingSemicolon && i >= (int)args.length()) Serial.println();
      }
      break;
    }
    
    case CMD_INPUT: {
      String prompt = "? ";
      String varPart = args;
      if (args.startsWith("\"")) {
        int endQuote = args.indexOf("\"", 1);
        if (endQuote != -1) {
          prompt = args.substring(1, endQuote);
          int sepPos = endQuote + 1;
          while (sepPos < (int)args.length() && (args.charAt(sepPos) == ' ' || args.charAt(sepPos) == ';' || args.charAt(sepPos) == ',')) sepPos++;
          varPart = args.substring(sepPos);
          varPart.trim();
        }
      }
      String varName = "";
      for (int i = 0; i < (int)varPart.length(); i++) {
        char c = varPart.charAt(i);
        if (isAlpha(c) || (i > 0 && isDigit(c)) || c == '$') varName += c;
        else break;
      }
      varName.toUpperCase();
      if (varName.length() > 0) {
        Serial.print(prompt);
        awaitingInputVarName = varName;
        programState = AWAIT_INPUT;
      }
      break;
    }
    
    case CMD_GOTO: {
      int newPc = findLineIndex(args.toInt());
      if (newPc != -1) { pc = newPc; jumped = true; }
      break;
    }
    
    case CMD_GOSUB: {
      int targetLine = args.toInt();
      int targetPc = findLineIndex(targetLine);
      if (targetPc != -1 && gosubStackPtr < MAX_GOSUB_DEPTH - 1) {
        gosubStackPtr++;
        gosubStack[gosubStackPtr] = pc;
        pc = targetPc;
        jumped = true;
      }
      break;
    }
    
    case CMD_RETURN:
      if (gosubStackPtr >= 0) {
        pc = gosubStack[gosubStackPtr];
        gosubStackPtr--;
      }
      break;
    
    case CMD_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) {
          forLoopStackPtr++;
          forLoopStack[forLoopStackPtr].varName = varName;
          forLoopStack[forLoopStackPtr].endValue = evalExprFast(endExpr, isRadiansMode);
          forLoopStack[forLoopStackPtr].stepValue = evalExprFast(stepExpr, isRadiansMode);
          forLoopStack[forLoopStackPtr].returnAddress = pc;
          setVariableValue(varName, evalExprFast(startExpr, isRadiansMode));
        }
      }
      break;
    }
    
    case CMD_NEXT: {
      if (forLoopStackPtr >= 0) {
        String varName = args; varName.trim(); varName.toUpperCase();
        if (varName.length() == 0 || varName == forLoopStack[forLoopStackPtr].varName) {
          double currentVal = getVariableValue(forLoopStack[forLoopStackPtr].varName);
          double step = forLoopStack[forLoopStackPtr].stepValue;
          double endVal = forLoopStack[forLoopStackPtr].endValue;
          currentVal += step;
          setVariableValue(forLoopStack[forLoopStackPtr].varName, currentVal);
          bool loopDone = (step > 0) ? (currentVal > endVal) : (currentVal < endVal);
          if (!loopDone) {
            pc = forLoopStack[forLoopStackPtr].returnAddress;
            jumped = true;
          } else {
            forLoopStackPtr--;
          }
        }
      }
      break;
    }
    
    case CMD_WHILE: {
      double condResult = evalExprFast(args, isRadiansMode);
      if (condResult != 0) {
        if (whileLoopStackPtr < MAX_LOOP_DEPTH - 1) {
          whileLoopStackPtr++;
          whileLoopStack[whileLoopStackPtr].conditionAddress = pc;
        }
      } else {
        int depth = 1;
        while (pc < programSize - 1 && depth > 0) {
          pc++;
          String nextLine = programLines[pc];
          nextLine.toUpperCase();
          if (nextLine.startsWith("WHILE")) depth++;
          else if (nextLine.startsWith("WEND")) depth--;
        }
      }
      break;
    }
    
    case CMD_WEND:
      if (whileLoopStackPtr >= 0) {
        pc = whileLoopStack[whileLoopStackPtr].conditionAddress;
        whileLoopStackPtr--;
        jumped = true;
      }
      break;
    
    case CMD_IF: {
      // Fast path for simple IF condition THEN linenumber
      String uArgs = args;
      uArgs.toUpperCase();
      int thenPos = uArgs.indexOf("THEN");
      if (thenPos == -1) break;
      
      String condStr = args.substring(0, thenPos);
      condStr.trim();
      String afterThen = args.substring(thenPos + 4);
      afterThen.trim();
      
      // Quick check: if afterThen is just digits (line number), use fast path
      bool simpleGoto = true;
      for (int i = 0; i < (int)afterThen.length(); i++) {
        if (!isDigit(afterThen.charAt(i))) { simpleGoto = false; break; }
      }
      
      if (simpleGoto && afterThen.length() > 0) {
        // Simple IF cond THEN linenum
        double condResult = evalExprFast(condStr, isRadiansMode);
        if (condResult != 0) {
          int newPc = findLineIndex(afterThen.toInt());
          if (newPc != -1) { pc = newPc; jumped = true; }
        }
        break;
      }
      
      // Complex IF - use executeSingleStatement
      executeSingleStatement(line);
      break;
    }
    
    case CMD_PREC: {
      int val = args.toInt();
      if (val < 0) val = 0;
      if (val > 15) val = 15;
      printPrecision = val;
      break;
    }
    
    case CMD_DIM: {
      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);
        int d1 = 0, d2 = 0, d3 = 0;
        int comma1 = dimExpr.indexOf(',');
        if (comma1 == -1) {
          d1 = roundToInt(evalExprFast(dimExpr, isRadiansMode));
          dimArray(varName, d1 + 1);
        } else {
          d1 = roundToInt(evalExprFast(dimExpr.substring(0, comma1), isRadiansMode));
          String rest = dimExpr.substring(comma1 + 1);
          int comma2 = rest.indexOf(',');
          if (comma2 == -1) {
            d2 = roundToInt(evalExprFast(rest, isRadiansMode));
            dimArray(varName, d1 + 1, d2 + 1);
          } else {
            d2 = roundToInt(evalExprFast(rest.substring(0, comma2), isRadiansMode));
            d3 = roundToInt(evalExprFast(rest.substring(comma2 + 1), isRadiansMode));
            dimArray(varName, d1 + 1, d2 + 1, d3 + 1);
          }
        }
      }
      break;
    }
    
    case CMD_LET:
    default: {
      String uL = line; uL.toUpperCase();
      if (uL.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();
        if (lhs.indexOf('(') != -1 && lhs.endsWith(")")) {
          int lp = lhs.indexOf('(');
          String varName = lhs.substring(0, lp); varName.trim(); varName.toUpperCase();
          String idxExpr = lhs.substring(lp + 1, lhs.length() - 1);
          int commas = 0;
          for (int i = 0; i < (int)idxExpr.length(); i++) if (idxExpr.charAt(i) == ',') commas++;
          double value = evalExprFast(rhs, isRadiansMode);
          if (commas == 0) {
            setArrayValue(varName, roundToInt(evalExprFast(idxExpr, isRadiansMode)), value);
          } else if (commas == 1) {
            int c = idxExpr.indexOf(',');
            setArrayValue2D(varName, roundToInt(evalExprFast(idxExpr.substring(0, c), isRadiansMode)),
                           roundToInt(evalExprFast(idxExpr.substring(c + 1), isRadiansMode)), value);
          } else {
            int c1 = idxExpr.indexOf(','), c2 = idxExpr.indexOf(',', c1 + 1);
            setArrayValue3D(varName, roundToInt(evalExprFast(idxExpr.substring(0, c1), isRadiansMode)),
                           roundToInt(evalExprFast(idxExpr.substring(c1 + 1, c2), isRadiansMode)),
                           roundToInt(evalExprFast(idxExpr.substring(c2 + 1), isRadiansMode)), value);
          }
        } else {
          String varName = lhs; varName.toUpperCase();
          if (varName.endsWith("$")) {
            setStringValue(varName, evalStringExpr(rhs));
          } else {
            setVariableValue(varName, evalExprFast(rhs, isRadiansMode));
          }
        }
      }
      break;
    }
  }
  
  if (programState == RUNNING && !jumped) pc++;
}

void executeSingleStatement(String line) {
  line.trim();
  if (line.length() == 0) 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();
  
  // Use fast token lookup instead of string comparisons
  uint8_t cmd = lookupCommand(command);
  
  switch (cmd) {
    case CMD_REM:
      // Comment - do nothing
      break;
      
    case CMD_RAD:
      isRadiansMode = true;
      break;
      
    case CMD_DEG:
      isRadiansMode = false;
      break;
      
    case CMD_RANDOMIZE:
      if (args.length() > 0) {
        randomSeed((unsigned long)evalExprFast(args, isRadiansMode));
      } else {
        randomSeed(analogRead(0) ^ micros());
      }
      break;
      
    case CMD_END:
      programState = IDLE;
      break;
      
    case CMD_PRINT: {
      int i = 0;
      while (i < (int)args.length()) {
        String token = "";
        bool trailingSemicolon = false;
        int parenDepth = 0;
        bool inQuote = false;
        
        while (i < (int)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.length() > 0) {
          // Quick check: if starts with " or contains $, it's a string
          char fc = token.charAt(0);
          if (fc == '"' || token.indexOf('$') >= 0) {
            Serial.print(evalStringExpr(token));
          } else {
            Serial.print(evalExprFast(token, isRadiansMode), printPrecision);
          }
        }
        if (!trailingSemicolon && i >= (int)args.length()) {
          Serial.println();
        }
      }
      break;
    }
    
    case CMD_INPUT: {
      String prompt = "? ";
      String varPart = args;
      
      if (args.startsWith("\"")) {
        int endQuote = args.indexOf("\"", 1);
        if (endQuote != -1) {
          prompt = args.substring(1, endQuote);
          int sepPos = endQuote + 1;
          while (sepPos < (int)args.length() && (args.charAt(sepPos) == ' ' || args.charAt(sepPos) == ';' || args.charAt(sepPos) == ',')) {
            sepPos++;
          }
          varPart = args.substring(sepPos);
          varPart.trim();
        }
      }
      
      String varName = "";
      for (int ii = 0; ii < (int)varPart.length(); ii++) {
        char c = varPart.charAt(ii);
        if (isAlpha(c) || (ii > 0 && isDigit(c)) || c == '$') {
          varName += c;
        } else {
          break;
        }
      }
      varName.toUpperCase();
      
      if (varName.length() > 0) {
        awaitingInputVarName = varName;
        programState = AWAIT_INPUT;
        Serial.print(prompt);
      }
      break;
    }
    
    case CMD_GOTO: {
      int newPc = findLineIndex(args.toInt());
      if (newPc != -1) {
        pc = newPc;
        jumped = true;
      }
      break;
    }
    
    case CMD_PREC: {
      int val = args.toInt();
      if (val < 0) val = 0;
      if (val > 15) val = 15;
      printPrecision = val;
      break;
    }
    
    case CMD_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);
        
        int d1 = 0, d2 = 0, d3 = 0;
        int comma1 = dimExpr.indexOf(',');
        if (comma1 == -1) {
          d1 = roundToInt(evalExprFast(dimExpr, isRadiansMode));
        } else {
          d1 = roundToInt(evalExprFast(dimExpr.substring(0, comma1), isRadiansMode));
          String rest = dimExpr.substring(comma1 + 1);
          int comma2 = rest.indexOf(',');
          if (comma2 == -1) {
            d2 = roundToInt(evalExprFast(rest, isRadiansMode));
          } else {
            d2 = roundToInt(evalExprFast(rest.substring(0, comma2), isRadiansMode));
            d3 = roundToInt(evalExprFast(rest.substring(comma2 + 1), isRadiansMode));
          }
        }
        
        if (d1 > 0) {
          dimArray(varName, d1, d2, d3);
        }
      }
      break;
    }
    
    case CMD_IF: {
      String uArgs = args;
      uArgs.toUpperCase();
      int thenPos = uArgs.indexOf("THEN");
      if (thenPos == -1) break;
    
    String condStr = args.substring(0, thenPos);
    condStr.trim();
    String afterThen = args.substring(thenPos + 4);
    afterThen.trim();
    
    // Find ELSE (but not inside quotes or after colon)
    int elsePos = -1;
    bool inQuote = false;
    String uAfterThen = afterThen;
    uAfterThen.toUpperCase();
    for (int i = 0; i < (int)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 = "";
    }
    
    double condResult = evalExprFast(condStr, isRadiansMode);
    bool conditionMet = (condResult != 0);
    
    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 < (int)execPart.length(); i++) {
        if (!isDigit(execPart.charAt(i))) {
          isLineNum = false;
          break;
        }
      }
      
      if (isLineNum) {
        int targetLine = execPart.toInt();
        int newPc = findLineIndex(targetLine);
        if (newPc != -1) {
          pc = newPc;
          jumped = true;
        }
      } else {
        // Execute all colon-separated statements
        int start = 0;
        while (start < (int)execPart.length()) {
          // Find next colon not in quotes
          int colonPos = -1;
          inQuote = false;
          for (int i = start; i < (int)execPart.length(); i++) {
            if (execPart.charAt(i) == '"') inQuote = !inQuote;
            if (!inQuote && execPart.charAt(i) == ':') {
              colonPos = i;
              break;
            }
          }
          
          String stmt;
          if (colonPos >= 0) {
            stmt = execPart.substring(start, colonPos);
            start = colonPos + 1;
          } else {
            stmt = execPart.substring(start);
            start = execPart.length();
          }
          stmt.trim();
          if (stmt.length() == 0) continue;
          
          // Parse and execute single statement
          String tempCommand, tempArgs;
          String uStmt = stmt;
          uStmt.toUpperCase();
          
          int spacePos = stmt.indexOf(' ');
          if (spacePos != -1) {
            tempCommand = uStmt.substring(0, spacePos);
            tempArgs = stmt.substring(spacePos + 1);
          } else {
            tempCommand = uStmt;
            tempArgs = "";
          }
          tempArgs.trim();
          
          if (tempCommand == "PRINT") {
            int ii = 0;
            while (ii < (int)tempArgs.length()) {
              String token = "";
              bool inQ = false;
              int parenDepth = 0;
              bool trailingSemicolon = false;
              
              while (ii < (int)tempArgs.length()) {
                char c = tempArgs.charAt(ii);
                if (c == '"') { inQ = !inQ; token += c; ii++; }
                else if (inQ) { token += c; ii++; }
                else if (c == '(') { parenDepth++; token += c; ii++; }
                else if (c == ')') { parenDepth--; token += c; ii++; }
                else if (c == ';' && parenDepth == 0) { trailingSemicolon = true; ii++; break; }
                else { token += c; ii++; }
              }
              
              token.trim();
              if (token.length() > 0) {
                char fc = token.charAt(0);
                if (fc == '"' || token.indexOf('$') >= 0) {
                  Serial.print(evalStringExpr(token));
                } else {
                  Serial.print(evalExprFast(token, isRadiansMode), printPrecision);
                }
              }
              if (!trailingSemicolon && ii >= (int)tempArgs.length()) {
                Serial.println();
              }
            }
          }
          else if (tempCommand == "GOTO") {
            int targetLine = tempArgs.toInt();
            int newPc = findLineIndex(targetLine);
            if (newPc != -1) {
              pc = newPc;
              jumped = true;
            }
            break;  // Stop processing after GOTO
          }
          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;
            }
            break;
          }
          else if (tempCommand == "RETURN") {
            if (gosubStackPtr >= 0) {
              pc = gosubStack[gosubStackPtr];
              gosubStackPtr--;
              jumped = true;
            }
            break;
          }
          else if (tempCommand == "END") {
            programState = IDLE;
            break;
          }
          else {
            // Variable assignment
            int eqPos = stmt.indexOf('=');
            if (eqPos != -1 && isAlpha(stmt.charAt(0))) {
              char beforeEq = (eqPos > 0) ? stmt.charAt(eqPos - 1) : ' ';
              if (beforeEq != '<' && beforeEq != '>' && beforeEq != '!') {
                String lhs = stmt.substring(0, eqPos);
                lhs.trim();
                String rhs = stmt.substring(eqPos + 1);
                rhs.trim();
                
                if (lhs.indexOf('(') != -1 && lhs.endsWith(")")) {
                  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 = evalExprFast(rhs, isRadiansMode);
                  int i1 = roundToInt(evalExprFast(idxExpr, isRadiansMode));
                  setArrayValue(varName, i1, value);
                } else {
                  String varName = lhs;
                  varName.toUpperCase();
                  if (varName.endsWith("$")) {
                    setStringValue(varName, evalStringExpr(rhs));
                  } else {
                    setVariableValue(varName, evalExprFast(rhs, isRadiansMode));
                  }
                }
              }
            }
          }
        }
      }
    }
    break;
  }
  
  case CMD_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 = evalExprFast(endExpr, isRadiansMode);
        forLoopStack[forLoopStackPtr].stepValue = evalExprFast(stepExpr, isRadiansMode);
        forLoopStack[forLoopStackPtr].returnAddress = pc;
        setVariableValue(varName, evalExprFast(startExpr, isRadiansMode));
      }
    }
    break;
  }
  
  case CMD_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--;
      }
    }
    break;
  }
  
  case CMD_WHILE: {
    double condResult = evalExprFast(args, isRadiansMode);
    
    if (condResult != 0) {
      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;
      } else {
        Serial.println("WHILE without WEND!");
        programState = IDLE;
      }
    }
    break;
  }
  
  case CMD_WEND: {
    if (whileLoopStackPtr < 0) {
      Serial.println("WEND without WHILE!");
      programState = IDLE;
    } else {
      pc = whileLoopStack[whileLoopStackPtr].conditionAddress;
      whileLoopStackPtr--;
      jumped = true;
    }
    break;
  }
  
  case CMD_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;
      }
    }
    break;
  }
  
  case CMD_RETURN: {
    if (gosubStackPtr < 0) {
      Serial.println("RETURN without GOSUB!");
      programState = IDLE;
    } else {
      pc = gosubStack[gosubStackPtr];
      gosubStackPtr--;
    }
    break;
  }
  
  case CMD_LET:
  default: {
    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
      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 = evalExprFast(rhs, isRadiansMode);
          
          int commaCount = 0;
          for (int ci = 0; ci < (int)idxExpr.length(); ci++) {
            if (idxExpr.charAt(ci) == ',') commaCount++;
          }
          
          if (commaCount == 0) {
            int i1 = roundToInt(evalExprFast(idxExpr, isRadiansMode));
            setArrayValue(varName, i1, value);
          } else if (commaCount == 1) {
            int comma = idxExpr.indexOf(',');
            int i1 = roundToInt(evalExprFast(idxExpr.substring(0, comma), isRadiansMode));
            int i2 = roundToInt(evalExprFast(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(evalExprFast(idxExpr.substring(0, comma1), isRadiansMode));
            int i2 = roundToInt(evalExprFast(idxExpr.substring(comma1 + 1, comma2), isRadiansMode));
            int i3 = roundToInt(evalExprFast(idxExpr.substring(comma2 + 1), isRadiansMode));
            setArrayValue3D(varName, i1, i2, i3, value);
          }
        }
      } else {
        // Simple variable assignment
        String varName = lhs;
        varName.toUpperCase();
        // Check if string variable (ends with $)
        if (varName.endsWith("$")) {
          setStringValue(varName, evalStringExpr(rhs));
        } else if (isValidVarName(varName)) {
          setVariableValue(varName, evalExprFast(rhs, isRadiansMode));
        }
      }
    }
    break;
  }
  } // end switch
}

// =================================================================
// Main Setup and Loop
// =================================================================
void setup() {
  Serial.begin(9600);
  while (!Serial) { ; }
  
  randomSeed(analogRead(0) ^ micros());
  
  newProgram();
  Serial.print("\n--- yaZEKA BASIC v");
  Serial.print(YAZEKA_VERSION);
  Serial.println(" ---");
  Serial.println("Type HELP for commands");
  Serial.println("-------------------------------------");
  isRadiansMode = true;
  Serial.println("Ready.");
}

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 = evalExprFast(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(evalExprFast(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 = evalExprFast(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(evalExprFast(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("HELP")) {
    Serial.println("=== yaZEKA BASIC HELP ===");
    Serial.println("Commands: RUN, LIST, NEW, BREAK, HELP");
    Serial.println("Statements: LET, PRINT, INPUT, GOTO, GOSUB, RETURN");
    Serial.println("Control: IF/THEN/ELSE, FOR/TO/STEP/NEXT, WHILE/WEND");
    Serial.println("Arrays: DIM A(n), DIM B(x,y), DIM C(x,y,z)");
    Serial.println("Math: SIN COS TAN ASIN ACOS ATAN SQRT LOG EXP ABS");
    Serial.println("      INT SGN RND MIN MAX POW FACT MILLIS");
    Serial.println("String: LEFT$ RIGHT$ MID$ LEN ASC CHR$ STR$ VAL");
    Serial.println("        UPPER$ LOWER$ INSTR STRING$ SPACE$ HEX$ BIN$");
    Serial.println("Operators: + - * / ^ MOD AND OR XOR NOT < > <= >= = <>");
    Serial.println("Modes: RAD, DEG, PREC n");
    Serial.println("Use : to separate statements on one line");
    Serial.println("Use \\ or newline to paste multiple lines");
  } else if (command.equalsIgnoreCase("RANDOMIZE")) {
    if (args.length() > 0) {
      randomSeed((unsigned long)evalExprFast(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(evalExprFast(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(evalExprFast(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(evalExprFast(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 = evalExprFast(input, isRadiansMode);
      setVariableValue(awaitingInputVarName, res);
      awaitingInputVarName = "";
      programState = RUNNING;
      pc++;
    }
    return;
  }

  // Replace CR and LF with backslash for multi-line paste support
  input.replace("\r\n", "\\");
  input.replace("\r", "\\");
  input.replace("\n", "\\");

  // If the input contains backslashes, split by them.
  if (input.indexOf('\\') != -1) {
    int lastPos = 0;
    while (lastPos < (int)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.println("Ready.");
      }
    } 
    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);
    }
  }
}