/*
 * ESP32 BASIC Scientific Calculator
 * 
 * A complete BASIC interpreter with scientific calculator capabilities
 * for ESP32 microcontrollers. Communicates via serial at 9600 baud.
 * 
 * Features:
 * - Full expression parser with operator precedence
 * - Scientific functions (trig, log, exp, etc.)
 * - BASIC programming with line numbers
 * - Arrays, loops, conditionals
 * - Degree/Radian modes
 */

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

// ==================== Configuration ====================
#define STACK_SIZE 32
#define MAX_PROGRAM_LINES 500
#define MAX_LOOP_DEPTH 32
#define MAX_ARRAY_SIZE 256
#define MAX_INPUT_LENGTH 128
#define MAX_LINE_LENGTH 96
#define NUM_VARIABLES 26
#define SERIAL_TIMEOUT 50

// ==================== Enums ====================
enum ExecutionState {
  STATE_IDLE,
  STATE_RUNNING,
  STATE_AWAIT_INPUT
};

enum TokenType {
  TOKEN_NUMBER,
  TOKEN_VARIABLE,
  TOKEN_OPERATOR,
  TOKEN_FUNCTION,
  TOKEN_LPAREN,
  TOKEN_RPAREN,
  TOKEN_COMMA,
  TOKEN_ARRAY_ACCESS,
  TOKEN_END
};

// ==================== Data Structures ====================
struct Token {
  TokenType type;
  double numValue;
  char strValue[12];
  int varIndex;
};

struct ProgramLine {
  int lineNumber;
  char code[MAX_LINE_LENGTH];
};

struct ForLoopState {
  int varIndex;
  double endValue;
  double stepValue;
  int returnLine;
};

// ==================== Global Variables ====================
// Calculator state
double variables[NUM_VARIABLES];
double* arrays[NUM_VARIABLES];
int arraySizes[NUM_VARIABLES];
bool degreeMode = false;

// Program storage
ProgramLine program[MAX_PROGRAM_LINES];
int programLineCount = 0;

// Execution state
ExecutionState execState = STATE_IDLE;
int programCounter = 0;
int inputTargetVar = -1;
int printPrecision = 15;

// Loop stack
ForLoopState loopStack[MAX_LOOP_DEPTH];
int loopStackTop = 0;

// Input buffer
char inputBuffer[MAX_INPUT_LENGTH];
int inputBufferPos = 0;

// Expression evaluation stacks
double valueStack[STACK_SIZE];
int valueStackTop = 0;
char opStack[STACK_SIZE][12];
int opStackTop = 0;

// ==================== Function Declarations ====================
void processCommand(const char* cmd);
void executeStatement(const char* stmt);
double evaluateExpression(const char* expr);
bool isProgramLineStart(const char* str);
int getOperatorPrecedence(const char* op);
bool isRightAssociative(const char* op);
double applyOperator(const char* op, double a, double b);
double applyFunction(const char* func, double arg);
void tokenize(const char* expr, Token* tokens, int* tokenCount);
void infixToPostfix(Token* infix, int infixCount, Token* postfix, int* postfixCount);
double evaluatePostfix(Token* postfix, int count);
void runProgram();
void listProgram();
void clearProgram();
int findLineIndex(int lineNum);
void insertProgramLine(int lineNum, const char* code);
void deleteProgramLine(int lineNum);
void handlePrint(const char* args);
void handleInput(const char* args);
void handleLet(const char* args);
void handleLetSilent(const char* args);
void handleLetInternal(const char* args, bool silent);
void handleGoto(const char* args);
void handleIf(const char* stmt);
void handleFor(const char* args);
void handleNext(const char* args);
void handleDim(const char* args);
void handlePrec(const char* args);
void handlePrecSilent(const char* args);
double getArrayElement(int varIndex, int index);
void setArrayElement(int varIndex, int index, double value);
void trimWhitespace(char* str);
void toUpperCase(char* str);
void removeSpaces(char* str);
bool startsWith(const char* str, const char* prefix);
int findNextLine(int currentLineNum);

// ==================== Setup and Loop ====================
void setup() {
  Serial.begin(9600);
  while (!Serial) {
    delay(10);
  }
  
  // Initialize variables
  for (int i = 0; i < NUM_VARIABLES; i++) {
    variables[i] = 0.0;
    arrays[i] = nullptr;
    arraySizes[i] = 0;
  }
  
  // Print startup banner
  Serial.println();
  Serial.println(F("================================"));
  Serial.println(F("  ESP32 BASIC Scientific Calc"));
  Serial.println(F("================================"));
  Serial.println(F("Type expressions or BASIC commands"));
  Serial.println(F("Commands: RUN, LIST, NEW, BREAK"));
  Serial.println(F("Functions: SIN, COS, TAN, SQRT, LN, LOG, EXP"));
  Serial.println(F("Constants: PI, E"));
  Serial.println(F("Use \\ to paste multiple lines"));
  Serial.println();
  Serial.print(F("> "));
}

void loop() {
  // Handle serial input
  while (Serial.available()) {
    char c = Serial.read();
    
    // Backslash acts as line terminator (for pasting multi-line programs)
    if (c == '\\') {
      if (inputBufferPos > 0) {
        inputBuffer[inputBufferPos] = '\0';
        Serial.println();
        processSingleLine(inputBuffer);
        inputBufferPos = 0;
      }
      // Don't print prompt - more lines coming
      continue;
    }
    
    if (c == '\r' || c == '\n') {
      if (inputBufferPos > 0) {
        inputBuffer[inputBufferPos] = '\0';
        Serial.println();
        processSingleLine(inputBuffer);
        inputBufferPos = 0;
      }
      printPrompt();
    } else if (c == 8 || c == 127) {  // Backspace
      if (inputBufferPos > 0) {
        inputBufferPos--;
        Serial.print(F("\b \b"));
      }
    } else if (isPrintable(c) && inputBufferPos < MAX_INPUT_LENGTH - 1) {
      inputBuffer[inputBufferPos++] = c;
      Serial.print(c);
    }
  }
  
  // Continue program execution if running
  if (execState == STATE_RUNNING) {
    executeNextLine();
  }
}

