From 8504a809ddb9b191056da7036d2a3bb9bd5040be Mon Sep 17 00:00:00 2001
From: "Jim Leonard (Xuth)" <jim@xuth.net>
Date: Tue, 24 Mar 2015 12:13:41 +0100
Subject: [PATCH] Fix findUntil in Stream library

PR #2696 without timeout-related changes
---
 cores/arduino/Stream.cpp | 102 +++++++++++++++++++++++++++------------
 cores/arduino/Stream.h   |  14 ++++++
 2 files changed, 86 insertions(+), 30 deletions(-)

diff --git a/cores/arduino/Stream.cpp b/cores/arduino/Stream.cpp
index 9c581be..14f972a 100644
--- a/cores/arduino/Stream.cpp
+++ b/cores/arduino/Stream.cpp
@@ -18,6 +18,8 @@
 
  Created July 2011
  parsing functions based on TextFinder library by Michael Margolis
+
+ findMulti/findUntil routines written by Jim Leonard/Xuth
  */
 
 #include "Arduino.h"
@@ -75,7 +77,7 @@ void Stream::setTimeout(unsigned long timeout)  // sets the maximum number of mi
  // find returns true if the target string is found
 bool  Stream::find(char *target)
 {
-  return findUntil(target, (char*)"");
+  return findUntil(target, strlen(target), NULL, 0);
 }
 
 // reads data from the stream until the target string of given length is found
@@ -96,32 +98,13 @@ bool  Stream::findUntil(char *target, char *terminator)
 // returns true if target string is found, false if terminated or timed out
 bool Stream::findUntil(char *target, size_t targetLen, char *terminator, size_t termLen)
 {
-  size_t index = 0;  // maximum target string length is 64k bytes!
-  size_t termIndex = 0;
-  int c;
-  
-  if( *target == 0)
-    return true;   // return true if target is a null string
-  while( (c = timedRead()) > 0){
-    
-    if(c != target[index])
-      index = 0; // reset index if any char does not match
-    
-    if( c == target[index]){
-      //////Serial.print("found "); Serial.write(c); Serial.print("index now"); Serial.println(index+1);
-      if(++index >= targetLen){ // return true if all chars in the target match
-        return true;
-      }
-    }
-    
-    if(termLen > 0 && c == terminator[termIndex]){
-      if(++termIndex >= termLen)
-        return false;       // return false if terminate string found before target string
-    }
-    else
-      termIndex = 0;
+  if (terminator == NULL) {
+    MultiTarget t[1] = {{target, targetLen, 0}};
+    return findMulti(t, 1) == 0 ? true : false;
+  } else {
+    MultiTarget t[2] = {{target, targetLen, 0}, {terminator, termLen, 0}};
+    return findMulti(t, 2) == 0 ? true : false;
   }
-  return false;
 }
 
 
@@ -137,7 +120,7 @@ long Stream::parseInt()
 // this allows format characters (typically commas) in values to be ignored
 long Stream::parseInt(char skipChar)
 {
-  boolean isNegative = false;
+  bool isNegative = false;
   long value = 0;
   int c;
 
@@ -173,10 +156,10 @@ float Stream::parseFloat()
 // as above but the given skipChar is ignored
 // this allows format characters (typically commas) in values to be ignored
 float Stream::parseFloat(char skipChar){
-  boolean isNegative = false;
-  boolean isFraction = false;
+  bool isNegative = false;
+  bool isFraction = false;
   long value = 0;
-  int c;
+  char c;
   float fraction = 1.0;
 
   c = peekNextDigit();
@@ -268,3 +251,62 @@ String Stream::readStringUntil(char terminator)
   return ret;
 }
 
+int Stream::findMulti( struct Stream::MultiTarget *targets, int tCount) {
+  // any zero length target string automatically matches and would make
+  // a mess of the rest of the algorithm.
+  for (struct MultiTarget *t = targets; t < targets+tCount; ++t)
+    if (t->len <= 0)
+      return t - targets;
+
+  while(1) {
+    int c = timedRead();
+    if (c < 0)
+      return -1;
+
+    for (struct MultiTarget *t = targets; t < targets+tCount; ++t) {
+      // the simple case is if we match, deal with that first.
+      if (c == t->str[t->index])
+	if (++t->index == t->len)
+	  return t - targets;
+	else
+	  continue;
+
+      // if not we need to walk back and see if we could have matched further
+      // down the stream (ie '1112' doesn't match the first position in '11112'
+      // but it will match the second position so we can't just reset the current
+      // index to 0 when we find a mismatch.
+      if (t->index == 0)
+	continue;
+
+      int origIndex = t->index;
+      do {
+	--t->index;
+	// first check if current char works against the new current index
+	if (c != t->str[t->index])
+	  continue;
+
+	// if it's the only char then we're good, nothing more to check
+	if (t->index == 0) {
+	  t->index++;
+	  break;
+	}
+
+	// otherwise we need to check the rest of the found string
+	int diff = origIndex - t->index;
+	int i;
+	for (i = 0; i < t->index; ++i)
+	  if (t->str[i] != t->str[i + diff])
+	    break;
+	// if we successfully got through the previous loop then our current
+	// index is good.
+	if (i == t->index) {
+	  t->index++;
+	  break;
+	}
+	// otherwise we just try the next index
+      } while (t->index);
+    }
+  }
+  // unreachable
+  return -1;
+}
\ No newline at end of file
diff --git a/cores/arduino/Stream.h b/cores/arduino/Stream.h
index 5cf5ddf..d0b1f32 100644
--- a/cores/arduino/Stream.h
+++ b/cores/arduino/Stream.h
@@ -97,6 +97,20 @@ class Stream : public Print
   // this allows format characters (typically commas) in values to be ignored
 
   float parseFloat(char skipChar);  // as above but the given skipChar is ignored
+
+  public:
+  struct MultiTarget {
+    const char *str;  // string you're searching for
+    size_t len;       // length of string you're searching for
+    size_t index;     // index used by the search routine.
+  };
+
+  // This allows you to search for an arbitrary number of strings.
+  // Returns index of the target that is found first or -1 if timeout occurs.
+  int findMulti(struct MultiTarget *targets, int tCount);
+
+
 };
 
+
 #endif