// https://forum.arduino.cc/t/sscanf-bug-what-i-am-doing-wrong/1046254

# define BLOCKS false      // select blocking (true) or non-blocking (false) code 

char message[32];

uint16_t SLAVE_ID ;
uint16_t COMMAND ;
uint16_t IO_ID ;
uint16_t DATA1 ;
uint16_t DATA2 ;

bool gthering = false;
unsigned long aTime;

unsigned loopCounter;

# if BLOCKS

bool getMessage()
{
    if( Serial.available() > 0 ) {
        aTime = millis();
        int n = Serial.readBytesUntil ('\n', message, sizeof(message)-1);
        message [n] = '\0';

        aTime = millis() - aTime;
        return true ;
    }
    return false;
}

# else // non-blocking version

bool getMessage()
{
    static int index = 0 ;

    if( Serial.available() > 0 )
    {  

// turn on counter when we begin gathering
      if (!loopCounter) loopCounter = 1;

        char c = Serial.read() ;

        if( c == '\n')
        {
            //message[index++] = c ;
            message[index] = 0 ;
            index = 0 ;
            return true ;
        }
        else if( c >= ' ' )   // discard all non printable characters except newline
        {
            message[index++] = c ;
        }
    }
    return false; 
}

# endif

void setup()
{
    Serial.begin( 9600 ) ;
    Serial.print("wake up!  ");
# if BLOCKS
    Serial.println("blocking code version");
# else
    Serial.println("non-blocking code version");
#  endif
}

void loop()
{
    if (loopCounter) loopCounter++;    // we in the middle of gathering a complete line

    if( getMessage() )
    {
        sscanf( message, "%u,%u,%u,%u,%u", 
            &SLAVE_ID, 
            &COMMAND, 
            &IO_ID, 
            &DATA1,
            &DATA2 ) ; // message = 1,2,3,4

        Serial.print("message received ");

# if BLOCKS
      Serial.print(" and getting it blocked for ");
      Serial.print(aTime);
      Serial.println(" milliseconds ");

# else // doesn't block

        Serial.print("and the loop got attention while it was coming in ");
        Serial.print(loopCounter);
        Serial.println(" times.");
# endif
        loopCounter = 0;

        //delay(1000);

        Serial.println( SLAVE_ID ) ;
        Serial.println( COMMAND ) ;
        Serial.println( IO_ID ) ;
        Serial.println( DATA1 ) ;
        Serial.println( DATA2 ) ;

        //processMessage() ;
    }

    //updateServos() ;
    //readInputs() ;
}