void printPrompt() {
  if (execState == STATE_IDLE) {
    Serial.print(F("> "));
  } else if (execState == STATE_AWAIT_INPUT) {
    Serial.print(F("? "));
  }
}

// processInput no longer needed for backslash handling, simplified
void processInput(const char* input) {
  processSingleLine(input);
}

void processSingleLine(const char* line) {
  char cmdCopy[MAX_INPUT_LENGTH];
  strncpy(cmdCopy, line, MAX_INPUT_LENGTH - 1);
  cmdCopy[MAX_INPUT_LENGTH - 1] = '\0';
  trimWhitespace(cmdCopy);
  
  if (strlen(cmdCopy) == 0) return;
  
  // Handle input during AWAIT_INPUT state
  if (execState == STATE_AWAIT_INPUT) {
    double value = evaluateExpression(cmdCopy);
    if (inputTargetVar >= 0 && inputTargetVar < NUM_VARIABLES) {
      variables[inputTargetVar] = value;
    }
    execState = STATE_RUNNING;
    return;
  }
  
  // Check if this is a program line
  if (isProgramLineStart(cmdCopy)) {
    storeProgramLine(cmdCopy);
    return;
  }
  
  // Process as immediate command
  processCommand(cmdCopy);
}

bool isProgramLineStart(const char* str) {
  if (!isdigit(str[0])) return false;
  
  // Check for decimal point (would indicate expression like 0.5)
  const char* p = str;
  while (*p && (isdigit(*p))) p++;
  
  if (*p == '.') return false;  // Has decimal, not a line number
  
  // Must be followed by nothing, space, or end
  return (*p == '\0' || *p == ' ');
}

void storeProgramLine(const char* line) {
  int lineNum = atoi(line);
  
  // Find where code starts (after line number and space)
  const char* code = line;
  while (*code && isdigit(*code)) code++;
  while (*code && *code == ' ') code++;
  
  if (strlen(code) == 0) {
    // Empty code means delete line
    deleteProgramLine(lineNum);
  } else {
    insertProgramLine(lineNum, code);
  }
}

void insertProgramLine(int lineNum, const char* code) {
  // Check if line exists and update it
  for (int i = 0; i < programLineCount; i++) {
    if (program[i].lineNumber == lineNum) {
      strncpy(program[i].code, code, sizeof(program[i].code) - 1);
      program[i].code[sizeof(program[i].code) - 1] = '\0';
      return;
    }
  }
  
  // Add new line
  if (programLineCount >= MAX_PROGRAM_LINES) {
    Serial.println(F("Program memory full!"));
    return;
  }
  
  // Find insertion point to maintain sorted order
  int insertPos = programLineCount;
  for (int i = 0; i < programLineCount; i++) {
    if (program[i].lineNumber > lineNum) {
      insertPos = i;
      break;
    }
  }
  
  // Shift lines down
  for (int i = programLineCount; i > insertPos; i--) {
    program[i] = program[i - 1];
  }
  
  // Insert new line
  program[insertPos].lineNumber = lineNum;
  strncpy(program[insertPos].code, code, sizeof(program[insertPos].code) - 1);
  program[insertPos].code[sizeof(program[insertPos].code) - 1] = '\0';
  programLineCount++;
}

void deleteProgramLine(int lineNum) {
  for (int i = 0; i < programLineCount; i++) {
    if (program[i].lineNumber == lineNum) {
      // Shift lines up
      for (int j = i; j < programLineCount - 1; j++) {
        program[j] = program[j + 1];
      }
      programLineCount--;
      return;
    }
  }
}

int findLineIndex(int lineNum) {
  for (int i = 0; i < programLineCount; i++) {
    if (program[i].lineNumber == lineNum) {
      return i;
    }
  }
  return -1;
}

int findNextLine(int currentLineNum) {
  for (int i = 0; i < programLineCount; i++) {
    if (program[i].lineNumber > currentLineNum) {
      return i;
    }
  }
  return -1;
}

void processCommand(const char* cmd) {
  char cmdUpper[MAX_INPUT_LENGTH];
  strncpy(cmdUpper, cmd, MAX_INPUT_LENGTH - 1);
  cmdUpper[MAX_INPUT_LENGTH - 1] = '\0';
  toUpperCase(cmdUpper);
  
  // Handle immediate commands
  if (strcmp(cmdUpper, "RUN") == 0) {
    runProgram();
    return;
  }
  
  if (strcmp(cmdUpper, "LIST") == 0) {
    listProgram();
    return;
  }
  
  if (strcmp(cmdUpper, "NEW") == 0) {
    clearProgram();
    Serial.println(F("Program cleared"));
    return;
  }
  
  if (strcmp(cmdUpper, "BREAK") == 0) {
    if (execState == STATE_RUNNING) {
      execState = STATE_IDLE;
      Serial.println(F("*** BREAK ***"));
    }
    return;
  }
  
  if (strcmp(cmdUpper, "RAD") == 0) {
    degreeMode = false;
    Serial.println(F("Radian mode"));
    return;
  }
  
  if (strcmp(cmdUpper, "DEG") == 0) {
    degreeMode = true;
    Serial.println(F("Degree mode"));
    return;
  }
  
  // Try to execute as statement
  executeStatement(cmd);
}

void runProgram() {
  if (programLineCount == 0) {
    Serial.println(F("No program loaded"));
    return;
  }
  
  execState = STATE_RUNNING;
  programCounter = 0;
  loopStackTop = 0;
}

void executeNextLine() {
  if (programCounter >= programLineCount) {
    execState = STATE_IDLE;
    return;
  }
  
  int currentLine = program[programCounter].lineNumber;
  char* code = program[programCounter].code;
  
  // Default: move to next line
  programCounter++;
  
  // Execute the statement
  executeStatementInProgram(code, currentLine);
}

