diff --git a/src/ExtendedSerial.h b/src/ExtendedSerial.h
new file mode 100644
index 0000000..d3432ef
--- /dev/null
+++ b/src/ExtendedSerial.h
@@ -0,0 +1,164 @@
+//-----------------------------------------------------------------------------
+// Decorator for Arduino's Stream class, which is a/o implemented by Serial
+//-----------------------------------------------------------------------------
+//
+// Copyright (C) 2018 Arend Lammertink
+//
+// 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, version 3.
+//
+// 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, see <http://www.gnu.org/licenses/>.
+//
+//-----------------------------------------------------------------------------
+//
+//
+// Description:
+//
+// The Serial class in Arduino is derived from the Stream class, which on it's
+// turn is derived from the Print class. In these, there is no support for
+// reading/writing integers or floats in binary form.
+//
+// In order to add this functionality, we use the Decorator pattern:
+//
+//  https://cpppatterns.com/patterns/decorator.html 
+//
+// The ExtendedSerial class is derived from the Stream class and thus
+// implements all methods which are present in the Stream class, while it adds
+// a number of new methods to send and receive values in binary format as well
+// as support for printing Fix16 fixed point numbers. Also, all the methods
+// from the HardwareSerial class are implemented.
+//
+// It can be instantiated using an existing ExtendedSerial instance, such as
+// Serial, Serial1, etc:
+//
+//   ExtentedSerial mySerial(Serial);
+//
+// It can then be used just like any other Serial, like:
+//   
+//   mySerial.begin(9600);
+//   mySerial.println("Hello, world!");
+//
+// In additon to the standard methods, one can also send and receive ints,
+// floats and Fix16s in binary format. Both the send and receive methods
+// return the number of bytes transferred. For example:
+//
+//   float f=1.23;
+//   mySerial.send(f);
+//
+//   float g;
+//   if( !mySerial.receive(g) ) { mySerial.println("No data available"); }
+//
+//-----------------------------------------------------------------------------
+
+//-----------------------------------------------------------------------------
+// Multi-include protection
+//-----------------------------------------------------------------------------
+
+#ifndef _EXTENDED_SERIAL_H
+#define _EXTENDED_SERIAL_H
+
+//-----------------------------------------------------------------------------
+// Includes
+//-----------------------------------------------------------------------------
+
+#include <HardwareSerial.h>
+#include <fix16.hpp>
+
+//-----------------------------------------------------------------------------
+// ExtendedSerial class
+//-----------------------------------------------------------------------------
+
+class ExtendedSerial : public Stream
+{
+
+  public:
+                    ExtendedSerial(HardwareSerial& s) : _s(s) {}
+
+    // Methods delegated to existing Serial instance
+    void            begin(unsigned long baud)         { begin(baud, SERIAL_8N1);  }
+    void            begin(unsigned long b, uint8_t m) { _s.begin(b,m);            }
+    void            end()                             { _s.end();                 }
+    virtual int     available(void)                   { return _s.available();    }
+    virtual int     peek(void)                        { return _s.peek();         }
+    virtual int     read(void)                        { return _s.read();         }
+    virtual void    flush(void)                       { return _s.flush();        }
+    virtual size_t  write(uint8_t n)                  { return _s.write(n);       }
+    virtual size_t  write(const uint8_t* b,size_t sz) { return _s.write(b,sz);    }
+    inline  size_t  write(unsigned long n)            { return write((uint8_t)n); }
+    inline  size_t  write(long n)                     { return write((uint8_t)n); }
+    inline  size_t  write(unsigned int n)             { return write((uint8_t)n); }
+    inline  size_t  write(int n)                      { return write((uint8_t)n); }
+    virtual int     availableForWrite(void)           { return _s.availableForWrite(); }
+    operator bool()                                   { return true;                   }
+
+    // Extra methods
+    size_t          print(  Fix16 f, int d = 2)       { return print(  (double)f,d); }
+    size_t          println(Fix16 f, int d = 2)       { return println((double)f,d); }
+
+    size_t          receive(uint8_t* buf, size_t sz)
+                    {
+                      if( available() < sz )            return 0;
+
+                      for( int i=0; i<sz; i++)        { *buf++ = (uint8_t)read();    }
+
+                      return sz;
+                    }
+
+    size_t          send(const uint8_t& x)  { return write(x);                                   }
+    size_t          receive(uint8_t& x)     { return receive((uint8_t*)&x, sizeof(uint8_t));     }
+
+    size_t          send(const int8_t& x)   { return write((uint8_t)x);                          }
+    size_t          receive(int8_t& x)      { return receive((uint8_t*)&x, sizeof(int8_t));      }
+
+    size_t          send(const int16_t& x)  { return write((const uint8_t*)&x, sizeof(int16_t)); }
+    size_t          receive(int16_t& x)     { return receive((uint8_t*)&x,     sizeof(int16_t)); }
+
+    size_t          send(const uint16_t& x) { return write((const uint8_t*)&x, sizeof(uint16_t));}
+    size_t          receive(uint16_t& x)    { return receive((uint8_t*)&x,     sizeof(uint16_t));}
+
+    size_t          send(const int32_t& x)  { return write((const uint8_t*)&x, sizeof(int32_t)); }
+    size_t          receive(int32_t& x)     { return receive((uint8_t*)&x,     sizeof(int32_t)); }
+
+    size_t          send(const uint32_t& x) { return write((const uint8_t*)&x, sizeof(uint32_t));}
+    size_t          receive(uint32_t& x)    { return receive((uint8_t*)&x,     sizeof(uint32_t));}
+
+    size_t          send(const float& x)    { return write((const uint8_t*)&x, sizeof(float));   }
+    size_t          receive(float& x)       { return receive((uint8_t*)&x,     sizeof(float));   }
+
+    size_t          send(const double& x)   { return write((const uint8_t*)&x, sizeof(double));  }
+    size_t          receive(double& x)      { return receive((uint8_t*)&x,     sizeof(double));  }
+
+    size_t          send(const Fix16& x)    { return write((const uint8_t*)&x, sizeof(Fix16));   }
+    size_t          receive(Fix16& x)       { return receive((uint8_t*)&x,     sizeof(Fix16));   }
+
+    using           Print::write;   // pull in the other write methods from Print.
+    using           Print::print;   // pull in existing print methods from Print.
+    using           Print::println; // pull in existing println methods from Print.
+
+  protected:
+                    // Must be instantiated with a HardwareSerial&.
+                    ExtendedSerial()                {}
+                    ExtendedSerial(ExtendedSerial&) {}
+                    ExtendedSerial(int)             {}
+                    ExtendedSerial(const char*)     {}
+
+  private:
+
+    HardwareSerial& _s;
+};
+
+
+
+
+//-----------------------------------------------------------------------------
+// 
+//-----------------------------------------------------------------------------
+
+#endif // End multi-include protection
diff --git a/src/Map2D3D.h b/src/Map2D3D.h
index 2bfb7e6..b479383 100644
--- a/src/Map2D3D.h
+++ b/src/Map2D3D.h
@@ -75,6 +75,7 @@
 #include <Print.h>
 #include "interpolate.h"
 #include "toString.h"
