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

// --- Configuration ---
#define STACK_SIZE 100
#define MAX_PROGRAM_LINES 50

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

// =================================================================
// NEW: Corrected Global Variables for BASIC
// =================================================================
double variableValues[26]; // A=0, B=1, ..., Z=25
String programLines[MAX_PROGRAM_LINES];
int    lineNumbers[MAX_PROGRAM_LINES];
int    programSize = 0;

// State machine for the interpreter
enum ProgramState { IDLE, RUNNING, AWAIT_INPUT };
ProgramState programState = IDLE;
int pc = 0; // Program Counter
int awaitingInputForVarIndex = -1;

// --- Forward Declarations ---
void newProgram();
void listProgram();
void startProgram();
void executeNextBasicLine();

// --- NEW: Corrected Helper Functions for A-Z Variables ---
bool isVariable(char c) { return c >= 'A' && c <= 'Z'; }
int getVariableIndex(char c) { if (!isVariable(c)) return -1; return c - 'A'; }

// =================================================================
// Stacks & Calculator Class (Unchanged and Correct)
// =================================================================
class CharStack{public:CharStack(int s=STACK_SIZE){maxSize=s;stackArray=new String[maxSize];top=-1;}~CharStack(){delete[] stackArray;}void push(String v){if(!isFull())stackArray[++top]=v;}String pop(){return!isEmpty()?stackArray[top--]:"";}String peek(){return!isEmpty()?stackArray[top]:"";}bool isEmpty(){return top==-1;}bool isFull(){return top==maxSize-1;}private:String*stackArray;int top,maxSize;};
class FloatStack{public:FloatStack(int s=STACK_SIZE){maxSize=s;stackArray=new double[maxSize];top=-1;}~FloatStack(){delete[] stackArray;}void push(double v){if(!isFull())stackArray[++top]=v;}double pop(){return!isEmpty()?stackArray[top--]:NAN;}private:double*stackArray;int top,maxSize;bool isEmpty(){return top==-1;}bool isFull(){return top==maxSize-1;}};
class Calculator{public:String toPostfix(String i);double evaluatePostfix(String p,bool isRad);private:int getPrecedence(String o);bool isFunc(String t);String handleUnary(String i);double power(double b,double e);};
String Calculator::handleUnary(String i){i.replace(" ","");if(i.startsWith("-"))i="0"+i;i.replace("(-","(0-");return i;}
int Calculator::getPrecedence(String o){if(isFunc(o))return 4;if(o=="^")return 3;if(o=="*"||o=="/"||o=="%")return 2;if(o=="+"||o=="-")return 1;return 0;}
bool Calculator::isFunc(String t){const char*f[]={"sin","cos","tan","asin","acos","atan","sind","cosd","tand","sinh","cosh","tanh","asinh","acosh","atanh","ln","log","exp","sqrt"};for(const char*i:f){if(t.equalsIgnoreCase(i))return true;}return false;}
String Calculator::toPostfix(String i){i.toUpperCase();i=handleUnary(i);String p="";CharStack o;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+" ";continue;}if(c=='P'&&i.substring(k,k+2)=="PI"){p+="pi ";k+=2;continue;}if(c=='E'&&(k+1==i.length()||!isAlpha(i.charAt(k+1)))){p+="e ";k++;continue;}if(isVariable(c)){p+=String(c)+" ";k++;continue;}if(isAlpha(c)){String f="";while(k<i.length()&&isAlpha(i.charAt(k)))f+=i.charAt(k++);if(isFunc(f))o.push(f);continue;}if(c=='('){o.push("(");k++;continue;}if(c==')'){while(!o.isEmpty()&&o.peek()!="(")p+=o.pop()+" ";o.pop();if(!o.isEmpty()&&isFunc(o.peek()))p+=o.pop()+" ";k++;continue;}char op=i.charAt(k);if(String("+-*/%^").indexOf(op)!=-1){String o1=String(op);while(!o.isEmpty()&&o.peek()!="("&&((getPrecedence(o1)<getPrecedence(o.peek()))||(getPrecedence(o1)==getPrecedence(o.peek())&&o1!="^")))p+=o.pop()+" ";o.push(o1);k++;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.length()>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.length()==1&&isVariable(t.charAt(0)))v.push(variableValues[getVariableIndex(t.charAt(0))]);else if(isFunc(t)){double val=v.pop(),res=0;if(t.equalsIgnoreCase("sin"))res=r?sin(val):sin(val*PI_CONST/180.0);else if(t.equalsIgnoreCase("cos"))res=r?cos(val):cos(val*PI_CONST/180.0);else if(t.equalsIgnoreCase("tan"))res=r?tan(val):tan(val*PI_CONST/180.0);else if(t.equalsIgnoreCase("asin"))res=r?asin(val):asin(val)*180.0/PI_CONST;else if(t.equalsIgnoreCase("acos"))res=r?acos(val):acos(val)*180.0/PI_CONST;else if(t.equalsIgnoreCase("atan"))res=r?atan(val):atan(val)*180.0/PI_CONST;else if(t.equalsIgnoreCase("sind"))res=sin(val*PI_CONST/180.0);else if(t.equalsIgnoreCase("cosd"))res=cos(val*PI_CONST/180.0);else if(t.equalsIgnoreCase("tand"))res=tan(val*PI_CONST/180.0);else if(t.equalsIgnoreCase("sinh"))res=sinh(val);else if(t.equalsIgnoreCase("cosh"))res=cosh(val);else if(t.equalsIgnoreCase("tanh"))res=tanh(val);else if(t.equalsIgnoreCase("asinh"))res=log(val+sqrt(val*val+1.0));else if(t.equalsIgnoreCase("acosh"))res=log(val+sqrt(val*val-1.0));else if(t.equalsIgnoreCase("atanh"))res=0.5*log((1.0+val)/(1.0-val));else if(t.equalsIgnoreCase("ln"))res=log(val);else if(t.equalsIgnoreCase("log"))res=log10(val);else if(t.equalsIgnoreCase("exp"))res=exp(val);else if(t.equalsIgnoreCase("sqrt"))res=sqrt(val);v.push(res);}else{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(v1/v2);else if(t=="%")v.push(fmod(v1,v2));else if(t=="^")v.push(power(v1,v2));}}return v.pop();}
double Calculator::power(double b,double e){if(e==0)return 1;if(b==0)return 0;if(floor(e)==e&&e>0){long i_e=(long)e;double r=1.0;while(i_e>0){if(i_e%2==1)r*=b;b*=b;i_e/=2;}return r;}return pow(b,e);}