void executeStatementInProgram(const char* stmt, int lineNum) {
  char stmtUpper[MAX_LINE_LENGTH];
  strncpy(stmtUpper, stmt, sizeof(stmtUpper) - 1);
  stmtUpper[sizeof(stmtUpper) - 1] = '\0';
  toUpperCase(stmtUpper);
  
  // Skip REM statements
  if (startsWith(stmtUpper, "REM")) {
    return;
  }
  
  // Handle END
  if (strcmp(stmtUpper, "END") == 0) {
    execState = STATE_IDLE;
    return;
  }
  
  // Handle GOTO
  if (startsWith(stmtUpper, "GOTO")) {
    const char* args = stmt + 4;
    while (*args == ' ') args++;
    int targetLine = atoi(args);
    int targetIndex = findLineIndex(targetLine);
    if (targetIndex >= 0) {
      programCounter = targetIndex;
    }
    return;
  }
  
  // Handle IF/THEN
  if (startsWith(stmtUpper, "IF")) {
    handleIfInProgram(stmt);
    return;
  }
  
  // Handle FOR
  if (startsWith(stmtUpper, "FOR")) {
    handleForInProgram(stmt, lineNum);
    return;
  }
  
  // Handle NEXT
  if (startsWith(stmtUpper, "NEXT")) {
    handleNextInProgram(stmt);
    return;
  }
  
  // Handle PRINT
  if (startsWith(stmtUpper, "PRINT")) {
    const char* args = stmt + 5;
    while (*args == ' ') args++;
    handlePrint(args);
    return;
  }
  
  // Handle INPUT
  if (startsWith(stmtUpper, "INPUT")) {
    const char* args = stmt + 5;
    while (*args == ' ') args++;
    handleInput(args);
    return;
  }
  
  // Handle DIM
  if (startsWith(stmtUpper, "DIM")) {
    const char* args = stmt + 3;
    while (*args == ' ') args++;
    handleDim(args);
    return;
  }
  
  // Handle PREC
  if (startsWith(stmtUpper, "PREC")) {
    const char* args = stmt + 4;
    while (*args == ' ') args++;
    handlePrecSilent(args);
    return;
  }
  
  // Handle RAD mode switch
  if (strcmp(stmtUpper, "RAD") == 0) {
    degreeMode = false;
    return;
  }
  
  // Handle DEG mode switch
  if (strcmp(stmtUpper, "DEG") == 0) {
    degreeMode = true;
    return;
  }
  
  // Handle LET or direct assignment
  if (startsWith(stmtUpper, "LET")) {
    const char* args = stmt + 3;
    while (*args == ' ') args++;
    handleLetSilent(args);
    return;
  }
  
  // Try as direct assignment or expression
  handleLetSilent(stmt);
}

void executeStatement(const char* stmt) {
  char stmtUpper[MAX_INPUT_LENGTH];
  strncpy(stmtUpper, stmt, MAX_INPUT_LENGTH - 1);
  stmtUpper[MAX_INPUT_LENGTH - 1] = '\0';
  toUpperCase(stmtUpper);
  
  // Handle PRINT
  if (startsWith(stmtUpper, "PRINT")) {
    const char* args = stmt + 5;
    while (*args == ' ') args++;
    handlePrint(args);
    return;
  }
  
  // Handle DIM
  if (startsWith(stmtUpper, "DIM")) {
    const char* args = stmt + 3;
    while (*args == ' ') args++;
    handleDim(args);
    Serial.print(F("DIM "));
    Serial.println(args);
    return;
  }
  
  // Handle PREC
  if (startsWith(stmtUpper, "PREC")) {
    const char* args = stmt + 4;
    while (*args == ' ') args++;
    handlePrec(args);
    return;
  }
  
  // Handle LET or direct assignment
  if (startsWith(stmtUpper, "LET")) {
    const char* args = stmt + 3;
    while (*args == ' ') args++;
    handleLet(args);
    return;
  }
  
  // Try as assignment or expression
  handleLet(stmt);
}

void listProgram() {
  if (programLineCount == 0) {
    Serial.println(F("No program loaded"));
    return;
  }
  
  for (int i = 0; i < programLineCount; i++) {
    Serial.print(program[i].lineNumber);
    Serial.print(F(" "));
    Serial.println(program[i].code);
  }
}

void clearProgram() {
  programLineCount = 0;
  loopStackTop = 0;
  execState = STATE_IDLE;
  
  // Clear variables
  for (int i = 0; i < NUM_VARIABLES; i++) {
    variables[i] = 0.0;
    if (arrays[i] != nullptr) {
      free(arrays[i]);
      arrays[i] = nullptr;
    }
    arraySizes[i] = 0;
  }
}

// ==================== Statement Handlers ====================
void handlePrint(const char* args) {
  if (strlen(args) == 0) {
    Serial.println();
    return;
  }
  
  bool suppressNewline = false;
  char buffer[MAX_INPUT_LENGTH];
  strncpy(buffer, args, MAX_INPUT_LENGTH - 1);
  buffer[MAX_INPUT_LENGTH - 1] = '\0';
  
  char* p = buffer;
  
  while (*p) {
    // Skip whitespace
    while (*p == ' ') p++;
    if (*p == '\0') break;
    
    // Check for string literal
    if (*p == '"') {
      p++;  // Skip opening quote
      while (*p && *p != '"') {
        Serial.print(*p);
        p++;
      }
      if (*p == '"') p++;  // Skip closing quote
    } else {
      // Find end of expression (semicolon or end)
      char* exprStart = p;
      int parenDepth = 0;
      bool inQuotes = false;
      
      while (*p) {
        if (*p == '"') inQuotes = !inQuotes;
        else if (!inQuotes) {
          if (*p == '(') parenDepth++;
          else if (*p == ')') parenDepth--;
          else if (*p == ';' && parenDepth == 0) break;
        }
        p++;
      }
      
      // Extract and evaluate expression
      char expr[MAX_LINE_LENGTH];
      int len = p - exprStart;
      strncpy(expr, exprStart, len);
      expr[len] = '\0';
      trimWhitespace(expr);
      
      if (strlen(expr) > 0) {
        double value = evaluateExpression(expr);
        Serial.print(value, printPrecision);
      }
    }
    
    // Check for semicolon
    if (*p == ';') {
      suppressNewline = true;
      p++;
    } else {
      suppressNewline = false;
    }
  }
  
  if (!suppressNewline) {
    Serial.println();
  }
}