+#include "ExtendedSerial.h"
 
 #ifdef AVR
 #include <avr/eeprom.h>
@@ -104,19 +105,30 @@
 class Map
 {
   public:
-    virtual int     memSize() const                                 =0;
+    virtual int   memSize() const                               =0;
 
-#ifdef AVR
+    virtual bool  updateEeprom(uint8_t*      dest) const        =0;
+    virtual bool  readEeprom( const uint8_t* src)               =0;
 
-    virtual bool    updateEeprom(uint8_t* dest) const               =0;
-    virtual bool    readEeprom(const uint8_t* src)                  =0;
+    virtual void  printTo( Print& p, const uint8_t tabsize = 4,
+                           const char delim = ' '              )=0;
 
-#endif
+    virtual void  sendTo( ExtendedSerial& s)                    =0;
+
+    void          initReceive( uint16_t offset, size_t nr_bytes) 
+                  { bytesToReceive=nr_bytes; bytesReceived=0; curOffset=offset; }
+
+    bool		  receiveDone()
+                  { return bytesReceived >= bytesToReceive; }
+                
+    virtual bool  receiveFrom( ExtendedSerial& s)
+                  { return receiveDone();                   }
 
-    virtual void    printTo( Print& p, const uint8_t tabsize = 4,
-                             const char delim = ' ')                =0;
+    protected:
 
-//  virtual void    printBinaryTo( Print& p)
+    size_t		 bytesToReceive;
+    size_t		 bytesReceived;
+    uint16_t	 curOffset;
 };
 
 
@@ -134,33 +146,31 @@ class Map2D : public Map
                       for( int i=0; i<S; i++ ) { xs[i] = 0; ys[i] = 0; }
                   }
 
-    int           xSize()   const  { return S; }
-    int           ySize()   const  { return S; }
-    int           memSize() const  { return S*(sizeof(X)+sizeof(Y));}
+    int           xSize()   const       { return S; }
+    int           ySize()   const       { return S; }
+    int           memSize() const       { return S*(sizeof(X)+sizeof(Y)); }
 
-    void          setXs( const X* xss )
-                        { memcpy( xs, xss, S*sizeof(X) ); }
+    void          setXs( const X* xss ) { memcpy( xs, xss, S*sizeof(X) ); }
 
     void          setXsFromFloat( const float* xss )
                   {
-                      for( int i=0; i<S; i++ )  { xs[i] = static_cast<X>(xss[i]); }
-                  }
+                      for( int i=0; i<S; i++ ) { xs[i] = static_cast<X>(xss[i]); }
+                  }                        
 
-    int           getXInt( int i )              { return 0<=i<S ? static_cast<int>(xs[i])   : 0; }
+    int           getXInt( int i )      { return 0<=i<S ? static_cast<int>(xs[i])   : 0; }
 
-    float         getXFloat( int i )            { return 0<=i<S ? static_cast<float>(xs[i]) : 0; }
+    float         getXFloat( int i )    { return 0<=i<S ? static_cast<float>(xs[i]) : 0; }
 
-    void          setYs( const Y* yss )
-                        { memcpy( ys, yss, S*sizeof(Y) ); }
+    void          setYs( const Y* yss ) { memcpy( ys, yss, S*sizeof(Y) ); }
 
     void          setYsFromFloat( const float* yss )
                   {
                       for( int i=0; i<S; i++ ) { ys[i] = static_cast<Y>(yss[i]); }
                   }
 
-    int           getYInt( int i )              { return 0<=i<S ? static_cast<int>(ys[i])   : 0; }
+    int           getYInt( int i )      { return 0<=i<S ? static_cast<int>(ys[i])   : 0; }
 
-    float         getYFloat( int i )            { return 0<=i<S ? static_cast<float>(ys[i]) : 0; }
+    float         getYFloat( int i )    { return 0<=i<S ? static_cast<float>(ys[i]) : 0; }
 
 #ifdef AVR
 
@@ -169,7 +179,7 @@ class Map2D : public Map
                     if( !eeprom_is_ready() ) return false;
 
                     eeprom_update_block( xs, dest, sizeof(xs) );
-                    eeprom_update_block( ys, dest+sizeof(xs), sizeof(ys) );
+                    eeprom_update_block( ys, dest+ sizeof(xs), sizeof(ys) );
 
                     return true;
                   }
@@ -179,7 +189,7 @@ class Map2D : public Map
                     if( !eeprom_is_ready() ) return false;
 
                     eeprom_read_block( xs, src, sizeof(xs) );
-                    eeprom_read_block( ys, src+sizeof(xs), sizeof(ys) );
+                    eeprom_read_block( ys, src+ sizeof(xs), sizeof(ys) );
 
                     return true;
                   }
@@ -187,8 +197,7 @@ class Map2D : public Map
 
 #ifdef ARDUINO    // Initialization from array in PROGMEM
 
-    void          setXs_P( const X* xss )
-                        { memcpy_P( xs, xss, S*sizeof(X) ); }
+    void          setXs_P( const X* xss ) { memcpy_P( xs, xss, S*sizeof(X) ); }
 
     void          setXsFromFloat_P( const float* xss )
                   {
@@ -196,9 +205,8 @@ class Map2D : public Map
                         xs[i] = static_cast<X>(pgm_read_float_near(xss+i)); }
                   }
 
-    void          setYs_P( const Y* yss )
-                        { memcpy_P( ys, yss, S*sizeof(Y) ); }
-
+    void          setYs_P( const Y* yss ) { memcpy_P( ys, yss, S*sizeof(Y) ); }
+                       
     void          setYsFromFloat_P( const float* yss )
                   {
                       for( int i=0; i<S; i++ )
@@ -206,19 +214,19 @@ class Map2D : public Map
                   }
 #endif
 
