CommandStation-EX/StringParser.cpp

#include "StringParser.h"
#include <stdarg.h>

int StringParser::parse(const char * com, int result[], byte maxResults) {
  byte state=1;
  byte parameterCount=0;
  int runningValue=0;
  const char * remainingCmd=com;  // skips the opcode
  bool signNegative=false;
  
  // clear all parameters in case not enough found
  for (int i=0;i<maxResults;i++) result[i]=0;
  
  while(parameterCount<maxResults) {
      char hot=*remainingCmd;
      
       switch (state) {
    
            case 1: // skipping spaces before a param
               if (hot==' ') break;
               if (hot == '\0' || hot=='>') return parameterCount;
               state=2;
               continue;
               
            case 2: // checking sign
               signNegative=false;
               runningValue=0;
               state=3; 
               if (hot!='-') continue; 
               signNegative=true;
               break; 
            case 3: // building a parameter   
               if (hot>='0' && hot<='9') {
                   runningValue=10*runningValue+(hot-'0');
                   break;
               }
               result[parameterCount] = runningValue * (signNegative ?-1:1);
               parameterCount++;
               state=1; 
               continue;
         }   
         remainingCmd++;
      }
      return parameterCount;
}



void StringParser::print( const __FlashStringHelper* input...) {
  va_list args;
  va_start(args, input);
  send(Serial,input,args);
}

void StringParser::send(Stream & stream, const __FlashStringHelper* input...) {
  va_list args;
  va_start(args, input);
  send(stream,input,args);
}

void StringParser::send(Stream & stream,const __FlashStringHelper* format, va_list args) {
    

  // thanks to Jan Turoň  https://arduino.stackexchange.com/questions/56517/formatting-strings-in-arduino-for-output

  char* flash=(char*)format;
  for(int i=0; ; ++i) {
    char c=pgm_read_byte_near(flash+i);
    if (c=='\0') return;
    if(c!='%') { stream.print(c); continue; }
    i++;
    c=pgm_read_byte_near(flash+i);
    switch(c) {
      case '%': stream.print('%'); break;
      case 's': stream.print(va_arg(args, char*)); break;
      case 'd': stream.print(va_arg(args, int), DEC); break;
      case 'b': stream.print(va_arg(args, int), BIN); break;
      case 'o': stream.print(va_arg(args, int), OCT); break;
      case 'x': stream.print(va_arg(args, int), HEX); break;
      case 'f': stream.print(va_arg(args, double), 2); break;
    }
  }
  va_end(args);
}