void handleInput(const char* args) {
  char varName = toupper(args[0]);
  if (varName >= 'A' && varName <= 'Z') {
    inputTargetVar = varName - 'A';
    execState = STATE_AWAIT_INPUT;
    Serial.print(F("? "));
  }
}

void handleLet(const char* args) {
  handleLetInternal(args, false);
}

void handleLetSilent(const char* args) {
  handleLetInternal(args, true);
}

void handleLetInternal(const char* args, bool silent) {
  // Find the equals sign
  const char* eqPos = strchr(args, '=');
  if (eqPos == nullptr) {
    // Just an expression, evaluate and print (only if not silent)
    double result = evaluateExpression(args);
    if (!silent) {
      Serial.println(result, printPrecision);
    }
    return;
  }
  
  // Extract variable/array part
  char varPart[32];
  int varLen = eqPos - args;
  strncpy(varPart, args, varLen);
  varPart[varLen] = '\0';
  trimWhitespace(varPart);
  toUpperCase(varPart);
  
  // Extract expression part
  const char* exprPart = eqPos + 1;
  while (*exprPart == ' ') exprPart++;
  
  double value = evaluateExpression(exprPart);
  
  // Check for array assignment
  char* parenPos = strchr(varPart, '(');
  if (parenPos != nullptr) {
    // Array assignment
    char varName = varPart[0];
    int varIndex = varName - 'A';
    
    // Extract index expression
    char indexExpr[32];
    char* closeParen = strchr(parenPos, ')');
    if (closeParen != nullptr) {
      int indexLen = closeParen - parenPos - 1;
      strncpy(indexExpr, parenPos + 1, indexLen);
      indexExpr[indexLen] = '\0';
      
      int arrayIndex = (int)evaluateExpression(indexExpr);
      setArrayElement(varIndex, arrayIndex, value);
      
      if (!silent) {
        Serial.print(varName);
        Serial.print(F("("));
        Serial.print(arrayIndex);
        Serial.print(F(") = "));
        Serial.println(value, printPrecision);
      }
    }
  } else {
    // Simple variable assignment
    char varName = varPart[0];
    if (varName >= 'A' && varName <= 'Z') {
      int varIndex = varName - 'A';
      variables[varIndex] = value;
      
      if (!silent) {
        Serial.print(varName);
        Serial.print(F(" = "));
        Serial.println(value, printPrecision);
      }
    }
  }
}

void handleIfInProgram(const char* stmt) {
  char stmtCopy[MAX_LINE_LENGTH];
  strncpy(stmtCopy, stmt, sizeof(stmtCopy) - 1);
  stmtCopy[sizeof(stmtCopy) - 1] = '\0';
  toUpperCase(stmtCopy);
  
  // Find THEN
  char* thenPos = strstr(stmtCopy, "THEN");
  if (thenPos == nullptr) return;
  
  // Extract condition
  char condition[48];
  int condLen = thenPos - stmtCopy - 2;  // -2 for "IF"
  strncpy(condition, stmtCopy + 2, condLen);
  condition[condLen] = '\0';
  trimWhitespace(condition);
  
  // Extract target line
  const char* targetStr = thenPos + 4;
  while (*targetStr == ' ') targetStr++;
  int targetLine = atoi(targetStr);
  
  // Evaluate condition
  bool condResult = evaluateCondition(condition);
  
  if (condResult) {
    int targetIndex = findLineIndex(targetLine);
    if (targetIndex >= 0) {
      programCounter = targetIndex;
    }
  }
}

bool evaluateCondition(const char* condition) {
  // Find comparison operator
  const char* ops[] = {"<=", ">=", "<>", "!=", "<", ">", "="};
  int opIndex = -1;
  const char* opPos = nullptr;
  
  for (int i = 0; i < 7; i++) {
    const char* pos = strstr(condition, ops[i]);
    if (pos != nullptr) {
      opPos = pos;
      opIndex = i;
      break;
    }
  }
  
  if (opPos == nullptr) return false;
  
  // Extract left and right expressions
  char left[32], right[32];
  int leftLen = opPos - condition;
  strncpy(left, condition, leftLen);
  left[leftLen] = '\0';
  
  int opLen = strlen(ops[opIndex]);
  strcpy(right, opPos + opLen);
  
  trimWhitespace(left);
  trimWhitespace(right);
  
  double leftVal = evaluateExpression(left);
  double rightVal = evaluateExpression(right);
  
  switch (opIndex) {
    case 0: return leftVal <= rightVal;  // <=
    case 1: return leftVal >= rightVal;  // >=
    case 2: return leftVal != rightVal;  // <>
    case 3: return leftVal != rightVal;  // !=
    case 4: return leftVal < rightVal;   // <
    case 5: return leftVal > rightVal;   // >
    case 6: return fabs(leftVal - rightVal) < 0.0001;  // =
  }
  
  return false;
}

