#include "mbed.h"
#include "CommandDispatcher.h"
    
Serial pc(USBTX, USBRX);

void echoCommand(unsigned int argc, char* argv[], char* result);
    
int main() {
    CommandDispatcher disp = CommandDispatcher();
    char buffer[50];
    char result[50];
    int i=0;
    
    // register a command
    disp.addCommand("echo", echoCommand);
    pc.printf("Example Command Dispatcher\n\n");

    while(true) {
        // get a complete line from serial
        buffer[i++] = pc.getc();
        if (buffer[i-1]=='\n') {
            buffer[i-1]='\0';
            i=0;
        
            // send it to the dispatcher and print result
            if (disp.executeCommand(buffer, result)) {
                pc.printf("%s\n", result); 
            } else {
                pc.printf("Command not found.\n");  
            }
        }
    }
}

// the actual function called
void echoCommand(unsigned int argc, char* argv[], char* result) {
    int i;
    sprintf(result, "");
    for (i=1; i<argc; i++) {
        sprintf(result, "%s %s", result, argv[i]);
    }
    sprintf(result, "%s\n", result);
}