7 files changed, 438 insertions, 109 deletions

diff --git a/cores/arduino/HardwareSerial.cpp b/cores/arduino/HardwareSerial.cpp
index db6b149..d6be218 100644
--- a/cores/arduino/HardwareSerial.cpp
+++ b/cores/arduino/HardwareSerial.cpp
@@ -352,12 +352,13 @@ void HardwareSerial::flush()
     ;
 }
 
-void HardwareSerial::write(uint8_t c)
+size_t HardwareSerial::write(uint8_t c)
 {
   int i = (_tx_buffer->head + 1) % SERIAL_BUFFER_SIZE;
 	
   // If the output buffer is full, there's nothing for it other than to 
   // wait for the interrupt handler to empty it a bit
+  // ???: return 0 here instead?
   while (i == _tx_buffer->tail)
     ;
 	
@@ -365,6 +366,8 @@ void HardwareSerial::write(uint8_t c)
   _tx_buffer->head = i;
 	
   sbi(*_ucsrb, _udrie);
+  
+  return 1;
 }
 
 // Preinstantiate Objects //////////////////////////////////////////////////////
diff --git a/cores/arduino/HardwareSerial.h b/cores/arduino/HardwareSerial.h
index eefdcbe..1895f08 100644
--- a/cores/arduino/HardwareSerial.h
+++ b/cores/arduino/HardwareSerial.h
@@ -55,7 +55,7 @@ class HardwareSerial : public Stream
     virtual int peek(void);
     virtual int read(void);
     virtual void flush(void);
-    virtual void write(uint8_t);
+    virtual size_t write(uint8_t);
     using Print::write; // pull in write(str) and write(buf, size) from Print
 };
 
diff --git a/cores/arduino/Print.cpp b/cores/arduino/Print.cpp
index 06ac52a..8190d4f 100755
--- a/cores/arduino/Print.cpp
+++ b/cores/arduino/Print.cpp
@@ -30,167 +30,190 @@
 // Public Methods //////////////////////////////////////////////////////////////
 
 /* default implementation: may be overridden */
-void Print::write(const char *str)
+size_t Print::write(const char *str)
 {
-  while (*str)
-    write(*str++);
+  size_t n = 0;
+  while (*str) {
+    n += write(*str++);
+  }
+  return n;
 }
 
 /* default implementation: may be overridden */
-void Print::write(const uint8_t *buffer, size_t size)
+size_t Print::write(const uint8_t *buffer, size_t size)
 {
-  while (size--)
-    write(*buffer++);
+  size_t n = 0;
+  while (size--) {
+    n += write(*buffer++);
+  }
+  return n;
 }
 
-void Print::print(const __FlashStringHelper *ifsh)
+size_t Print::print(const __FlashStringHelper *ifsh)
 {
   const prog_char *p = (const prog_char *)ifsh;
+  size_t n = 0;
   while (1) {
     unsigned char c = pgm_read_byte(p++);
-    if (c == 0) return;
-    write(c);
+    if (c == 0) break;
+    n += write(c);
   }
+  return n;
 }
 
-void Print::print(const String &s)
+size_t Print::print(const String &s)
 {
+  size_t n = 0;
   for (int i = 0; i < s.length(); i++) {
-    write(s[i]);
+    n += write(s[i]);
   }
+  return n;
 }
 
-void Print::print(const char str[])
+size_t Print::print(const char str[])
 {
-  write(str);
+  return write(str);
 }
 
-void Print::print(char c)
+size_t Print::print(char c)
 {
-  write(c);
+  return write(c);
 }
 
-void Print::print(unsigned char b, int base)
+size_t Print::print(unsigned char b, int base)
 {
-  print((unsigned long) b, base);
+  return print((unsigned long) b, base);
 }
 
-void Print::print(int n, int base)
+size_t Print::print(int n, int base)
 {
-  print((long) n, base);
+  return print((long) n, base);
 }
 
-void Print::print(unsigned int n, int base)
+size_t Print::print(unsigned int n, int base)
 {
-  print((unsigned long) n, base);
+  return print((unsigned long) n, base);
 }
 