void handleForInProgram(const char* stmt, int lineNum) {
  char stmtCopy[MAX_LINE_LENGTH];
  strncpy(stmtCopy, stmt, sizeof(stmtCopy) - 1);
  stmtCopy[sizeof(stmtCopy) - 1] = '\0';
  toUpperCase(stmtCopy);
  
  // Parse: FOR var = start TO end [STEP step]
  char* forPos = strstr(stmtCopy, "FOR");
  if (forPos == nullptr) return;
  
  char* p = forPos + 3;
  while (*p == ' ') p++;
  
  // Get variable
  char varName = *p;
  int varIndex = varName - 'A';
  p++;
  
  // Find =
  while (*p && *p != '=') p++;
  if (*p != '=') return;
  p++;
  
  // Find TO
  char* toPos = strstr(p, "TO");
  if (toPos == nullptr) return;
  
  // Extract start value
  char startExpr[32];
  int startLen = toPos - p;
  strncpy(startExpr, p, startLen);
  startExpr[startLen] = '\0';
  trimWhitespace(startExpr);
  
  // Find STEP (optional)
  char* stepPos = strstr(toPos, "STEP");
  
  // Extract end value
  char endExpr[32];
  if (stepPos != nullptr) {
    int endLen = stepPos - toPos - 2;
    strncpy(endExpr, toPos + 2, endLen);
    endExpr[endLen] = '\0';
  } else {
    strcpy(endExpr, toPos + 2);
  }
  trimWhitespace(endExpr);
  
  // Extract step value
  double stepVal = 1.0;
  if (stepPos != nullptr) {
    char stepExpr[32];
    strcpy(stepExpr, stepPos + 4);
    trimWhitespace(stepExpr);
    stepVal = evaluateExpression(stepExpr);
  }
  
  double startVal = evaluateExpression(startExpr);
  double endVal = evaluateExpression(endExpr);
  
  // Set variable to start value
  variables[varIndex] = startVal;
  
  // Push loop state
  if (loopStackTop >= MAX_LOOP_DEPTH) {
    Serial.println(F("Loop depth exceeded!"));
    execState = STATE_IDLE;
    return;
  }
  
  loopStack[loopStackTop].varIndex = varIndex;
  loopStack[loopStackTop].endValue = endVal;
  loopStack[loopStackTop].stepValue = stepVal;
  loopStack[loopStackTop].returnLine = lineNum;
  loopStackTop++;
}

void handleNextInProgram(const char* stmt) {
  char stmtCopy[MAX_LINE_LENGTH];
  strncpy(stmtCopy, stmt, sizeof(stmtCopy) - 1);
  stmtCopy[sizeof(stmtCopy) - 1] = '\0';
  toUpperCase(stmtCopy);
  
  // Get variable name
  char* p = stmtCopy + 4;
  while (*p == ' ') p++;
  char varName = *p;
  int varIndex = varName - 'A';
  
  // Find matching FOR
  int loopIndex = -1;
  for (int i = loopStackTop - 1; i >= 0; i--) {
    if (loopStack[i].varIndex == varIndex) {
      loopIndex = i;
      break;
    }
  }
  
  if (loopIndex < 0) {
    Serial.println(F("NEXT without FOR error!"));
    execState = STATE_IDLE;
    return;
  }
  
  // Increment variable
  variables[varIndex] += loopStack[loopIndex].stepValue;
  
  // Check condition
  double current = variables[varIndex];
  double end = loopStack[loopIndex].endValue;
  double step = loopStack[loopIndex].stepValue;
  
  bool continueLoop;
  if (step > 0) {
    continueLoop = current <= end + 0.0001;
  } else {
    continueLoop = current >= end - 0.0001;
  }
  
  if (continueLoop) {
    // Jump back to line after FOR
    int forLine = loopStack[loopIndex].returnLine;
    int nextIndex = findNextLine(forLine);
    if (nextIndex >= 0) {
      programCounter = nextIndex;
    }
  } else {
    // Exit loop - remove from stack
    loopStackTop = loopIndex;
  }
}

void handleDim(const char* args) {
  char argsCopy[32];
  strncpy(argsCopy, args, sizeof(argsCopy) - 1);
  argsCopy[sizeof(argsCopy) - 1] = '\0';
  toUpperCase(argsCopy);
  
  // Parse: DIM A(size)
  char varName = argsCopy[0];
  int varIndex = varName - 'A';
  
  char* parenPos = strchr(argsCopy, '(');
  if (parenPos == nullptr) return;
  
  char* closePos = strchr(parenPos, ')');
  if (closePos == nullptr) return;
  
  char sizeStr[16];
  int sizeLen = closePos - parenPos - 1;
  strncpy(sizeStr, parenPos + 1, sizeLen);
  sizeStr[sizeLen] = '\0';
  
  int size = atoi(sizeStr);
  if (size <= 0 || size > MAX_ARRAY_SIZE) {
    Serial.println(F("Invalid array size!"));
    return;
  }
  
  // Free existing array if any
  if (arrays[varIndex] != nullptr) {
    free(arrays[varIndex]);
  }
  
  // Allocate new array
  arrays[varIndex] = (double*)malloc(size * sizeof(double));
  if (arrays[varIndex] == nullptr) {
    Serial.println(F("Memory allocation failed!"));
    arraySizes[varIndex] = 0;
    return;
  }
  
  arraySizes[varIndex] = size;
  
  // Initialize to zero
  for (int i = 0; i < size; i++) {
    arrays[varIndex][i] = 0.0;
  }
}

void handlePrec(const char* args) {
  int prec = atoi(args);
  if (prec >= 0 && prec <= 15) {
    printPrecision = prec;
    Serial.print(F("Precision set to "));
    Serial.println(prec);
  }
}

void handlePrecSilent(const char* args) {
  int prec = atoi(args);
  if (prec >= 0 && prec <= 15) {
    printPrecision = prec;
  }
}

double getArrayElement(int varIndex, int index) {
  if (varIndex < 0 || varIndex >= NUM_VARIABLES) {
    Serial.println(F("Invalid variable!"));
    return NAN;
  }
  
  if (arrays[varIndex] == nullptr) {
    Serial.println(F("Array not DIMmed!"));
    return NAN;
  }
  
  if (index < 0 || index >= arraySizes[varIndex]) {
    Serial.println(F("Array index out of range!"));
    return NAN;
  }
  
  return arrays[varIndex][index];
}

