#define TERM '\n' 

#define SBUFFERSIZE 64   // The length of the commandbuffer  (default: 64)

char terminator = '\n';           // Character indicating end of command (default: ';')
char separator = ' ';				// Character indicating end of argument (default: ',')

char inBuffer[SBUFFERSIZE]; // Buffer that holds the data
uint8_t bufferIndex = 0;

bool msgReady = false;

void clearBuffer(){
  bufferIndex = 0;
  msgReady = false;
}
/**
 * Feeds serial data in CmdMessenger
 */
void feedinSerialData()
{
	while (!msgReady && Serial.available())
	{
		// The Stream class has a readBytes() function that reads many bytes at once. On Teensy 2.0 and 3.0, readBytes() is optimized. 
		// Benchmarks about the incredible difference it makes: http://www.pjrc.com/teensy/benchmark_usb_serial_receive.html

    if(bufferIndex == SBUFFERSIZE){
      bufferIndex--;
      inBuffer[bufferIndex] = '\0';
      Serial.print("! ");
      Serial.println(inBuffer);
      clearBuffer();
    }else{
      char inChar = Serial.read();

      if(inChar != terminator){
        inBuffer[bufferIndex] = inChar;
        bufferIndex++;
      }else{
        inBuffer[bufferIndex] = '\0';
        msgReady=true;
        return;
      }
    }
	}
}

void processSerial(){

  Serial.print(F("Received ("));
  Serial.print(bufferIndex);
  Serial.print(F(") "));

  Serial.println();

  clearBuffer();

}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);


  Serial.println(F("Start"));
}

void loop() {
  // put your main code here, to run repeatedly:
  feedinSerialData();
  if(msgReady) processSerial();
}