-void Print::print(long n, int base)
+size_t Print::print(long n, int base)
 {
   if (base == 0) {
-    write(n);
+    return write(n);
   } else if (base == 10) {
     if (n < 0) {
-      print('-');
+      int t = print('-');
       n = -n;
+      return printNumber(n, 10) + t;
     }
-    printNumber(n, 10);
+    return printNumber(n, 10);
   } else {
-    printNumber(n, base);
+    return printNumber(n, base);
   }
 }
 
-void Print::print(unsigned long n, int base)
+size_t Print::print(unsigned long n, int base)
 {
-  if (base == 0) write(n);
-  else printNumber(n, base);
+  if (base == 0) return write(n);
+  else return printNumber(n, base);
 }
 
-void Print::print(double n, int digits)
+size_t Print::print(double n, int digits)
 {
-  printFloat(n, digits);
+  return printFloat(n, digits);
 }
 
-void Print::println(const __FlashStringHelper *ifsh)
+size_t Print::println(const __FlashStringHelper *ifsh)
 {
-  print(ifsh);
-  println();
+  size_t n = print(ifsh);
+  n += println();
+  return n;
 }
 
-void Print::print(const Printable& x)
+size_t Print::print(const Printable& x)
 {
-  x.printTo(*this);
+  return x.printTo(*this);
 }
 
-void Print::println(void)
+size_t Print::println(void)
 {
-  print('\r');
-  print('\n');
+  size_t n = print('\r');
+  n += print('\n');
+  return n;
 }
 
-void Print::println(const String &s)
+size_t Print::println(const String &s)
 {
-  print(s);
-  println();
+  size_t n = print(s);
+  n += println();
+  return n;
 }
 
-void Print::println(const char c[])
+size_t Print::println(const char c[])
 {
-  print(c);
-  println();
+  size_t n = print(c);
+  n += println();
+  return n;
 }
 
-void Print::println(char c)
+size_t Print::println(char c)
 {
-  print(c);
-  println();
+  size_t n = print(c);
+  n += println();
+  return n;
 }
 
-void Print::println(unsigned char b, int base)
+size_t Print::println(unsigned char b, int base)
 {
-  print(b, base);
-  println();
+  size_t n = print(b, base);
+  n += println();
+  return n;
 }
 
-void Print::println(int n, int base)
+size_t Print::println(int num, int base)
 {
-  print(n, base);
-  println();
+  size_t n = print(num, base);
+  n += println();
+  return n;
 }
 
-void Print::println(unsigned int n, int base)
+size_t Print::println(unsigned int num, int base)
 {
-  print(n, base);
-  println();
+  size_t n = print(num, base);
+  n += println();
+  return n;
 }
 
-void Print::println(long n, int base)
+size_t Print::println(long num, int base)
 {
-  print(n, base);
-  println();
+  size_t n = print(num, base);
+  n += println();
+  return n;
 }
 
-void Print::println(unsigned long n, int base)
+size_t Print::println(unsigned long num, int base)
 {
-  print(n, base);
-  println();
+  size_t n = print(num, base);
+  n += println();
+  return n;
 }
 
-void Print::println(double n, int digits)
+size_t Print::println(double num, int digits)
 {
-  print(n, digits);
-  println();
+  size_t n = print(num, digits);
+  n += println();
+  return n;
 }
 
-void Print::println(const Printable& x)
+size_t Print::println(const Printable& x)
 {
-  print(x);
-  println();
+  size_t n = print(x);
+  n += println();
+  return n;
 }
 
 // Private Methods /////////////////////////////////////////////////////////////
 