void setArrayElement(int varIndex, int index, double value) {
  if (varIndex < 0 || varIndex >= NUM_VARIABLES) {
    Serial.println(F("Invalid variable!"));
    return;
  }
  
  // Auto-allocate if needed
  if (arrays[varIndex] == nullptr) {
    int size = index + 1;
    if (size > MAX_ARRAY_SIZE) size = MAX_ARRAY_SIZE;
    arrays[varIndex] = (double*)malloc(size * sizeof(double));
    if (arrays[varIndex] == nullptr) {
      Serial.println(F("Memory allocation failed!"));
      return;
    }
    arraySizes[varIndex] = size;
    for (int i = 0; i < size; i++) {
      arrays[varIndex][i] = 0.0;
    }
  }
  
  // Auto-grow if needed
  if (index >= arraySizes[varIndex]) {
    int newSize = index + 1;
    if (newSize > MAX_ARRAY_SIZE) {
      Serial.println(F("Array size limit exceeded!"));
      return;
    }
    
    double* newArray = (double*)realloc(arrays[varIndex], newSize * sizeof(double));
    if (newArray == nullptr) {
      Serial.println(F("Memory allocation failed!"));
      return;
    }
    
    // Initialize new elements
    for (int i = arraySizes[varIndex]; i < newSize; i++) {
      newArray[i] = 0.0;
    }
    
    arrays[varIndex] = newArray;
    arraySizes[varIndex] = newSize;
  }
  
  if (index < 0) {
    Serial.println(F("Negative array index!"));
    return;
  }
  
  arrays[varIndex][index] = value;
}

// ==================== Expression Evaluator ====================
double evaluateExpression(const char* expr) {
  char exprCopy[MAX_INPUT_LENGTH];
  strncpy(exprCopy, expr, MAX_INPUT_LENGTH - 1);
  exprCopy[MAX_INPUT_LENGTH - 1] = '\0';
  
  // Preprocess: replace π with PI
  char* piPos;
  while ((piPos = strstr(exprCopy, "π")) != nullptr) {
    // Replace π (2 bytes UTF-8) with PI
    memmove(piPos + 2, piPos + 2, strlen(piPos + 2) + 1);
    piPos[0] = 'P';
    piPos[1] = 'I';
  }
  
  // Remove spaces and convert to uppercase
  removeSpaces(exprCopy);
  toUpperCase(exprCopy);
  
  if (strlen(exprCopy) == 0) return 0.0;
  
  // Tokenize
  Token tokens[STACK_SIZE];
  int tokenCount = 0;
  tokenize(exprCopy, tokens, &tokenCount);
  
  if (tokenCount == 0) return 0.0;
  
  // Handle unary operators - convert to high-precedence operators
  for (int i = 0; i < tokenCount; i++) {
    if (tokens[i].type == TOKEN_OPERATOR) {
      if (strcmp(tokens[i].strValue, "-") == 0 || strcmp(tokens[i].strValue, "+") == 0) {
        // Check if unary (at start or after operator/lparen/comma/function)
        bool isUnary = (i == 0) ||
                       (tokens[i-1].type == TOKEN_OPERATOR) ||
                       (tokens[i-1].type == TOKEN_LPAREN) ||
                       (tokens[i-1].type == TOKEN_COMMA) ||
                       (tokens[i-1].type == TOKEN_FUNCTION) ||
                       (tokens[i-1].type == TOKEN_ARRAY_ACCESS);
        
        if (isUnary) {
          if (strcmp(tokens[i].strValue, "-") == 0) {
            strcpy(tokens[i].strValue, "NEG");
            // Keep as TOKEN_OPERATOR - will handle specially
          } else {
            strcpy(tokens[i].strValue, "POS");
            // Keep as TOKEN_OPERATOR - will handle specially
          }
        }
      }
    }
  }
  
  // Convert to postfix
  Token postfix[STACK_SIZE];
  int postfixCount = 0;
  infixToPostfix(tokens, tokenCount, postfix, &postfixCount);
  
  // Evaluate postfix
  return evaluatePostfix(postfix, postfixCount);
}

void tokenize(const char* expr, Token* tokens, int* tokenCount) {
  *tokenCount = 0;
  const char* p = expr;
  
  while (*p) {
    // Skip whitespace
    while (*p == ' ') p++;
    if (*p == '\0') break;
    
    Token* t = &tokens[*tokenCount];
    
    // Check for number
    if (isdigit(*p) || (*p == '.' && isdigit(*(p+1)))) {
      t->type = TOKEN_NUMBER;
      char numStr[20];
      int numLen = 0;
      
      // Collect digits and decimal point
      while (isdigit(*p) || *p == '.') {
        numStr[numLen++] = *p++;
      }
      
      // Check for scientific notation (E followed by digit or +/- then digit)
      if ((*p == 'E' || *p == 'e') && 
          (isdigit(*(p+1)) || 
           ((*(p+1) == '+' || *(p+1) == '-') && isdigit(*(p+2))))) {
        numStr[numLen++] = *p++;  // E or e
        if (*p == '+' || *p == '-') {
          numStr[numLen++] = *p++;  // sign
        }
        while (isdigit(*p)) {
          numStr[numLen++] = *p++;  // exponent digits
        }
      }
      
      numStr[numLen] = '\0';
      t->numValue = atof(numStr);
      (*tokenCount)++;
      continue;
    }
    
    // Check for variable or function
    if (isalpha(*p)) {
      char name[12];
      int nameLen = 0;
      
      while (isalpha(*p) || isdigit(*p)) {
        name[nameLen++] = *p++;
      }
      name[nameLen] = '\0';
      
      // Check for constants FIRST (before variable check)
      if (strcmp(name, "PI") == 0) {
        t->type = TOKEN_NUMBER;
        t->numValue = 3.141592653589793;
        (*tokenCount)++;
        continue;
      }
      if (strcmp(name, "E") == 0) {
        t->type = TOKEN_NUMBER;
        t->numValue = 2.718281828459045;
        (*tokenCount)++;
        continue;
      }
      
      // Check if followed by parenthesis (function or array)
      if (*p == '(') {
        // Check if it's a single letter (array access)
        if (nameLen == 1 && name[0] >= 'A' && name[0] <= 'Z') {
          t->type = TOKEN_ARRAY_ACCESS;
          t->varIndex = name[0] - 'A';
        } else {
          t->type = TOKEN_FUNCTION;
        }
        strcpy(t->strValue, name);
      } else if (nameLen == 1 && name[0] >= 'A' && name[0] <= 'Z') {
        // Single letter variable
        t->type = TOKEN_VARIABLE;
        t->varIndex = name[0] - 'A';
        strcpy(t->strValue, name);
      } else {
        // Unknown - treat as function
        t->type = TOKEN_FUNCTION;
        strcpy(t->strValue, name);
      }
      (*tokenCount)++;
      continue;
    }
    
    // Check for operators
    if (*p == '+' || *p == '-' || *p == '*' || *p == '/' || 
        *p == '%' || *p == '^') {
      t->type = TOKEN_OPERATOR;
      t->strValue[0] = *p++;
      t->strValue[1] = '\0';
      (*tokenCount)++;
      continue;
    }
    
    // Check for parentheses
    if (*p == '(') {
      t->type = TOKEN_LPAREN;
      strcpy(t->strValue, "(");
      p++;
      (*tokenCount)++;
      continue;
    }
    
    if (*p == ')') {
      t->type = TOKEN_RPAREN;
      strcpy(t->strValue, ")");
      p++;
      (*tokenCount)++;
      continue;
    }
    
    // Check for comma
    if (*p == ',') {
      t->type = TOKEN_COMMA;
      strcpy(t->strValue, ",");
      p++;
      (*tokenCount)++;
      continue;
    }
    
    // Unknown character, skip
    p++;
  }
}