Calculator calculator;
bool isRadiansMode = true;

// =================================================================
// BASIC Interpreter Implementation - REWRITTEN AND CORRECTED
// =================================================================

int findLineIndex(int lineNum){for(int i=0;i<programSize;i++){if(lineNumbers[i]==lineNum)return i;}return-1;}
void storeLine(String line){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){programSize--;for(int i=existingIndex;i<programSize;i++){lineNumbers[i]=lineNumbers[i+1];programLines[i]=programLines[i+1];}}else{programLines[existingIndex]=code;}}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++;}}
void newProgram(){programSize=0;for(int i=0;i<26;i++)variableValues[i]=0;programState=IDLE;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; } }

void executeNextBasicLine() {
    if (pc >= programSize) { programState = IDLE; return; } // End of program
    
    String line = programLines[pc];
    String command, args;
    int firstSpace = line.indexOf(' ');
    if(firstSpace!=-1){command=line.substring(0,firstSpace);args=line.substring(firstSpace+1);}else{command=line;args="";}
    command.toUpperCase(); args.trim();

    bool jumped = false;
    if (command == "REM") {}
    else if (command == "END") { programState = IDLE; }
    else if (command == "PRINT") {
        if(args.startsWith("\"")&&args.endsWith("\"")){Serial.println(args.substring(1,args.length()-1));}
        else{Serial.println(calculator.evaluatePostfix(calculator.toPostfix(args), isRadiansMode), 15);}
    }
    else if (command == "INPUT") {
        args.toUpperCase();
        awaitingInputForVarIndex = getVariableIndex(args.charAt(0));
        if (awaitingInputForVarIndex != -1) {
            programState = AWAIT_INPUT;
            Serial.print("? ");
        }
    }
    else if (command == "GOTO") {
        int newPc = findLineIndex(args.toInt());
        if (newPc != -1) { pc = newPc; jumped = true; }
    }
    else if (command == "IF") {
        String uArgs=args;uArgs.toUpperCase();int thenPos=uArgs.indexOf("THEN");
        if(thenPos!=-1){
            String cStr=args.substring(0,thenPos);int targetLine=args.substring(thenPos+4).toInt();
            String op;int opPos=-1;
            if((opPos=cStr.indexOf("<>"))!=-1)op="<>";else if((opPos=cStr.indexOf("!="))!=-1)op="!=";else if((opPos=cStr.indexOf("<="))!=-1)op="<=";else if((opPos=cStr.indexOf(">="))!=-1)op=">=";else if((opPos=cStr.indexOf('<'))!=-1)op="<";else if((opPos=cStr.indexOf('>'))!=-1)op=">";else if((opPos=cStr.indexOf('='))!=-1)op="=";
            if(opPos!=-1){
                double lhs=calculator.evaluatePostfix(calculator.toPostfix(cStr.substring(0,opPos)),isRadiansMode);
                double rhs=calculator.evaluatePostfix(calculator.toPostfix(cStr.substring(opPos+op.length())),isRadiansMode);
                bool conditionMet=false;
                if(op=="<")conditionMet=lhs<rhs;else if(op==">")conditionMet=lhs>rhs;else if(op=="=")conditionMet=lhs==rhs;else if(op=="<>"||op=="!=")conditionMet=lhs!=rhs;else if(op=="<=")conditionMet=lhs<=rhs;else if(op==">=")conditionMet=lhs>=rhs;
                if(conditionMet){int newPc=findLineIndex(targetLine);if(newPc!=-1){pc=newPc;jumped=true;}}
            }
        }
    }
    else { // LET
        String uLine=line;uLine.toUpperCase();
        if(uLine.startsWith("LET ")){line=line.substring(4);uLine=uLine.substring(4);}
        int eqPos=uLine.indexOf('=');
        if(eqPos!=-1){
            int varIdx=getVariableIndex(uLine.charAt(0));
            if(varIdx!=-1){variableValues[varIdx]=calculator.evaluatePostfix(calculator.toPostfix(line.substring(eqPos+1)),isRadiansMode);}
        }
    }
    if (!jumped && programState == RUNNING) { pc++; }
}