-void Print::printNumber(unsigned long n, uint8_t base) {
+size_t Print::printNumber(unsigned long n, uint8_t base) {
   char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte.
   char *str = &buf[sizeof(buf) - 1];
 
@@ -206,15 +229,17 @@ void Print::printNumber(unsigned long n, uint8_t base) {
     *--str = c < 10 ? c + '0' : c + 'A' - 10;
   } while(n);
 
-  write(str);
+  return write(str);
 }
 
-void Print::printFloat(double number, uint8_t digits) 
+size_t Print::printFloat(double number, uint8_t digits) 
 { 
+  size_t n = 0;
+  
   // Handle negative numbers
   if (number < 0.0)
   {
-     print('-');
+     n += print('-');
      number = -number;
   }
 
@@ -228,18 +253,21 @@ void Print::printFloat(double number, uint8_t digits)
   // Extract the integer part of the number and print it
   unsigned long int_part = (unsigned long)number;
   double remainder = number - (double)int_part;
-  print(int_part);
+  n += print(int_part);
 
   // Print the decimal point, but only if there are digits beyond
-  if (digits > 0)
-    print("."); 
+  if (digits > 0) {
+    n += print("."); 
+  }
 
   // Extract digits from the remainder one at a time
   while (digits-- > 0)
   {
     remainder *= 10.0;
     int toPrint = int(remainder);
-    print(toPrint);
+    n += print(toPrint);
     remainder -= toPrint; 
   } 
+  
+  return n;
 }
diff --git a/cores/arduino/Print.h b/cores/arduino/Print.h
index bf10b14..fce302e 100755
--- a/cores/arduino/Print.h
+++ b/cores/arduino/Print.h
@@ -34,37 +34,45 @@
 class Print
 {
   private:
-    void printNumber(unsigned long, uint8_t);
-    void printFloat(double, uint8_t);
+    int write_error;
+    size_t printNumber(unsigned long, uint8_t);
+    size_t printFloat(double, uint8_t);
+  protected:
+    void setWriteError(int err = 1) { write_error = err; }
   public:
-    virtual void write(uint8_t) = 0;
-    virtual void write(const char *str);
-    virtual void write(const uint8_t *buffer, size_t size);
+    Print() : write_error(0) {}
+  
+    int writeError() { return write_error; }
+    void clearWriteError() { setWriteError(0); }
+  
+    virtual size_t write(uint8_t) = 0;
+    virtual size_t write(const char *str);
+    virtual size_t write(const uint8_t *buffer, size_t size);
     
-    void print(const __FlashStringHelper *);
-    void print(const String &);
-    void print(const char[]);
-    void print(char);
-    void print(unsigned char, int = DEC);
-    void print(int, int = DEC);
-    void print(unsigned int, int = DEC);
-    void print(long, int = DEC);
-    void print(unsigned long, int = DEC);
-    void print(double, int = 2);
-    void print(const Printable&);
+    size_t print(const __FlashStringHelper *);
+    size_t print(const String &);
+    size_t print(const char[]);
+    size_t print(char);
+    size_t print(unsigned char, int = DEC);
+    size_t print(int, int = DEC);
+    size_t print(unsigned int, int = DEC);
+    size_t print(long, int = DEC);
+    size_t print(unsigned long, int = DEC);
+    size_t print(double, int = 2);
+    size_t print(const Printable&);
 
-    void println(const __FlashStringHelper *);
-    void println(const String &s);
-    void println(const char[]);
-    void println(char);
-    void println(unsigned char, int = DEC);
-    void println(int, int = DEC);
-    void println(unsigned int, int = DEC);
-    void println(long, int = DEC);
-    void println(unsigned long, int = DEC);
-    void println(double, int = 2);
-    void println(const Printable&);
-    void println(void);
+    size_t println(const __FlashStringHelper *);
+    size_t println(const String &s);
+    size_t println(const char[]);
+    size_t println(char);
+    size_t println(unsigned char, int = DEC);
+    size_t println(int, int = DEC);
+    size_t println(unsigned int, int = DEC);
+    size_t println(long, int = DEC);
+    size_t println(unsigned long, int = DEC);
+    size_t println(double, int = 2);
+    size_t println(const Printable&);
+    size_t println(void);
 };
 
 #endif
diff --git a/cores/arduino/Printable.h b/cores/arduino/Printable.h
index 5ff6077..d03c9af 100644
--- a/cores/arduino/Printable.h
+++ b/cores/arduino/Printable.h
@@ -33,8 +33,7 @@ class Print;
 class Printable
 {
   public:
-    virtual ~Printable() {};
-    virtual void printTo(Print& p) const =0;
+    virtual size_t printTo(Print& p) const = 0;
 };
 
 #endif
diff --git a/cores/arduino/Stream.cpp b/cores/arduino/Stream.cpp
new file mode 100644
index 0000000..d267bf0
--- /dev/null
+++ b/cores/arduino/Stream.cpp
@@ -0,0 +1,233 @@
+/*
+ Stream.cpp - adds parsing methods to Stream class
+ Copyright (c) 2008 David A. Mellis.  All right reserved.
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+ Created July 2011
+ parsing functions based on TextFinder library by Michael Margolis
+ */
+
+#include "Arduino.h"
+#include "Stream.h"
+
+#define PARSE_TIMEOUT 1000  // default number of milli-seconds to wait
+#define NO_SKIP_CHAR  1  // a magic char not found in a valid ASCII numeric field
+
+// private method to read stream with timeout
+int Stream::timedRead()
+{
+  //Serial.println(_timeout);
+  this->_startMillis = millis();
+  while(millis() - this->_startMillis < this->_timeout)
+  {
+    if (this->available() > 0) {
+      return this->read();
+    }
+  }
+  return -1;     // -1 indicates timeout
+}
+
+// returns the next digit in the stream or -1 if timeout
+// discards non-numeric characters
+int Stream::getNextDigit()
+{
+  int c;
+  do{
+    c = timedRead();
+    if( c < 0)
+      return c;  // timeout
+  }
+  while( c != '-' && (c < '0' || c > '9') ) ;
+
+return c;
+}
+
+// Public Methods
+//////////////////////////////////////////////////////////////
+
+void Stream::setTimeout( long timeout)  // sets the maximum number of milliseconds to wait
+{
+   this->_timeout = timeout;
+}
+
+ // find returns true if the target string is found
+bool  Stream::find(char *target)
+{
+  return findUntil(target, NULL);
+}
+
+// reads data from the stream until the target string of given length is found
+// returns true if target string is found, false if timed out
+bool Stream::find(char *target, size_t length)
+{
+  return findUntil(target, length, NULL, 0);
+}
+
+// as find but search ends if the terminator string is found
+bool  Stream::findUntil(char *target, char *terminator)
+{
+  return findUntil(target, strlen(target), terminator, strlen(terminator));
+}
+
+// reads data from the stream until the target string of the given length is found
+// search terminated if the terminator string is found
+// 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]){
+    //////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;
+      }
+    }
+    else{
+      index = 0;  // reset index if any char does not match
+    }
+    if(termLen > 0 && c == terminator[termIndex]){
+       if(++termIndex >= termLen)
+         return false;       // return false if terminate string found before target string
+    }
+    else
+        termIndex = 0;
+  }
+  return false;
+}
+
+
+// returns the first valid (long) integer value from the current position.
+// initial characters that are not digits (or the minus sign) are skipped
+// function is terminated by the first character that is not a digit.
+long Stream::parseInt()
+{
+  return parseInt(NO_SKIP_CHAR); // terminate on first non-digit character (or timeout)
+}
+
+// as above but a given skipChar is ignored
+// this allows format characters (typically commas) in values to be ignored
+long Stream::parseInt(char skipChar)
+{
+  boolean isNegative = false;
+  long value = 0;
+  int c;
+
+  c = getNextDigit();
+  // ignore non numeric leading characters
+  if(c < 0)
+    return 0; // zero returned if timeout
+
+  do{
+    if(c == skipChar)
+      ; // ignore this charactor
+    else if(c == '-')
+      isNegative = true;
+    else if(c >= '0' && c <= '9')        // is c a digit?
+      value = value * 10 + c - '0';
+    c = timedRead();
+  }
+  while( (c >= '0' && c <= '9')  || c == skipChar );
+
+  if(isNegative)
+    value = -value;
+  return value;
+}
+
+
+// as parseInt but returns a floating point value
+float Stream::parseFloat()
+{
+  parseFloat(NO_SKIP_CHAR);
+}
+
+// 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;
+  long value = 0;
+  float fValue;
+  char c;
+  float fraction = 1.0;
+
+  c = getNextDigit();
+    // ignore non numeric leading characters
+  if(c < 0)
+    return 0; // zero returned if timeout
+
+  do{
+    if(c == skipChar)
+      ; // ignore
+    else if(c == '-')
+      isNegative = true;
+    else if (c == '.')
+      isFraction = true;
+    else if(c >= '0' && c <= '9')  {      // is c a digit?
+      value = value * 10 + c - '0';
+      if(isFraction)
+         fraction *= 0.1;
+    }
+    c = timedRead();
+  }
+  while( (c >= '0' && c <= '9')  || c == '.' || c == skipChar );
+
+  if(isNegative)
+    value = -value;
+  if(isFraction)
+    return value * fraction;
+  else
+    return value;
+}
+
+// read characters from stream into buffer
+// terminates if length characters have been read, null is detected or timeout (see setTimeout)
+// returns the number of characters placed in the buffer (0 means no valid data found)
+int Stream::readBytes( char *buffer, size_t length)
+{
+   return readBytesUntil( 0, buffer, length);
+}
+
+
+// as readBytes with terminator character
+// terminates if length characters have been read, timeout, or if the terminator character  detected
+// returns the number of characters placed in the buffer (0 means no valid data found)
+
+int Stream::readBytesUntil( char terminator, char *buffer, size_t length)
+{
+ int index = 0;
+    *buffer = 0;
+    while(index < length-1 ){
+      int c = timedRead();
+      if( c <= 0 ){
+        return 0;   // timeout returns 0 !
+      }
+      else if( c == terminator){
+        buffer[index] = 0; // terminate the string
+        return index;               // data got successfully
+      }
+      else{
+        buffer[index++] = (char)c;
+      }
+    }
+    buffer[index] = 0;
+    return index; // here if buffer is full before detecting the terminator
+}
+
diff --git a/cores/arduino/Stream.h b/cores/arduino/Stream.h
index 93d8275..1633f15 100644
--- a/cores/arduino/Stream.h
+++ b/cores/arduino/Stream.h
@@ -15,6 +15,8 @@
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+  parsing functions based on TextFinder library by Michael Margolis
 */
 
 #ifndef Stream_h