int getOperatorPrecedence(const char* op) {
  if (strcmp(op, "NEG") == 0 || strcmp(op, "POS") == 0) return 5;  // Highest - unary
  if (strcmp(op, "^") == 0) return 4;
  if (strcmp(op, "*") == 0 || strcmp(op, "/") == 0 || strcmp(op, "%") == 0) return 3;
  if (strcmp(op, "+") == 0 || strcmp(op, "-") == 0) return 2;
  return 0;
}

bool isRightAssociative(const char* op) {
  return strcmp(op, "^") == 0 || strcmp(op, "NEG") == 0 || strcmp(op, "POS") == 0;
}

void infixToPostfix(Token* infix, int infixCount, Token* postfix, int* postfixCount) {
  *postfixCount = 0;
  opStackTop = 0;
  
  for (int i = 0; i < infixCount; i++) {
    Token* t = &infix[i];
    
    switch (t->type) {
      case TOKEN_NUMBER:
      case TOKEN_VARIABLE:
        postfix[(*postfixCount)++] = *t;
        break;
        
      case TOKEN_FUNCTION:
      case TOKEN_ARRAY_ACCESS:
        strcpy(opStack[opStackTop++], t->strValue);
        if (t->type == TOKEN_ARRAY_ACCESS) {
          // Store array index in the string with a marker
          char marker[12];
          sprintf(marker, "ARR%d", t->varIndex);
          strcpy(opStack[opStackTop - 1], marker);
        }
        break;
        
      case TOKEN_OPERATOR:
        while (opStackTop > 0 && 
               strcmp(opStack[opStackTop - 1], "(") != 0) {
          int topPrec = getOperatorPrecedence(opStack[opStackTop - 1]);
          int curPrec = getOperatorPrecedence(t->strValue);
          
          if (topPrec > curPrec || 
              (topPrec == curPrec && !isRightAssociative(t->strValue))) {
            postfix[*postfixCount].type = TOKEN_OPERATOR;
            strcpy(postfix[*postfixCount].strValue, opStack[--opStackTop]);
            (*postfixCount)++;
          } else {
            break;
          }
        }
        strcpy(opStack[opStackTop++], t->strValue);
        break;
        
      case TOKEN_LPAREN:
        strcpy(opStack[opStackTop++], "(");
        break;
        
      case TOKEN_RPAREN:
        while (opStackTop > 0 && strcmp(opStack[opStackTop - 1], "(") != 0) {
          postfix[*postfixCount].type = TOKEN_OPERATOR;
          strcpy(postfix[*postfixCount].strValue, opStack[--opStackTop]);
          (*postfixCount)++;
        }
        if (opStackTop > 0) opStackTop--;  // Pop the '('
        
        // Check if function on stack (but not NEG/POS which are operators)
        if (opStackTop > 0 && isalpha(opStack[opStackTop - 1][0]) &&
            strcmp(opStack[opStackTop - 1], "NEG") != 0 &&
            strcmp(opStack[opStackTop - 1], "POS") != 0) {
          postfix[*postfixCount].type = TOKEN_FUNCTION;
          strcpy(postfix[*postfixCount].strValue, opStack[--opStackTop]);
          (*postfixCount)++;
        }
        break;
        
      case TOKEN_COMMA:
        while (opStackTop > 0 && strcmp(opStack[opStackTop - 1], "(") != 0) {
          postfix[*postfixCount].type = TOKEN_OPERATOR;
          strcpy(postfix[*postfixCount].strValue, opStack[--opStackTop]);
          (*postfixCount)++;
        }
        break;
        
      default:
        break;
    }
  }
  
  // Pop remaining operators
  while (opStackTop > 0) {
    postfix[*postfixCount].type = TOKEN_OPERATOR;
    strcpy(postfix[*postfixCount].strValue, opStack[--opStackTop]);
    (*postfixCount)++;
  }
}