// =================================================================
// Main Setup and Loop
// =================================================================

void setup() {
    Serial.begin(9600);
    while (!Serial) { ; }
    newProgram();
    Serial.println("\n--- Arduino BASIC Scientific Calculator (v2) ---");
    Serial.println("BASIC: RUN, LIST, NEW, LET, PRINT, INPUT, GOTO, IF/THEN, REM, END");
    Serial.println("Calculator: sin(pi/2) or a=5 (A-Z variables)");
    Serial.println("-------------------------------------");
    Serial.print("Select angle mode - (r)adians or (d)egrees: ");
    while (Serial.available() == 0) {}
    char mode = Serial.read(); while (Serial.available() > 0) Serial.read();
    if (mode == 'd' || mode == 'D'){isRadiansMode=false;Serial.println("Degree Mode.");}
    else{isRadiansMode=true;Serial.println("Radian Mode.");}
    Serial.println("-------------------------------------");
    Serial.print("> ");
}

void loop() {
    // State machine logic
    if (programState == RUNNING) {
        executeNextBasicLine();
    }
    else if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');
        input.trim();
        if (input.length() > 0) {
            if (programState == AWAIT_INPUT) {
                if (awaitingInputForVarIndex != -1) {
                    variableValues[awaitingInputForVarIndex] = input.toDouble();
                }
                programState = RUNNING; // Resume program
                pc++; // Move to next line after input
                executeNextBasicLine(); // Execute next line immediately
            }
            else { // IDLE state
                String command = input;
                int spacePos = input.indexOf(' ');
                if (spacePos != -1) command = input.substring(0, spacePos);
                
                if (isDigit(input.charAt(0))) { storeLine(input); }
                else if (command.equalsIgnoreCase("RUN")) { startProgram(); }
                else if (command.equalsIgnoreCase("LIST")) { listProgram(); }
                else if (command.equalsIgnoreCase("NEW")) { newProgram(); }
                else {
                    input.toUpperCase();
                    int eqPos=input.indexOf('=');
                    if (eqPos>0&&isVariable(input.charAt(0))){
                        int varIdx=getVariableIndex(input.charAt(0));
                        if(varIdx!=-1){
                            double res=calculator.evaluatePostfix(calculator.toPostfix(input.substring(eqPos+1)),isRadiansMode);
                            variableValues[varIdx]=res;
                            Serial.print((char)('A'+varIdx)); Serial.print(" = "); Serial.println(res,15);
                        }
                    } else {
                        Serial.println(calculator.evaluatePostfix(calculator.toPostfix(input),isRadiansMode),15);
                    }
                }
            }
        }
        if (programState == IDLE) Serial.print("> ");
    }
}