#include <Arduino.h>
#include <vector>

typedef struct {
    String command;
    std::vector<String> params;
} GCommand;

class ZCodeParser // Variant of GCode, but made by Zhen. Lol 
{
private:
    static std::vector<String> splitString(const String &input, char delimiter);
public:
    static std::vector<GCommand> ParseString(const String inString);
};

std::vector<String> ZCodeParser::splitString(const String &input, char delimiter) {
    std::vector<String> result;
    int startIndex = 0;
    int endIndex;

    while (true) {
        endIndex = input.indexOf(delimiter, startIndex);
        if (endIndex == -1) {
            result.push_back(input.substring(startIndex)); // Get the rest of the string
            break;
        }
        result.push_back(input.substring(startIndex, endIndex));
        startIndex = endIndex + 1;
    }

    return result;
}

std::vector<GCommand> ZCodeParser::ParseString(const String inString){
    // G0  = move, no block
    // G1  = move, block
    // M17 = Enable Stepper
    // M18 = Disable Stepper
    // ;   = comments
    // \n  = new command

    std::vector<String> stringCommands = ZCodeParser::splitString(inString, '\n');
    
    // Remove comments
    for (size_t i = 0; i < stringCommands.size(); i++)
    {
        int commentIndex = stringCommands[i].indexOf(';');
        if (commentIndex == -1) continue; // Continue if no comments

        stringCommands[i] = stringCommands[i].substring(0, commentIndex);
    }

    // Extract function name and params
    std::vector<GCommand> commnads;
    for (size_t i = 0; i < stringCommands.size(); i++)
    {
        std::vector<String> seperatedStrings = ZCodeParser::splitString(stringCommands[i], ' ');
        GCommand commnad;
        commnad.command = seperatedStrings[0];
        stringCommands.erase(stringCommands.begin());
        commnad.params = stringCommands;
    }

    Serial.println("Parsed: ");
    for (size_t i = 0; i < commnads.size(); i++)
    {
        Serial.print(commnads[i].command);
        Serial.print(", ");
    }
    Serial.println("\n");

    // return ;   
}