project_alpha/lib/SerialCmd/SerialCmd.cpp

#include "SerialCmd.h"
#include <cstring>
#include <cstdlib>

// --- CmdArgs ---

void CmdArgs::parse(char* line) {
    _command = nullptr;
    _rawCount = 0;
    _posCount = 0;

    // In-place tokenization with quoted string support
    bool inToken = false;
    bool inQuote = false;
    char* tokenStart = nullptr;

    for (char* p = line; ; ++p) {
        char c = *p;

        if (c == '\0') {
            if (inToken && _rawCount < SERIAL_CMD_MAX_ARGS) {
                _tokens[_rawCount++] = tokenStart;
            }
            break;
        }

        if (inQuote) {
            if (c == '"') {
                *p = '\0';
                inQuote = false;
                inToken = false;
                if (_rawCount < SERIAL_CMD_MAX_ARGS) {
                    _tokens[_rawCount++] = tokenStart;
                }
            }
            continue;
        }

        if (c == '"') {
            if (inToken && _rawCount < SERIAL_CMD_MAX_ARGS) {
                *p = '\0';
                _tokens[_rawCount++] = tokenStart;
            }
            inQuote = true;
            inToken = true;
            tokenStart = p + 1;
            continue;
        }

        if (c == ' ' || c == '\t') {
            if (inToken) {
                *p = '\0';
                if (_rawCount < SERIAL_CMD_MAX_ARGS) {
                    _tokens[_rawCount++] = tokenStart;
                }
                inToken = false;
            }
        } else if (!inToken) {
            inToken = true;
            tokenStart = p;
        }
    }

    // First token is the command name
    if (_rawCount > 0) {
        _command = _tokens[0];
    }

    // Classify remaining tokens: flags vs positional
    for (int i = 1; i < _rawCount; ++i) {
        if (_tokens[i][0] == '-') {
            // It's a flag — skip for positional list
        } else {
            if (_posCount < SERIAL_CMD_MAX_ARGS) {
                _positional[_posCount++] = _tokens[i];
            }
        }
    }
}

const char* CmdArgs::arg(int index) const {
    if (index < 0 || index >= _posCount) return nullptr;
    return _positional[index];
}

int CmdArgs::intArg(int index, int fallback) const {
    const char* v = arg(index);
    return v ? atoi(v) : fallback;
}

float CmdArgs::floatArg(int index, float fallback) const {
    const char* v = arg(index);
    return v ? atof(v) : fallback;
}

bool CmdArgs::has(const char* flag) const {
    for (int i = 1; i < _rawCount; ++i) {
        if (strcmp(_tokens[i], flag) == 0) return true;
    }
    return false;
}

const char* CmdArgs::raw(int index) const {
    if (index < 0 || index >= _rawCount) return nullptr;
    return _tokens[index];
}

// --- SerialCmd ---

SerialCmd::SerialCmd(Stream& stream) : _stream(stream) {
    _line[0] = '\0';
}

bool SerialCmd::on(const char* name, Handler handler) {
    return on(name, nullptr, handler);
}

bool SerialCmd::on(const char* name, const char* usage, Handler handler) {
    if (_commandCount >= SERIAL_CMD_MAX_COMMANDS) return false;
    _commands[_commandCount++] = { name, usage, handler };
    return true;
}

void SerialCmd::onDefault(Handler handler) {
    _defaultHandler = handler;
}

bool SerialCmd::poll() {
    while (_stream.available()) {
        feedChar(_stream.read());
        if (_lineReady) {
            dispatch();
            return true;
        }
    }
    return false;
}

void SerialCmd::wait() {
    _lineReady = false;
    while (!_lineReady) {
        if (_stream.available()) {
            feedChar(_stream.read());
        } else {
            yield();
        }
    }
    dispatch();
}

void SerialCmd::feedChar(char c) {
    // Handle \r\n sequences — ignore \n right after \r
    if (c == '\n' && _prevCR) {
        _prevCR = false;
        return;
    }
    _prevCR = (c == '\r');

    // End of line
    if (c == '\r' || c == '\n') {
        if (_echo) _stream.println();
        _line[_linePos] = '\0';
        if (_linePos > 0) {
            _lineReady = true;
        }
        return;
    }

    // Backspace (BS or DEL)
    if (c == '\b' || c == 127) {
        if (_linePos > 0) {
            _linePos--;
            if (_echo) _stream.print("\b \b");
        }
        return;
    }

    // Accumulate printable characters
    if (_linePos < SERIAL_CMD_MAX_LINE - 1) {
        _line[_linePos++] = c;
        if (_echo) _stream.print(c);
    }
}

void SerialCmd::dispatch() {
    _lineReady = false;

    CmdArgs args;
    args.parse(_line);

    if (!args.command()) {
        _linePos = 0;
        return;
    }

    // Built-in /sync — checked before user commands
    if (strcmp(args.command(), "/sync") == 0) {
        sendSyncList();
        _linePos = 0;
        return;
    }

    for (int i = 0; i < _commandCount; ++i) {
        if (strcmp(_commands[i].name, args.command()) == 0) {
            _commands[i].handler(args);
            _linePos = 0;
            return;
        }
    }

    if (_defaultHandler) {
        _defaultHandler(args);
    }

    _linePos = 0;
}

void SerialCmd::sendSyncList() {
    _stream.println("#sync-begin");
    for (int i = 0; i < _commandCount; ++i) {
        _stream.print(_commands[i].name);
        if (_commands[i].usage) {
            _stream.print(' ');
            _stream.print(_commands[i].usage);
        }
        _stream.println();
    }
    _stream.println("#sync-end");
}

bool SerialCmd::sync(unsigned long timeout_ms) {
    sendSyncList();

    unsigned long deadline = millis() + timeout_ms;
    char buf[SERIAL_CMD_MAX_LINE];
    int pos = 0;
    bool prevCR = false;

    while (millis() < deadline) {
        if (!_stream.available()) {
            yield();
            continue;
        }

        char c = _stream.read();

        // Handle \r\n sequences
        if (c == '\n' && prevCR) {
            prevCR = false;
            continue;
        }
        prevCR = (c == '\r');

        if (c == '\r' || c == '\n') {
            buf[pos] = '\0';
            if (pos > 0 && strcmp(buf, "#acknowledge-sync") == 0) {
                return true;
            }
            pos = 0;
            continue;
        }

        if (pos < SERIAL_CMD_MAX_LINE - 1) {
            buf[pos++] = c;
        }
    }

    return false;
}