@@ -23,13 +25,69 @@
 #include <inttypes.h>
 #include "Print.h"
 
+// compatability macros for testing
+/*
+#define   getInt()            parseInt()
+#define   getInt(skipChar)    parseInt(skipchar)
+#define   getFloat()          parseFloat()
+#define   getFloat(skipChar)  parseFloat(skipChar)
+#define   getString( pre_string, post_string, buffer, length)
+readBytesBetween( pre_string, terminator, buffer, length)
+*/
+
 class Stream : public Print
 {
+  private:
+    long _timeout;      // number of milliseconds to wait for the next char before aborting timed read
+    long _startMillis;  // used for timeout measurement
+    int timedRead();    // private method to read stream with timeout
+    int getNextDigit(); // returns the next numeric digit in the stream or -1 if timeout
+
   public:
     virtual int available() = 0;
     virtual int read() = 0;
     virtual int peek() = 0;
     virtual void flush() = 0;
+
+    Stream() {_timeout=1000;}
+
+// parsing methods
+
+  void setTimeout(long timeout);  // sets maximum milliseconds to wait for stream data, default is 1 second
+
+  bool find(char *target);   // reads data from the stream until the target string is found
+  // returns true if target string is found, false if timed out (see setTimeout)
+
+  bool find(char *target, size_t length);   // reads data from the stream until the target string of given length is found
+  // returns true if target string is found, false if timed out
+
+  bool findUntil(char *target, char *terminator);   // as find but search ends if the terminator string is found
+
+  bool findUntil(char *target, size_t targetLen, char *terminate, size_t termLen);   // as above but search ends if the terminate string is found
+
+
+  long parseInt(); // returns the first valid (long) integer value from the current position.
+  // initial characters that are not digits (or the minus sign) are skipped
+  // integer is terminated by the first character that is not a digit.
+
+  long parseInt(char skipChar); // as above but the given skipChar is ignored
+  // as above but the given skipChar is ignored
+  // this allows format characters (typically commas) in values to be ignored
+
+  float parseFloat();               // float version of parseInt
+
+  float parseFloat(char skipChar);  // as above but the given skipChar is ignored
+
+  int readBytes( char *buffer, size_t length); // read chars from stream into buffer
+  // terminates if length characters have been read or timeout (see setTimeout)
+  // returns the number of characters placed in the buffer (0 means no valid data found)
+
+  int readBytesUntil( char terminator, char *buffer, size_t length); // as readBytes with terminator character
+  // terminates if length characters have been read, timeout, or if the terminator character  detected
+  // returns the number of characters placed in the buffer (0 means no valid data found)
+
+  // Arduino String functions to be added here
+
 };
 
 #endif