-    void          printTo( Print& p, const uint8_t tabsize = 4, const char delim = ' ')
+    virtual void  printTo( Print& p, const uint8_t tabsize = 4, const char delim = ' ' )
                   {
                     const char spaceChar=' ';
-
+                    
 //                      p.println( table title ); // TODO
 
                     p.println();
 
-                    for (int x = 0; x < xSize(); x++)
+                    for (int x = 0; x < S; x++)
                     {
                       const char* _x = toString(xs[x]);
 
-                      for( uint16_t idx=0; idx<tabsize-strlen(_x); idx++)     p.write(spaceChar);
+                      for( uint16_t idx=0; idx<tabsize-strlen(_x); idx++)    p.write(spaceChar);
 
                       p.print(_x);// Vertical Bins
                       p.write(delim);
@@ -234,6 +242,38 @@ class Map2D : public Map
                     p.println();
                   }
 
+    virtual void  sendTo( ExtendedSerial& s)
+                  {
+                    for (int i = 0; i < S; i++) { s.send( xs[i] ); }
+                    for (int i = 0; i < S; i++) { s.send( ys[i] ); }
+                  }
+
+    virtual bool  receiveFrom( ExtendedSerial& s) 
+				  {
+                    const size_t xend = S*sizeof(X);
+                    const size_t yend = xend + S*sizeof(Y);
+                      
+                    while( (curOffset < xend) && !receiveDone() )
+                    {
+                       int idx = curOffset / sizeof(X);
+                       size_t recvd = s.receive(xs[idx]);
+
+                       if( recvd) { curOffset += recvd; }
+                       else return false; // avoid infinite loop when we receive nothing.
+                    }
+
+                    while( (curOffset < yend) && !receiveDone() )
+                    {
+                       int idx = (curOffset - xend) / sizeof(Y);
+                       size_t recvd = s.receive(ys[idx]);
+
+                       if( recvd) { curOffset += recvd; }
+                       else return false; // avoid infinite loop when we receive nothing.
+                    }
+
+                    return receiveDone();   // bytesReceived >= bytesToReceive
+				  }
+
 
     Y             f( X x )              // approximate f(x)
                   {
@@ -266,28 +306,28 @@ class Map2D : public Map
 // 3D lookup table / fuel map. X axis must be sorted in ascending order.
 //-----------------------------------------------------------------------------
 
-template<int R, int S, typename X, typename Y> // R,S: size, X,Y: data type
+template<int R, int C, typename X, typename Y> // R,C: size, X,Y: data type
 class Map3D : public Map
 {
 public:
                   Map3D()
                   {
                       for( int i=0; i<R;   i++ ) { x1s[i] = 0; }
-                      for( int i=0; i<S;   i++ ) { x2s[i] = 0; }
-                      for( int i=0; i<R+S; i++ ) { ys[i]  = 0; }
-                  }
+                      for( int i=0; i<C;   i++ ) { x2s[i] = 0; }
+                      for( int i=0; i<R*C; i++ ) { ys[i]  = 0; }
+                  }                        
 
     int           x1Size()  const    { return R;   }
-    int           x2Size()  const    { return S;   }
-    int           ySize()   const    { return R*S; }
-    int           memSize() const    { return (R+S)*sizeof(X) + (R*S)*sizeof(Y);}
+    int           x2Size()  const    { return C;   }
+    int           ySize()   const    { return R*C; }
+    int           memSize() const    { return (R+C)*sizeof(X) + (R*C)*sizeof(Y);}
 
 
     void          setX1s( const X* x1ss )
                         { memcpy( x1s, x1ss, R*sizeof(X) ); }
 
-    void          setX2s( const X* x2ss )
-                        { memcpy( x2s, x2ss, S*sizeof(X) ); }
+    void          setX2s( const X* x2ss ) 
+                        { memcpy( x2s, x2ss, C*sizeof(X) ); }
 
     void          setX1sFromFloat( const float* xss )
                   {
@@ -296,32 +336,29 @@ class Map3D : public Map
 
     void          setX2sFromFloat( const float* xss )
                   {
-                      for( int i=0; i<S; i++ )  { x2s[i] = static_cast<X>(xss[i]); }
+                      for( int i=0; i<C; i++ )  { x2s[i] = static_cast<X>(xss[i]); }
                   }
 
-    int           getX1Int( int i )              { return 0<=i<R ? static_cast<int>(x1s[i])   : 0; }
-    int           getX2Int( int i )              { return 0<=i<S ? static_cast<int>(x2s[i])   : 0; }
+    int           getX1Int( int i )   { return 0<=i<R ? static_cast<int>(x1s[i])   : 0; }
+    int           getX2Int( int i )   { return 0<=i<C ? static_cast<int>(x2s[i])   : 0; }
 
-    float         getX1Float( int i )            { return 0<=i<R ? static_cast<float>(x1s[i]) : 0; }
-    float         getX2Float( int i )            { return 0<=i<S ? static_cast<float>(x2s[i]) : 0; }
+    float         getX1Float( int i ) { return 0<=i<R ? static_cast<float>(x1s[i]) : 0; }
+    float         getX2Float( int i ) { return 0<=i<C ? static_cast<float>(x2s[i]) : 0; }
 
     void          setYs( const Y* yss )
-                        { memcpy( ys, yss, R*S*sizeof(Y) ); }
-
+                        { memcpy( ys, yss, R*C*sizeof(Y) ); }
 
     void          setYsFromFloat( const float* yss )
                   {
-                      for( int i=0; i<R*S; i++ )
+                      for( int i=0; i<R*C; i++ ) 
                         { ys[i] = static_cast<Y>(yss[i]); }
                   }
 
     int           getYInt( int i, int j )
-                      { return 0<=i<R && 0<=j<S ? static_cast<int>(ys[i][j]) : 0; }
+                      { return 0<=i<R && 0<=j<C ? static_cast<int>(ys[i][j]) : 0; }
 
     float         getYFloat( int i, int j )
-                      { return 0<=i<R && 0<=j<S ? static_cast<float>(ys[i][j]) : 0; }
-
-#ifdef AVR
+                      { return 0<=i<R && 0<=j<C ? static_cast<float>(ys[i][j]) : 0; }
 
     virtual bool  updateEeprom(uint8_t* dest) const
                   {
@@ -344,15 +381,11 @@ class Map3D : public Map
 
                     return true;
                   }
-#endif
 
 #ifdef ARDUINO    // Initialize from array in PROGMEM
 
-    void          setX1s_P( const X* x1ss )
-                        { memcpy_P( x1s, x1ss, R*sizeof(X) ); }
-
-    void          setX2s_P( const X* x2ss )
-                        { memcpy_P( x2s, x2ss, S*sizeof(X) ); }
+    void          setX1s_P( const X* x1ss ) { memcpy_P( x1s, x1ss, R*sizeof(X) ); }
+    void          setX2s_P( const X* x2ss ) { memcpy_P( x2s, x2ss, C*sizeof(X) ); }
 
     void          setX1sFromFloat_P( const float* xss )
                   {
@@ -362,42 +395,41 @@ class Map3D : public Map
 
     void          setX2sFromFloat_P( const float* xss )
                   {
-                      for( int i=0; i<S; i++ )
+                      for( int i=0; i<C; i++ )
                         { x2s[i] = static_cast<X>(pgm_read_float_near(xss+i)); }
                   }
 
-    void          setYs_P( const Y* yss )
-                        { memcpy_P( ys, yss, R*S*sizeof(Y) ); }
+    void          setYs_P( const Y* yss ) { memcpy_P( ys, yss, R*C*sizeof(Y) ); }
 
     void          setYsFromFloat_P( const float* yss )
                   {
-                      for( int i=0; i<R*S; i++ )
+                      for( int i=0; i<R*C; i++ ) 
                           { ys[i] = static_cast<Y>( pgm_read_float_near(yss+i) ); }
                   }
 
 
 #endif
 
-    void          printTo( Print& p, const uint8_t tabsize = 4, const char delim = ' ')
+    virtual void  printTo( Print& p, const uint8_t tabsize = 4, const char delim = ' ' )
                   {
                     const char spaceChar=' ';
-
+                    
 //                  p.println( table title ); // TODO
 
                     p.println();
-                    for (int y = 0; y < x2Size(); y++)
+                    for( int x = 0; x < R; x++ )
                     {
-                      const char* _x2 = toString(x2s[y]);
+                      const char* _x1 = toString(x1s[x]);
 
-                      for( uint16_t idx=0; idx<tabsize-strlen(_x2); idx++)  p.write(spaceChar);
+                      for( int idx=0; idx<tabsize-strlen(_x1); idx++)    p.write(spaceChar);
 
-                      p.print(_x2);// Vertical Bins
+                      p.print(_x1);             // Vertical
                       p.write(delim);
 
-                      for (int i = 0; i < x1Size(); i++)
+                      for (int y = 0; y < C; y++)
                       {
-                        const char* value = toString(ys[y][i]);
-                        for( uint16_t idx=0; idx<tabsize-strlen(value); idx++) p.write(spaceChar);
+                        const char* value = toString(ys[x][y]);
+                        for( int idx=0; idx<tabsize-strlen(value); idx++) p.write(spaceChar);
 
                         p.print(value);
                         p.write(delim);
@@ -405,26 +437,82 @@ class Map3D : public Map
                       p.println();
                     }
 
-                    for( uint16_t idx=0; idx<tabsize; idx++)                p.write(spaceChar);
+                    for( int idx=0; idx<tabsize; idx++)                p.write(spaceChar);
 
-                    for (int x = 0; x < x1Size(); x++)// Horizontal bins
+                    for (int x = 0; x < C; x++) // Horizontal 
                     {
-                      const char* _x1 = toString(x1s[x]); //  / 100; for TS
-                      for( uint16_t idx=0; idx<tabsize-strlen(_x1); idx++)  p.write(spaceChar);
+                      const char* _x2 = toString(x2s[x]);
+                      for( uint16_t idx=0; idx<tabsize-strlen(_x2); idx++)  p.write(spaceChar);
 
-                      p.print(_x1);
+                      p.print(_x2);
                       p.write(delim);
                     }
                     p.println();
                   }
 
+    virtual void  sendTo( ExtendedSerial& s)
+                  {
+                    for (int i = 0; i < R; i++) { s.send( x1s[i] ); }
+                    for (int i = 0; i < C; i++) { s.send( x2s[i] ); }
+
+                    //for (int x = 0; x < R; x++)
+                    for (int x = R-1; x >=0; x--)  // TS likes rows in reverse order
+                    {
+                      for (int y = 0; y < C; x++)
+                        { s.send( ys[x][y] ); }
+                    }
+                  }
+
+    virtual bool  receiveFrom( ExtendedSerial& s) 
+				  {
+                    const size_t x1end = R*sizeof(X);
+                    const size_t x2end = x1end + C*sizeof(X);
+                    const size_t yend  = x2end + R*C*sizeof(Y);
+                      
+                    while( (curOffset < x1end) && !receiveDone() )
+                    {
+                       int idx = curOffset / sizeof(X);
+
+                       size_t recvd = s.receive(x1s[idx]);
+
+                       if( recvd) { curOffset += recvd; }
+                       else return false; // avoid infinite loop when we receive nothing.
+                    }
+
+                    while( (curOffset < x2end) && !receiveDone() )
+                    {
+                       int idx = (curOffset - x1end) / sizeof(X);
+
+                       size_t recvd = s.receive(x2s[idx]);
+
+                       if( recvd) { curOffset += recvd; }
+                       else return false; // avoid infinite loop when we receive nothing.
+                    }
+
+                    while( (curOffset < yend) && !receiveDone() )
+                    {
+                       int idx = (curOffset - x2end) / sizeof(Y);
+
+                       //int idx1 = idx/C; 
+                       int idx1 = R - 1 - idx/C;  // TS likes the rows in reverse order
+                       int idx2 = idx%C;
+
+                       size_t recvd = s.receive(ys[idx1][idx2]);
+
+                       if( recvd) { curOffset += recvd; }
+                       else return false; // avoid infinite loop when we receive nothing.
+                    }
+
+                    return receiveDone();   // bytesReceived >= bytesToReceive
+				  }
+
 
     Y             f( X x1, X x2 )
                   {
                     if (x1 < x1s[0])      { x1 = x1s[0];   } // minimum
                     if (x1 > x1s[R-1])    { x1 = x1s[R-1]; } // maximum
                     if (x2 < x2s[0])      { x2 = x2s[0];   } // minimum
-                    if (x2 > x2s[S-1])    { x2 = x2s[S-1]; } // maximum
+                    if (x2 > x2s[C-1])    { x2 = x2s[C-1]; } // maximum
 
                     int i=0, j=R-1, m;
 
@@ -436,7 +524,7 @@ class Map3D : public Map
                         else                j = m;
                     }
 
-                    int k=S-1;
+                    int k=C-1; 
                     j=0;
 
                     // find j, such that x2s[j] <= x2 < x2s[k] using bisection
@@ -454,9 +542,9 @@ class Map3D : public Map
 
 protected:
 
-    X             x1s[S];
-    X             x2s[S];
-    Y             ys[R][S];
+    X             x1s[R];
+    X             x2s[C];
+    Y             ys[R][C];
 
 };
 
diff --git a/src/examples/test2d/test2d.ino b/src/examples/test2d/test2d.ino
index 0073b91..69b12ae 100644
--- a/src/examples/test2d/test2d.ino
+++ b/src/examples/test2d/test2d.ino
@@ -29,7 +29,7 @@
 
 using namespace std;
 
-#define BAUDRATE    9600
+#define BAUDRATE    115200
 
 // Stuff needed for printing
 FILE                    serial_stdout;
@@ -103,8 +103,11 @@ void setup()
     Map2D<8, int16_t, int8_t>  testInt8;
     testInt8.setXs_P(xs);
     testInt8.setYs_P(ys8);
+    testInt8.printTo( Serial );
     testInt8.updateEeprom(0);
+    testInt8.printTo( Serial );
     testInt8.readEeprom(0);
+    testInt8.printTo( Serial );
 
     Map2D<8, int16_t, Fix16>  testFix16;
     testFix16.setXs_P(xs);
diff --git a/src/gen_spec1D.py b/src/gen_spec1D.py
index 67fa8ec..b6a5ca8 100755
--- a/src/gen_spec1D.py
+++ b/src/gen_spec1D.py
@@ -1,25 +1,12 @@
 #!/usr/bin/python
 
-print "//-----------------------------------------------------------------------------"
-print "//"
-print "// Copyright (C) 2018 Arend Lammertink"
-print "//"
-print "// This program is free software; you can redistribute it and/or modify it"
-print "// under the terms of the GNU General Public License as published by the Free"
-print "// Software Foundation, version 3."
-print "//"
-print "// This program is distributed in the hope that it will be useful, but WITHOUT"
-print "// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or"
-print "// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for"
-print "// more details."
-print "//"
-print "// You should have received a copy of the GNU Lesser General Public License"
-print "// along with this program. If not, see <http://www.gnu.org/licenses/>."
-print "//-----------------------------------------------------------------------------"
-print
-print"//-----------------------------------------------------------------------------"
+from utils import *
+
+print inc_header
+
+print sep
 print"// 1D specialization for integers by casting to Fix16, generated by script "
-print"//-----------------------------------------------------------------------------"
+print sep
 print
 
 print 
diff --git a/src/gen_spec2D.py b/src/gen_spec2D.py
index 60b3540..9f3da3c 100755
--- a/src/gen_spec2D.py
+++ b/src/gen_spec2D.py
@@ -1,25 +1,12 @@
 #!/usr/bin/python
 
-print "//-----------------------------------------------------------------------------"
-print "//"
-print "// Copyright (C) 2018 Arend Lammertink"
-print "//"
-print "// This program is free software; you can redistribute it and/or modify it"
-print "// under the terms of the GNU General Public License as published by the Free"
-print "// Software Foundation, version 3."
-print "//"
-print "// This program is distributed in the hope that it will be useful, but WITHOUT"
-print "// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or"
-print "// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for"
-print "// more details."
-print "//"
-print "// You should have received a copy of the GNU Lesser General Public License"
-print "// along with this program. If not, see <http://www.gnu.org/licenses/>."
-print "//-----------------------------------------------------------------------------"
-print
-print"//-----------------------------------------------------------------------------"
+from utils import *
+
+print inc_header
+
+print sep
 print"// 2D specialization for integers by casting to Fix16, generated by script."
-print"//-----------------------------------------------------------------------------"
+print sep
 print
 
 print 
diff --git a/src/interpol1d_spec.h b/src/interpol1d_spec.h
index 796243c..8f98b38 100644
--- a/src/interpol1d_spec.h
+++ b/src/interpol1d_spec.h
@@ -2,17 +2,18 @@
 //
 // Copyright (C) 2018 Arend Lammertink
 //
-// This program is free software; you can redistribute it and/or modify it
-// under the terms of the GNU General Public License as published by the Free
-// Software Foundation, version 3.
+// 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, version 3.
 //
-// This program is distributed in the hope that it will be useful, but WITHOUT
+// 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 General Public License for
-// more details.
+// 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 program. If not, see <http://www.gnu.org/licenses/>.
+// along with this library. If not, see <http://www.gnu.org/licenses/>.
+//
 //-----------------------------------------------------------------------------
 
 //-----------------------------------------------------------------------------
diff --git a/src/interpol2d_spec.h b/src/interpol2d_spec.h
index be660b6..1eb7a64 100644
--- a/src/interpol2d_spec.h
+++ b/src/interpol2d_spec.h
@@ -2,17 +2,18 @@
 //
 // Copyright (C) 2018 Arend Lammertink
 //
-// This program is free software; you can redistribute it and/or modify it
-// under the terms of the GNU General Public License as published by the Free
-// Software Foundation, version 3.
+// 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, version 3.
 //
-// This program is distributed in the hope that it will be useful, but WITHOUT
+// 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 General Public License for
-// more details.
+// 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 program. If not, see <http://www.gnu.org/licenses/>.
+// along with this library. If not, see <http://www.gnu.org/licenses/>.
+//
 //-----------------------------------------------------------------------------
 
 //-----------------------------------------------------------------------------
@@ -23,299 +24,464 @@
 #ifndef _interpol2d_spec_h
 #define _interpol2d_spec_h
 
+template<>
 inline int8_t interpolate( int8_t x1, int8_t x2,
                             int8_t x_1, int8_t x_2, int8_t x_3, int8_t x_4,
                             int8_t y_1, int8_t y_2, int8_t y_3, int8_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline uint8_t interpolate( int8_t x1, int8_t x2,
                             int8_t x_1, int8_t x_2, int8_t x_3, int8_t x_4,
                             uint8_t y_1, uint8_t y_2, uint8_t y_3, uint8_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline int16_t interpolate( int8_t x1, int8_t x2,
                             int8_t x_1, int8_t x_2, int8_t x_3, int8_t x_4,
                             int16_t y_1, int16_t y_2, int16_t y_3, int16_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int16_t>(
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
                                static_cast<Fix16>(y_1),
                                static_cast<Fix16>(y_2),
                                static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(y_4) )
+#ifndef DEBUG
+                             );
+#else
+  return static_cast<int16_t>(retval);
+#endif
 }
 
 
+template<>
 inline uint16_t interpolate( int8_t x1, int8_t x2,
                             int8_t x_1, int8_t x_2, int8_t x_3, int8_t x_4,
                             uint16_t y_1, uint16_t y_2, uint16_t y_3, uint16_t y_4 )
 {
-  return static_cast< int32_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint16_t>(static_cast<float>(
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
                                static_cast<Fix16>(static_cast<float>(y_1)),
                                static_cast<Fix16>(static_cast<float>(y_2)),
                                static_cast<Fix16>(static_cast<float>(y_3)),
-                               static_cast<Fix16>(static_cast<float>(y_4)) ));
+                               static_cast<Fix16>(static_cast<float>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint16_t>(static_cast<float>(retval));
+#endif
 }
 
 
+template<>
 inline Fix16 interpolate( int8_t x1, int8_t x2,
                             int8_t x_1, int8_t x_2, int8_t x_3, int8_t x_4,
                             Fix16 y_1, Fix16 y_2, Fix16 y_3, Fix16 y_4 )
 {
-  return static_cast< Fix16 >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return 
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline float interpolate( int8_t x1, int8_t x2,
                             int8_t x_1, int8_t x_2, int8_t x_3, int8_t x_4,
                             float y_1, float y_2, float y_3, float y_4 )
 {
-  return static_cast< float >(
+#ifdef DEBUG
+  float retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<float>(x1),
                                static_cast<float>(x2),
                                static_cast<float>(x_1),
                                static_cast<float>(x_2),
                                static_cast<float>(x_3),
                                static_cast<float>(x_4),
-                               static_cast<float>(y_1),
-                               static_cast<float>(y_2),
-                               static_cast<float>(y_3),
-                               static_cast<float>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline double interpolate( int8_t x1, int8_t x2,
                             int8_t x_1, int8_t x_2, int8_t x_3, int8_t x_4,
                             double y_1, double y_2, double y_3, double y_4 )
 {
-  return static_cast< double >(
+#ifdef DEBUG
+  double retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<double>(x1),
                                static_cast<double>(x2),
                                static_cast<double>(x_1),
                                static_cast<double>(x_2),
                                static_cast<double>(x_3),
                                static_cast<double>(x_4),
-                               static_cast<double>(y_1),
-                               static_cast<double>(y_2),
-                               static_cast<double>(y_3),
-                               static_cast<double>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline int8_t interpolate( uint8_t x1, uint8_t x2,
                             uint8_t x_1, uint8_t x_2, uint8_t x_3, uint8_t x_4,
                             int8_t y_1, int8_t y_2, int8_t y_3, int8_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline uint8_t interpolate( uint8_t x1, uint8_t x2,
                             uint8_t x_1, uint8_t x_2, uint8_t x_3, uint8_t x_4,
                             uint8_t y_1, uint8_t y_2, uint8_t y_3, uint8_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline int16_t interpolate( uint8_t x1, uint8_t x2,
                             uint8_t x_1, uint8_t x_2, uint8_t x_3, uint8_t x_4,
                             int16_t y_1, int16_t y_2, int16_t y_3, int16_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int16_t>(
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
                                static_cast<Fix16>(y_1),
                                static_cast<Fix16>(y_2),
                                static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(y_4) )
+#ifndef DEBUG
+                             );
+#else
+  return static_cast<int16_t>(retval);
+#endif
 }
 
 
+template<>
 inline uint16_t interpolate( uint8_t x1, uint8_t x2,
                             uint8_t x_1, uint8_t x_2, uint8_t x_3, uint8_t x_4,
                             uint16_t y_1, uint16_t y_2, uint16_t y_3, uint16_t y_4 )
 {
-  return static_cast< int32_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint16_t>(static_cast<float>(
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
                                static_cast<Fix16>(static_cast<float>(y_1)),
                                static_cast<Fix16>(static_cast<float>(y_2)),
                                static_cast<Fix16>(static_cast<float>(y_3)),
-                               static_cast<Fix16>(static_cast<float>(y_4)) ));
+                               static_cast<Fix16>(static_cast<float>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint16_t>(static_cast<float>(retval));
+#endif
 }
 
 
+template<>
 inline Fix16 interpolate( uint8_t x1, uint8_t x2,
                             uint8_t x_1, uint8_t x_2, uint8_t x_3, uint8_t x_4,
                             Fix16 y_1, Fix16 y_2, Fix16 y_3, Fix16 y_4 )
 {
-  return static_cast< Fix16 >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return 
+#endif
+                  interpolate( static_cast<Fix16>(static_cast<int16_t>(x1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(x_4)),
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline float interpolate( uint8_t x1, uint8_t x2,
                             uint8_t x_1, uint8_t x_2, uint8_t x_3, uint8_t x_4,
                             float y_1, float y_2, float y_3, float y_4 )
 {
-  return static_cast< float >(
+#ifdef DEBUG
+  float retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<float>(x1),
                                static_cast<float>(x2),
                                static_cast<float>(x_1),
                                static_cast<float>(x_2),
                                static_cast<float>(x_3),
                                static_cast<float>(x_4),
-                               static_cast<float>(y_1),
-                               static_cast<float>(y_2),
-                               static_cast<float>(y_3),
-                               static_cast<float>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline double interpolate( uint8_t x1, uint8_t x2,
                             uint8_t x_1, uint8_t x_2, uint8_t x_3, uint8_t x_4,
                             double y_1, double y_2, double y_3, double y_4 )
 {
-  return static_cast< double >(
+#ifdef DEBUG
+  double retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<double>(x1),
                                static_cast<double>(x2),
                                static_cast<double>(x_1),
                                static_cast<double>(x_2),
                                static_cast<double>(x_3),
                                static_cast<double>(x_4),
-                               static_cast<double>(y_1),
-                               static_cast<double>(y_2),
-                               static_cast<double>(y_3),
-                               static_cast<double>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline int8_t interpolate( int16_t x1, int16_t x2,
                             int16_t x_1, int16_t x_2, int16_t x_3, int16_t x_4,
                             int8_t y_1, int8_t y_2, int8_t y_3, int8_t y_4 )
 {
-  return static_cast< int16_t >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(
+#endif
                   interpolate( static_cast<Fix16>(x1),
                                static_cast<Fix16>(x2),
                                static_cast<Fix16>(x_1),
                                static_cast<Fix16>(x_2),
                                static_cast<Fix16>(x_3),
                                static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline uint8_t interpolate( int16_t x1, int16_t x2,
                             int16_t x_1, int16_t x_2, int16_t x_3, int16_t x_4,
                             uint8_t y_1, uint8_t y_2, uint8_t y_3, uint8_t y_4 )
 {
-  return static_cast< int16_t >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(
+#endif
                   interpolate( static_cast<Fix16>(x1),
                                static_cast<Fix16>(x2),
                                static_cast<Fix16>(x_1),
                                static_cast<Fix16>(x_2),
                                static_cast<Fix16>(x_3),
                                static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline int16_t interpolate( int16_t x1, int16_t x2,
                             int16_t x_1, int16_t x_2, int16_t x_3, int16_t x_4,
                             int16_t y_1, int16_t y_2, int16_t y_3, int16_t y_4 )
 {
-  return static_cast< int16_t >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int16_t>(
+#endif
                   interpolate( static_cast<Fix16>(x1),
                                static_cast<Fix16>(x2),
                                static_cast<Fix16>(x_1),
@@ -325,15 +491,25 @@ inline int16_t interpolate( int16_t x1, int16_t x2,
                                static_cast<Fix16>(y_1),
                                static_cast<Fix16>(y_2),
                                static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(y_4) )
+#ifndef DEBUG
+                             );
+#else
+  return static_cast<int16_t>(retval);
+#endif
 }
 
 
+template<>
 inline uint16_t interpolate( int16_t x1, int16_t x2,
                             int16_t x_1, int16_t x_2, int16_t x_3, int16_t x_4,
                             uint16_t y_1, uint16_t y_2, uint16_t y_3, uint16_t y_4 )
 {
-  return static_cast< int32_t >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint16_t>(static_cast<float>(
+#endif
                   interpolate( static_cast<Fix16>(x1),
                                static_cast<Fix16>(x2),
                                static_cast<Fix16>(x_1),
@@ -343,105 +519,165 @@ inline uint16_t interpolate( int16_t x1, int16_t x2,
                                static_cast<Fix16>(static_cast<float>(y_1)),
                                static_cast<Fix16>(static_cast<float>(y_2)),
                                static_cast<Fix16>(static_cast<float>(y_3)),
-                               static_cast<Fix16>(static_cast<float>(y_4)) ));
+                               static_cast<Fix16>(static_cast<float>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint16_t>(static_cast<float>(retval));
+#endif
 }
 
 
+template<>
 inline Fix16 interpolate( int16_t x1, int16_t x2,
                             int16_t x_1, int16_t x_2, int16_t x_3, int16_t x_4,
                             Fix16 y_1, Fix16 y_2, Fix16 y_3, Fix16 y_4 )
 {
-  return static_cast< Fix16 >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<Fix16>(x1),
                                static_cast<Fix16>(x2),
                                static_cast<Fix16>(x_1),
                                static_cast<Fix16>(x_2),
                                static_cast<Fix16>(x_3),
                                static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline float interpolate( int16_t x1, int16_t x2,
                             int16_t x_1, int16_t x_2, int16_t x_3, int16_t x_4,
                             float y_1, float y_2, float y_3, float y_4 )
 {
-  return static_cast< float >(
+#ifdef DEBUG
+  float retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<float>(x1),
                                static_cast<float>(x2),
                                static_cast<float>(x_1),
                                static_cast<float>(x_2),
                                static_cast<float>(x_3),
                                static_cast<float>(x_4),
-                               static_cast<float>(y_1),
-                               static_cast<float>(y_2),
-                               static_cast<float>(y_3),
-                               static_cast<float>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline double interpolate( int16_t x1, int16_t x2,
                             int16_t x_1, int16_t x_2, int16_t x_3, int16_t x_4,
                             double y_1, double y_2, double y_3, double y_4 )
 {
-  return static_cast< double >(
+#ifdef DEBUG
+  double retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<double>(x1),
                                static_cast<double>(x2),
                                static_cast<double>(x_1),
                                static_cast<double>(x_2),
                                static_cast<double>(x_3),
                                static_cast<double>(x_4),
-                               static_cast<double>(y_1),
-                               static_cast<double>(y_2),
-                               static_cast<double>(y_3),
-                               static_cast<double>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline int8_t interpolate( uint16_t x1, uint16_t x2,
                             uint16_t x_1, uint16_t x_2, uint16_t x_3, uint16_t x_4,
                             int8_t y_1, int8_t y_2, int8_t y_3, int8_t y_4 )
 {
-  return static_cast< int16_t >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(
+#endif
                   interpolate( static_cast<Fix16>(static_cast<float>(x1)),
                                static_cast<Fix16>(static_cast<float>(x2)),
                                static_cast<Fix16>(static_cast<float>(x_1)),
                                static_cast<Fix16>(static_cast<float>(x_2)),
                                static_cast<Fix16>(static_cast<float>(x_3)),
                                static_cast<Fix16>(static_cast<float>(x_4)),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline uint8_t interpolate( uint16_t x1, uint16_t x2,
                             uint16_t x_1, uint16_t x_2, uint16_t x_3, uint16_t x_4,
                             uint8_t y_1, uint8_t y_2, uint8_t y_3, uint8_t y_4 )
 {
-  return static_cast< int16_t >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(
+#endif
                   interpolate( static_cast<Fix16>(static_cast<float>(x1)),
                                static_cast<Fix16>(static_cast<float>(x2)),
                                static_cast<Fix16>(static_cast<float>(x_1)),
                                static_cast<Fix16>(static_cast<float>(x_2)),
                                static_cast<Fix16>(static_cast<float>(x_3)),
                                static_cast<Fix16>(static_cast<float>(x_4)),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline int16_t interpolate( uint16_t x1, uint16_t x2,
                             uint16_t x_1, uint16_t x_2, uint16_t x_3, uint16_t x_4,
                             int16_t y_1, int16_t y_2, int16_t y_3, int16_t y_4 )
 {
-  return static_cast< int16_t >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int16_t>(
+#endif
                   interpolate( static_cast<Fix16>(static_cast<float>(x1)),
                                static_cast<Fix16>(static_cast<float>(x2)),
                                static_cast<Fix16>(static_cast<float>(x_1)),
@@ -451,15 +687,25 @@ inline int16_t interpolate( uint16_t x1, uint16_t x2,
                                static_cast<Fix16>(y_1),
                                static_cast<Fix16>(y_2),
                                static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(y_4) )
+#ifndef DEBUG
+                             );
+#else
+  return static_cast<int16_t>(retval);
+#endif
 }
 
 
+template<>
 inline uint16_t interpolate( uint16_t x1, uint16_t x2,
                             uint16_t x_1, uint16_t x_2, uint16_t x_3, uint16_t x_4,
                             uint16_t y_1, uint16_t y_2, uint16_t y_3, uint16_t y_4 )
 {
-  return static_cast< int32_t >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint16_t>(static_cast<float>(
+#endif
                   interpolate( static_cast<Fix16>(static_cast<float>(x1)),
                                static_cast<Fix16>(static_cast<float>(x2)),
                                static_cast<Fix16>(static_cast<float>(x_1)),
@@ -469,133 +715,208 @@ inline uint16_t interpolate( uint16_t x1, uint16_t x2,
                                static_cast<Fix16>(static_cast<float>(y_1)),
                                static_cast<Fix16>(static_cast<float>(y_2)),
                                static_cast<Fix16>(static_cast<float>(y_3)),
-                               static_cast<Fix16>(static_cast<float>(y_4)) ));
+                               static_cast<Fix16>(static_cast<float>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint16_t>(static_cast<float>(retval));
+#endif
 }
 
 
+template<>
 inline Fix16 interpolate( uint16_t x1, uint16_t x2,
                             uint16_t x_1, uint16_t x_2, uint16_t x_3, uint16_t x_4,
                             Fix16 y_1, Fix16 y_2, Fix16 y_3, Fix16 y_4 )
 {
-  return static_cast< Fix16 >(
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<Fix16>(static_cast<float>(x1)),
                                static_cast<Fix16>(static_cast<float>(x2)),
                                static_cast<Fix16>(static_cast<float>(x_1)),
                                static_cast<Fix16>(static_cast<float>(x_2)),
                                static_cast<Fix16>(static_cast<float>(x_3)),
                                static_cast<Fix16>(static_cast<float>(x_4)),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline float interpolate( uint16_t x1, uint16_t x2,
                             uint16_t x_1, uint16_t x_2, uint16_t x_3, uint16_t x_4,
                             float y_1, float y_2, float y_3, float y_4 )
 {
-  return static_cast< float >(
+#ifdef DEBUG
+  float retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<float>(x1),
                                static_cast<float>(x2),
                                static_cast<float>(x_1),
                                static_cast<float>(x_2),
                                static_cast<float>(x_3),
                                static_cast<float>(x_4),
-                               static_cast<float>(y_1),
-                               static_cast<float>(y_2),
-                               static_cast<float>(y_3),
-                               static_cast<float>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline double interpolate( uint16_t x1, uint16_t x2,
                             uint16_t x_1, uint16_t x_2, uint16_t x_3, uint16_t x_4,
                             double y_1, double y_2, double y_3, double y_4 )
 {
-  return static_cast< double >(
+#ifdef DEBUG
+  double retval = (
+#else
+  return 
+#endif
                   interpolate( static_cast<double>(x1),
                                static_cast<double>(x2),
                                static_cast<double>(x_1),
                                static_cast<double>(x_2),
                                static_cast<double>(x_3),
                                static_cast<double>(x_4),
-                               static_cast<double>(y_1),
-                               static_cast<double>(y_2),
-                               static_cast<double>(y_3),
-                               static_cast<double>(y_4) ));
+                               y_1,
+                               y_2,
+                               y_3,
+                               y_4 )
+#ifndef DEBUG
+                             ;
+#else
+  return retval;
+#endif
 }
 
 
+template<>
 inline int8_t interpolate( Fix16 x1, Fix16 x2,
                             Fix16 x_1, Fix16 x_2, Fix16 x_3, Fix16 x_4,
                             int8_t y_1, int8_t y_2, int8_t y_3, int8_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(
+#endif
+                  interpolate( x1,
+                               x2,
+                               x_1,
+                               x_2,
+                               x_3,
+                               x_4,
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<int8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline uint8_t interpolate( Fix16 x1, Fix16 x2,
                             Fix16 x_1, Fix16 x_2, Fix16 x_3, Fix16 x_4,
                             uint8_t y_1, uint8_t y_2, uint8_t y_3, uint8_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
-                               static_cast<Fix16>(y_1),
-                               static_cast<Fix16>(y_2),
-                               static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(
+#endif
+                  interpolate( x1,
+                               x2,
+                               x_1,
+                               x_2,
+                               x_3,
+                               x_4,
+                               static_cast<Fix16>(static_cast<int16_t>(y_1)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_2)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_3)),
+                               static_cast<Fix16>(static_cast<int16_t>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint8_t>(static_cast<int16_t>(retval));
+#endif
 }
 
 
+template<>
 inline int16_t interpolate( Fix16 x1, Fix16 x2,
                             Fix16 x_1, Fix16 x_2, Fix16 x_3, Fix16 x_4,
                             int16_t y_1, int16_t y_2, int16_t y_3, int16_t y_4 )
 {
-  return static_cast< int16_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<int16_t>(
+#endif
+                  interpolate( x1,
+                               x2,
+                               x_1,
+                               x_2,
+                               x_3,
+                               x_4,
                                static_cast<Fix16>(y_1),
                                static_cast<Fix16>(y_2),
                                static_cast<Fix16>(y_3),
-                               static_cast<Fix16>(y_4) ));
+                               static_cast<Fix16>(y_4) )
+#ifndef DEBUG
+                             );
+#else
+  return static_cast<int16_t>(retval);
+#endif
 }
 
 
+template<>
 inline uint16_t interpolate( Fix16 x1, Fix16 x2,
                             Fix16 x_1, Fix16 x_2, Fix16 x_3, Fix16 x_4,
                             uint16_t y_1, uint16_t y_2, uint16_t y_3, uint16_t y_4 )
 {
-  return static_cast< int32_t >(
-                  interpolate( static_cast<Fix16>(x1),
-                               static_cast<Fix16>(x2),
-                               static_cast<Fix16>(x_1),
-                               static_cast<Fix16>(x_2),
-                               static_cast<Fix16>(x_3),
-                               static_cast<Fix16>(x_4),
+#ifdef DEBUG
+  Fix16 retval = (
+#else
+  return static_cast<uint16_t>(static_cast<float>(
+#endif
+                  interpolate( x1,
+                               x2,
+                               x_1,
+                               x_2,
+                               x_3,
+                               x_4,
                                static_cast<Fix16>(static_cast<float>(y_1)),
                                static_cast<Fix16>(static_cast<float>(y_2)),
                                static_cast<Fix16>(static_cast<float>(y_3)),
-                               static_cast<Fix16>(static_cast<float>(y_4)) ));
+                               static_cast<Fix16>(static_cast<float>(y_4)) )
+#ifndef DEBUG
+                             ));
+#else
+  return static_cast<uint16_t>(static_cast<float>(retval));
+#endif
 }
 
 
diff --git a/src/src.ino b/src/src.ino
index d616e2b..6c7fbe9 100644
--- a/src/src.ino
+++ b/src/src.ino
@@ -20,12 +20,15 @@
 
 #include "Arduino.h"
 #include "Map2D3D.h"
+#include "ExtendedSerial.h"
 
 //#include <fix16.h>
 #include <fix16.hpp>
 
 #define BAUDRATE    115200
 
+ExtendedSerial serial(Serial);
+
 /* http://www.gammon.com.au/progmem 
  *  
  *  AVR Libc, and therefore Arduino provides several standard string and memory 
@@ -51,7 +54,7 @@ const int8_t   ys[] PROGMEM = {-127,     -50,  127,    0,    10,   -30,   -50,
 const byte    ysb[] PROGMEM = {   0,      30,   55,   89,    99,   145,   255,   10 };
 
 
-#define USEPROGMEM
+//#define USEPROGMEM
 // In order to use program memory for Fix16's directly, we need to cast to/from int32_t 
 
 #ifdef USEPROGMEM    
@@ -94,7 +97,7 @@ const float tuningMap[256] PROGMEM =
 void initSerial()
 {
     // Open serial port with a baud rate of BAUDRATE b/s
-    Serial.begin(BAUDRATE);
+    serial.begin(BAUDRATE);
 }
 
 
@@ -102,18 +105,9 @@ void setup()
 {
     initSerial();
 
-    Serial.println();
-    Serial.println( F("Hi there") );
-
-    dprint( INT8_MIN );
-    dprint( INT8_MAX );
-    dprint( INT16_MIN );
-    dprint( INT16_MAX );
-    dprint( INT32_MIN );
-    dprint( INT32_MAX );
+    serial.println();
+    serial.println( F("Hi there") );
 
-    
-/*
     Map2D<8, int16_t, int8_t>  test;
     test.setXs_P(xs);
     test.setYs_P(ys);
@@ -121,9 +115,9 @@ void setup()
     for( int idx=250; idx<2550; idx+=50)
     {
       int8_t val = test.f(idx);
-      Serial.print(idx);
-      Serial.print( F(": ") );
-      Serial.println( (int)val );
+      serial.print(idx);
+      serial.print( F(": ") );
+      serial.println( (int)val );
     }
 
 
@@ -134,9 +128,9 @@ void setup()
     for( int idx=250; idx<2550; idx+=50)
     {
       byte val = testb.f(idx);
-      Serial.print(idx);
-      Serial.print( F(": ") );
-      Serial.println( (int)val );
+      serial.print(idx);
+      serial.print( F(": ") );
+      serial.println( (int)val );
     }
 
 
@@ -154,14 +148,14 @@ void setup()
 
     for( int idx=0; idx<8; idx++)
     {
-      Serial.print(idx);
-      Serial.print( F(": ") );
-      Serial.print( (float)ysf[idx] );
-      Serial.print( F(": ") );
+      serial.print(idx);
+      serial.print( F(": ") );
+      serial.print( ysf[idx] );
+      serial.print( F(": ") );
 #ifdef USEPROGMEM       
-      Serial.println();
+      serial.println();
 #else
-      Serial.println( ysf[idx].value, HEX );
+      serial.println( ysf[idx].value, HEX );
 #endif
     }
 
@@ -169,9 +163,9 @@ void setup()
     for( int idx=250; idx<2550; idx+=50)
     {
       Fix16 val = testFix16.f(idx);
-      Serial.print(idx);
-      Serial.print( F(": ") );
-      Serial.println( (float)val );
+      serial.print(idx);
+      serial.print( F(": ") );
+      serial.println( val );
     }
 
 
@@ -190,13 +184,12 @@ void setup()
     {
       Fix16 val = testFix16FromFloat.f(idx);
       Fix16 val2 = testFix16.f(idx);
-      Serial.print(idx);
-      Serial.print( F(": ") );
-      Serial.print( (float)val );
-      Serial.print( F(", ") );
-      Serial.println( (float)val2 );
+      serial.print(idx);
+      serial.print( F(": ") );
+      serial.print( val );
+      serial.print( F(", ") );
+      serial.println( val2 );
     }
-*/
  
 }
 
diff --git a/src/utils.py b/src/utils.py
new file mode 100644
index 0000000..ab65b1f
--- /dev/null
+++ b/src/utils.py
@@ -0,0 +1,39 @@
+
+sep = '//-----------------------------------------------------------------------------'
+
+prog_header = sep + '''\n//
+// Copyright (C) 2018 Arend Lammertink
+//
+// This program is free software; you can redistribute it and/or modify it
+// under the terms of the GNU General Public License as published by the Free
+// Software Foundation, version 3.
+//
+// This program 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 General Public License for
+// more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//-----------------------------------------------------------------------------
+'''
+
+inc_header = sep + '''\n//
+// Copyright (C) 2018 Arend Lammertink
+//
+// 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, version 3.
+//
+// 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, see <http://www.gnu.org/licenses/>.
+//
+//-----------------------------------------------------------------------------
+'''
+
+