double evaluatePostfix(Token* postfix, int count) {
  valueStackTop = 0;
  
  for (int i = 0; i < count; i++) {
    Token* t = &postfix[i];
    
    switch (t->type) {
      case TOKEN_NUMBER:
        valueStack[valueStackTop++] = t->numValue;
        break;
        
      case TOKEN_VARIABLE:
        valueStack[valueStackTop++] = variables[t->varIndex];
        break;
        
      case TOKEN_OPERATOR:
        // Handle unary operators NEG and POS
        if (strcmp(t->strValue, "NEG") == 0) {
          if (valueStackTop < 1) {
            return NAN;
          }
          double a = valueStack[--valueStackTop];
          valueStack[valueStackTop++] = -a;
        } else if (strcmp(t->strValue, "POS") == 0) {
          // Unary plus is a no-op, value stays the same
          if (valueStackTop < 1) {
            return NAN;
          }
          // Just leave the value on the stack
        } else {
          // Binary operators
          if (valueStackTop < 2) {
            return NAN;
          }
          double b = valueStack[--valueStackTop];
          double a = valueStack[--valueStackTop];
          valueStack[valueStackTop++] = applyOperator(t->strValue, a, b);
        }
        break;
        
      case TOKEN_FUNCTION:
        // Check for array access marker
        if (strncmp(t->strValue, "ARR", 3) == 0) {
          int varIndex = atoi(t->strValue + 3);
          if (valueStackTop < 1) {
            return NAN;
          }
          int arrayIndex = (int)valueStack[--valueStackTop];
          valueStack[valueStackTop++] = getArrayElement(varIndex, arrayIndex);
        } else {
          if (valueStackTop < 1) {
            return NAN;
          }
          double arg = valueStack[--valueStackTop];
          valueStack[valueStackTop++] = applyFunction(t->strValue, arg);
        }
        break;
        
      default:
        break;
    }
  }
  
  if (valueStackTop == 1) {
    return valueStack[0];
  }
  
  return NAN;
}

double applyOperator(const char* op, double a, double b) {
  if (strcmp(op, "+") == 0) return a + b;
  if (strcmp(op, "-") == 0) return a - b;
  if (strcmp(op, "*") == 0) return a * b;
  if (strcmp(op, "/") == 0) {
    if (b == 0) return NAN;
    return a / b;
  }
  if (strcmp(op, "%") == 0) {
    if (b == 0) return NAN;
    return fmod(a, b);
  }
  if (strcmp(op, "^") == 0) {
    if (a == 0 && b == 0) return 1;
    if (a == 0 && b < 0) return NAN;
    
    // Optimize for integer exponents
    if (b == floor(b) && b >= 0 && b <= 100) {
      double result = 1;
      int exp = (int)b;
      while (exp > 0) {
        if (exp & 1) result *= a;
        a *= a;
        exp >>= 1;
      }
      return result;
    }
    return pow(a, b);
  }
  
  return NAN;
}

double applyFunction(const char* func, double arg) {
  // Basic math
  if (strcmp(func, "SQRT") == 0) return sqrt(arg);
  if (strcmp(func, "LN") == 0) return log(arg);
  if (strcmp(func, "LOG") == 0) return log10(arg);
  if (strcmp(func, "EXP") == 0) return exp(arg);
  if (strcmp(func, "ABS") == 0) return fabs(arg);
  if (strcmp(func, "FLOOR") == 0) return floor(arg);
  if (strcmp(func, "CEIL") == 0) return ceil(arg);
  if (strcmp(func, "ROUND") == 0) return round(arg);
  
  // Degree/Radian conversion
  if (strcmp(func, "RAD") == 0) return arg * M_PI / 180.0;
  if (strcmp(func, "DEG") == 0) return arg * 180.0 / M_PI;
  
  // Trigonometric (mode-dependent)
  if (strcmp(func, "SIN") == 0) {
    return sin(degreeMode ? arg * M_PI / 180.0 : arg);
  }
  if (strcmp(func, "COS") == 0) {
    return cos(degreeMode ? arg * M_PI / 180.0 : arg);
  }
  if (strcmp(func, "TAN") == 0) {
    return tan(degreeMode ? arg * M_PI / 180.0 : arg);
  }
  if (strcmp(func, "ASIN") == 0) {
    double result = asin(arg);
    return degreeMode ? result * 180.0 / M_PI : result;
  }
  if (strcmp(func, "ACOS") == 0) {
    double result = acos(arg);
    return degreeMode ? result * 180.0 / M_PI : result;
  }
  if (strcmp(func, "ATAN") == 0) {
    double result = atan(arg);
    return degreeMode ? result * 180.0 / M_PI : result;
  }
  
  // Degree-only trig
  if (strcmp(func, "SIND") == 0) return sin(arg * M_PI / 180.0);
  if (strcmp(func, "COSD") == 0) return cos(arg * M_PI / 180.0);
  if (strcmp(func, "TAND") == 0) return tan(arg * M_PI / 180.0);
  
  // Hyperbolic
  if (strcmp(func, "SINH") == 0) return sinh(arg);
  if (strcmp(func, "COSH") == 0) return cosh(arg);
  if (strcmp(func, "TANH") == 0) return tanh(arg);
  if (strcmp(func, "ASINH") == 0) return asinh(arg);
  if (strcmp(func, "ACOSH") == 0) return acosh(arg);
  if (strcmp(func, "ATANH") == 0) return atanh(arg);
  
  return NAN;
}

// ==================== Utility Functions ====================
void trimWhitespace(char* str) {
  // Trim leading whitespace
  char* start = str;
  while (*start && isspace(*start)) start++;
  
  // Trim trailing whitespace
  char* end = start + strlen(start) - 1;
  while (end > start && isspace(*end)) end--;
  *(end + 1) = '\0';
  
  // Move to beginning if needed
  if (start != str) {
    memmove(str, start, strlen(start) + 1);
  }
}

void toUpperCase(char* str) {
  while (*str) {
    *str = toupper(*str);
    str++;
  }
}

void removeSpaces(char* str) {
  char* dest = str;
  bool inQuotes = false;
  
  while (*str) {
    if (*str == '"') {
      inQuotes = !inQuotes;
      *dest++ = *str++;
    } else if (!inQuotes && *str == ' ') {
      str++;
    } else {
      *dest++ = *str++;
    }
  }
  *dest = '\0';
}

bool startsWith(const char* str, const char* prefix) {
  return strncmp(str, prefix, strlen(prefix)) == 0;
}