commit dd6f37a96079dcf16939517a2e26f7626d1d3b0d
Author: IMback <philipp@uvos.net>
Date:   Thu Sep 14 18:10:46 2017 +0200

    initial

diff --git a/WString.cpp b/WString.cpp
new file mode 100644
index 0000000..f2572d6
--- /dev/null
+++ b/WString.cpp
@@ -0,0 +1,750 @@
+/*
+  WString.cpp - String library for Wiring & Arduino
+  ...mostly rewritten by Paul Stoffregen...
+  Copyright (c) 2009-10 Hernando Barragan.  All rights reserved.
+  Copyright 2011, Paul Stoffregen, paul@pjrc.com
+
+  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
+*/
+
+#include "WString.h"
+
+/*********************************************/
+/*  Constructors                             */
+/*********************************************/
+
+String::String(const char *cstr)
+{
+	init();
+	if (cstr) copy(cstr, strlen(cstr));
+}
+
+String::String(const String &value)
+{
+	init();
+	*this = value;
+}
+
+String::String(const __FlashStringHelper *pstr)
+{
+	init();
+	*this = pstr;
+}
+
+#if __cplusplus >= 201103L || defined(__GXX_EXPERIMENTAL_CXX0X__)
+String::String(String &&rval)
+{
+	init();
+	move(rval);
+}
+String::String(StringSumHelper &&rval)
+{
+	init();
+	move(rval);
+}
+#endif
+
+String::String(char c)
+{
+	init();
+	char buf[2];
+	buf[0] = c;
+	buf[1] = 0;
+	*this = buf;
+}
+
+String::String(unsigned char value, unsigned char base)
+{
+	init();
+	char buf[1 + 8 * sizeof(unsigned char)];
+	utoa(value, buf, base);
+	*this = buf;
+}
+
+String::String(int value, unsigned char base)
+{
+	init();
+	char buf[2 + 8 * sizeof(int)];
+	itoa(value, buf, base);
+	*this = buf;
+}
+
+String::String(unsigned int value, unsigned char base)
+{
+	init();
+	char buf[1 + 8 * sizeof(unsigned int)];
+	utoa(value, buf, base);
+	*this = buf;
+}
+
+String::String(long value, unsigned char base)
+{
+	init();
+	char buf[2 + 8 * sizeof(long)];
+	ltoa(value, buf, base);
+	*this = buf;
+}
+
+String::String(unsigned long value, unsigned char base)
+{
+	init();
+	char buf[1 + 8 * sizeof(unsigned long)];
+	ultoa(value, buf, base);
+	*this = buf;
+}
+
+String::String(float value, unsigned char decimalPlaces)
+{
+	init();
+	char buf[33];
+	*this = dtostrf(value, (decimalPlaces + 2), decimalPlaces, buf);
+}
+
+String::String(double value, unsigned char decimalPlaces)
+{
+	init();
+	char buf[33];
+	*this = dtostrf(value, (decimalPlaces + 2), decimalPlaces, buf);
+}
+
+String::~String()
+{
+	free(buffer);
+}
+
+/*********************************************/
+/*  Memory Management                        */
+/*********************************************/
+
+inline void String::init(void)
+{
+	buffer = NULL;
+	capacity = 0;
+	len = 0;
+}
+
+void String::invalidate(void)
+{
+	if (buffer) free(buffer);
+	buffer = NULL;
+	capacity = len = 0;
+}
+
+unsigned char String::reserve(unsigned int size)
+{
+	if (buffer && capacity >= size) return 1;
+	if (changeBuffer(size)) {
+		if (len == 0) buffer[0] = 0;
+		return 1;
+	}
+	return 0;
+}
+
+unsigned char String::changeBuffer(unsigned int maxStrLen)
+{
+	char *newbuffer = (char *)realloc(buffer, maxStrLen + 1);
+	if (newbuffer) {
+		buffer = newbuffer;
+		capacity = maxStrLen;
+		return 1;
+	}
+	return 0;
+}
+
+/*********************************************/
+/*  Copy and Move                            */
+/*********************************************/
+
+String & String::copy(const char *cstr, unsigned int length)
+{
+	if (!reserve(length)) {
+		invalidate();
+		return *this;
+	}
+	len = length;
+	strcpy(buffer, cstr);
+	return *this;
+}
+
+String & String::copy(const __FlashStringHelper *pstr, unsigned int length)
+{
+	if (!reserve(length)) {
+		invalidate();
+		return *this;
+	}
+	len = length;
+	strcpy_P(buffer, (PGM_P)pstr);
+	return *this;
+}
+
+#if __cplusplus >= 201103L || defined(__GXX_EXPERIMENTAL_CXX0X__)
+void String::move(String &rhs)
+{
+	if (buffer) {
+		if (rhs && capacity >= rhs.len) {
+			strcpy(buffer, rhs.buffer);
+			len = rhs.len;
+			rhs.len = 0;
+			return;
+		} else {
+			free(buffer);
+		}
+	}
+	buffer = rhs.buffer;
+	capacity = rhs.capacity;
+	len = rhs.len;
+	rhs.buffer = NULL;
+	rhs.capacity = 0;
+	rhs.len = 0;
+}
+#endif
+
+String & String::operator = (const String &rhs)
+{
+	if (this == &rhs) return *this;
+	
+	if (rhs.buffer) copy(rhs.buffer, rhs.len);
+	else invalidate();
+	
+	return *this;
+}
+
+#if __cplusplus >= 201103L || defined(__GXX_EXPERIMENTAL_CXX0X__)
+String & String::operator = (String &&rval)
+{
+	if (this != &rval) move(rval);
+	return *this;
+}
+
+String & String::operator = (StringSumHelper &&rval)
+{
+	if (this != &rval) move(rval);
+	return *this;
+}
+#endif
+
+String & String::operator = (const char *cstr)
+{
+	if (cstr) copy(cstr, strlen(cstr));
+	else invalidate();
+	
+	return *this;
+}
+
+String & String::operator = (const __FlashStringHelper *pstr)
+{
+	if (pstr) copy(pstr, strlen_P((PGM_P)pstr));
+	else invalidate();
+
+	return *this;
+}
+
+/*********************************************/
+/*  concat                                   */
+/*********************************************/
+
+unsigned char String::concat(const String &s)
+{
+	return concat(s.buffer, s.len);
+}
+
+unsigned char String::concat(const char *cstr, unsigned int length)
+{
+	unsigned int newlen = len + length;
+	if (!cstr) return 0;
+	if (length == 0) return 1;
+	if (!reserve(newlen)) return 0;
+	strcpy(buffer + len, cstr);
+	len = newlen;
+	return 1;
+}
+
+unsigned char String::concat(const char *cstr)
+{
+	if (!cstr) return 0;
+	return concat(cstr, strlen(cstr));
+}
+
+unsigned char String::concat(char c)
+{
+	char buf[2];
+	buf[0] = c;
+	buf[1] = 0;
+	return concat(buf, 1);
+}
+
+unsigned char String::concat(unsigned char num)
+{
+	char buf[1 + 3 * sizeof(unsigned char)];
+	itoa(num, buf, 10);
+	return concat(buf, strlen(buf));
+}
+
+unsigned char String::concat(int num)
+{
+	char buf[2 + 3 * sizeof(int)];
+	itoa(num, buf, 10);
+	return concat(buf, strlen(buf));
+}
+
+unsigned char String::concat(unsigned int num)
+{
+	char buf[1 + 3 * sizeof(unsigned int)];
+	utoa(num, buf, 10);
+	return concat(buf, strlen(buf));
+}
+
+unsigned char String::concat(long num)
+{
+	char buf[2 + 3 * sizeof(long)];
+	ltoa(num, buf, 10);
+	return concat(buf, strlen(buf));
+}
+
+unsigned char String::concat(unsigned long num)
+{
+	char buf[1 + 3 * sizeof(unsigned long)];
+	ultoa(num, buf, 10);
+	return concat(buf, strlen(buf));
+}
+
+unsigned char String::concat(float num)
+{
+	char buf[20];
+	char* string = dtostrf(num, 4, 2, buf);
+	return concat(string, strlen(string));
+}
+
+unsigned char String::concat(double num)
+{
+	char buf[20];
+	char* string = dtostrf(num, 4, 2, buf);
+	return concat(string, strlen(string));
+}
+
+unsigned char String::concat(const __FlashStringHelper * str)
+{
+	if (!str) return 0;
+	int length = strlen_P((const char *) str);
+	if (length == 0) return 1;
+	unsigned int newlen = len + length;
+	if (!reserve(newlen)) return 0;
+	strcpy_P(buffer + len, (const char *) str);
+	len = newlen;
+	return 1;
+}
+
+/*********************************************/
+/*  Concatenate                              */
+/*********************************************/
+
+StringSumHelper & operator + (const StringSumHelper &lhs, const String &rhs)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(rhs.buffer, rhs.len)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, const char *cstr)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!cstr || !a.concat(cstr, strlen(cstr))) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, char c)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(c)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, unsigned char num)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(num)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, int num)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(num)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, unsigned int num)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(num)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, long num)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(num)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, unsigned long num)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(num)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, float num)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(num)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, double num)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(num)) a.invalidate();
+	return a;
+}
+
+StringSumHelper & operator + (const StringSumHelper &lhs, const __FlashStringHelper *rhs)
+{
+	StringSumHelper &a = const_cast<StringSumHelper&>(lhs);
+	if (!a.concat(rhs))	a.invalidate();
+	return a;
+}
+
+/*********************************************/
+/*  Comparison                               */
+/*********************************************/
+
+int String::compareTo(const String &s) const
+{
+	if (!buffer || !s.buffer) {
+		if (s.buffer && s.len > 0) return 0 - *(unsigned char *)s.buffer;
+		if (buffer && len > 0) return *(unsigned char *)buffer;
+		return 0;
+	}
+	return strcmp(buffer, s.buffer);
+}
+
+unsigned char String::equals(const String &s2) const
+{
+	return (len == s2.len && compareTo(s2) == 0);
+}
+
+unsigned char String::equals(const char *cstr) const
+{
+	if (len == 0) return (cstr == NULL || *cstr == 0);
+	if (cstr == NULL) return buffer[0] == 0;
+	return strcmp(buffer, cstr) == 0;
+}
+
+unsigned char String::operator<(const String &rhs) const
+{
+	return compareTo(rhs) < 0;
+}
+
+unsigned char String::operator>(const String &rhs) const
+{
+	return compareTo(rhs) > 0;
+}
+
+unsigned char String::operator<=(const String &rhs) const
+{
+	return compareTo(rhs) <= 0;
+}
+
+unsigned char String::operator>=(const String &rhs) const
+{
+	return compareTo(rhs) >= 0;
+}
+
+unsigned char String::equalsIgnoreCase( const String &s2 ) const
+{
+	if (this == &s2) return 1;
+	if (len != s2.len) return 0;
+	if (len == 0) return 1;
+	const char *p1 = buffer;
+	const char *p2 = s2.buffer;
+	while (*p1) {
+		if (tolower(*p1++) != tolower(*p2++)) return 0;
+	} 
+	return 1;
+}
+
+unsigned char String::startsWith( const String &s2 ) const
+{
+	if (len < s2.len) return 0;
+	return startsWith(s2, 0);
+}
+
+unsigned char String::startsWith( const String &s2, unsigned int offset ) const
+{
+	if (offset > len - s2.len || !buffer || !s2.buffer) return 0;
+	return strncmp( &buffer[offset], s2.buffer, s2.len ) == 0;
+}
+
+unsigned char String::endsWith( const String &s2 ) const
+{
+	if ( len < s2.len || !buffer || !s2.buffer) return 0;
+	return strcmp(&buffer[len - s2.len], s2.buffer) == 0;
+}
+
+/*********************************************/
+/*  Character Access                         */
+/*********************************************/
+
+char String::charAt(unsigned int loc) const
+{
+	return operator[](loc);
+}
+
+void String::setCharAt(unsigned int loc, char c) 
+{
+	if (loc < len) buffer[loc] = c;
+}
+
+char & String::operator[](unsigned int index)
+{
+	static char dummy_writable_char;
+	if (index >= len || !buffer) {
+		dummy_writable_char = 0;
+		return dummy_writable_char;
+	}
+	return buffer[index];
+}
+
+char String::operator[]( unsigned int index ) const
+{
+	if (index >= len || !buffer) return 0;
+	return buffer[index];
+}
+
+void String::getBytes(unsigned char *buf, unsigned int bufsize, unsigned int index) const
+{
+	if (!bufsize || !buf) return;
+	if (index >= len) {
+		buf[0] = 0;
+		return;
+	}
+	unsigned int n = bufsize - 1;
+	if (n > len - index) n = len - index;
+	strncpy((char *)buf, buffer + index, n);
+	buf[n] = 0;
+}
+
+/*********************************************/
+/*  Search                                   */
+/*********************************************/
+
+int String::indexOf(char c) const
+{
+	return indexOf(c, 0);
+}
+
+int String::indexOf( char ch, unsigned int fromIndex ) const
+{
+	if (fromIndex >= len) return -1;
+	const char* temp = strchr(buffer + fromIndex, ch);
+	if (temp == NULL) return -1;
+	return temp - buffer;
+}
+
+int String::indexOf(const String &s2) const
+{
+	return indexOf(s2, 0);
+}
+
+int String::indexOf(const String &s2, unsigned int fromIndex) const
+{
+	if (fromIndex >= len) return -1;
+	const char *found = strstr(buffer + fromIndex, s2.buffer);
+	if (found == NULL) return -1;
+	return found - buffer;
+}
+
+int String::lastIndexOf( char theChar ) const
+{
+	return lastIndexOf(theChar, len - 1);
+}
+
+int String::lastIndexOf(char ch, unsigned int fromIndex) const
+{
+	if (fromIndex >= len) return -1;
+	char tempchar = buffer[fromIndex + 1];
+	buffer[fromIndex + 1] = '\0';
+	char* temp = strrchr( buffer, ch );
+	buffer[fromIndex + 1] = tempchar;
+	if (temp == NULL) return -1;
+	return temp - buffer;
+}
+
+int String::lastIndexOf(const String &s2) const
+{
+	return lastIndexOf(s2, len - s2.len);
+}
+
+int String::lastIndexOf(const String &s2, unsigned int fromIndex) const
+{
+  	if (s2.len == 0 || len == 0 || s2.len > len) return -1;
+	if (fromIndex >= len) fromIndex = len - 1;
+	int found = -1;
+	for (char *p = buffer; p <= buffer + fromIndex; p++) {
+		p = strstr(p, s2.buffer);
+		if (!p) break;
+		if ((unsigned int)(p - buffer) <= fromIndex) found = p - buffer;
+	}
+	return found;
+}
+
+String String::substring(unsigned int left, unsigned int right) const
+{
+	if (left > right) {
+		unsigned int temp = right;
+		right = left;
+		left = temp;
+	}
+	String out;
+	if (left >= len) return out;
+	if (right > len) right = len;
+	char temp = buffer[right];  // save the replaced character
+	buffer[right] = '\0';	
+	out = buffer + left;  // pointer arithmetic
+	buffer[right] = temp;  //restore character
+	return out;
+}
+
+/*********************************************/
+/*  Modification                             */
+/*********************************************/
+
+void String::replace(char find, char replace)
+{
+	if (!buffer) return;
+	for (char *p = buffer; *p; p++) {
+		if (*p == find) *p = replace;
+	}
+}
+
+void String::replace(const String& find, const String& replace)
+{
+	if (len == 0 || find.len == 0) return;
+	int diff = replace.len - find.len;
+	char *readFrom = buffer;
+	char *foundAt;
+	if (diff == 0) {
+		while ((foundAt = strstr(readFrom, find.buffer)) != NULL) {
+			memcpy(foundAt, replace.buffer, replace.len);
+			readFrom = foundAt + replace.len;
+		}
+	} else if (diff < 0) {
+		char *writeTo = buffer;
+		while ((foundAt = strstr(readFrom, find.buffer)) != NULL) {
+			unsigned int n = foundAt - readFrom;
+			memcpy(writeTo, readFrom, n);
+			writeTo += n;
+			memcpy(writeTo, replace.buffer, replace.len);
+			writeTo += replace.len;
+			readFrom = foundAt + find.len;
+			len += diff;
+		}
+		strcpy(writeTo, readFrom);
+	} else {
+		unsigned int size = len; // compute size needed for result
+		while ((foundAt = strstr(readFrom, find.buffer)) != NULL) {
+			readFrom = foundAt + find.len;
+			size += diff;
+		}
+		if (size == len) return;
+		if (size > capacity && !changeBuffer(size)) return; // XXX: tell user!
+		int index = len - 1;
+		while (index >= 0 && (index = lastIndexOf(find, index)) >= 0) {
+			readFrom = buffer + index + find.len;
+			memmove(readFrom + diff, readFrom, len - (readFrom - buffer));
+			len += diff;
+			buffer[len] = 0;
+			memcpy(buffer + index, replace.buffer, replace.len);
+			index--;
+		}
+	}
+}
+
+void String::remove(unsigned int index){
+	// Pass the biggest integer as the count. The remove method
+	// below will take care of truncating it at the end of the
+	// string.
+	remove(index, (unsigned int)-1);
+}
+
+void String::remove(unsigned int index, unsigned int count){
+	if (index >= len) { return; }
+	if (count <= 0) { return; }
+	if (count > len - index) { count = len - index; }
+	char *writeTo = buffer + index;
+	len = len - count;
+	strncpy(writeTo, buffer + index + count,len - index);
+	buffer[len] = 0;
+}
+
+void String::toLowerCase(void)
+{
+	if (!buffer) return;
+	for (char *p = buffer; *p; p++) {
+		*p = tolower(*p);
+	}
+}
+
+void String::toUpperCase(void)
+{
+	if (!buffer) return;
+	for (char *p = buffer; *p; p++) {
+		*p = toupper(*p);
+	}
+}
+
+void String::trim(void)
+{
+	if (!buffer || len == 0) return;
+	char *begin = buffer;
+	while (isspace(*begin)) begin++;
+	char *end = buffer + len - 1;
+	while (isspace(*end) && end >= begin) end--;
+	len = end + 1 - begin;
+	if (begin > buffer) memcpy(buffer, begin, len);
+	buffer[len] = 0;
+}
+
+/*********************************************/
+/*  Parsing / Conversion                     */
+/*********************************************/
+
+long String::toInt(void) const
+{
+	if (buffer) return atol(buffer);
+	return 0;
+}
+
+float String::toFloat(void) const
+{
+	return float(toDouble());
+}
+
+double String::toDouble(void) const
+{
+	if (buffer) return atof(buffer);
+	return 0;
+}
diff --git a/WString.h b/WString.h
new file mode 100644
index 0000000..77709c3
--- /dev/null
+++ b/WString.h
@@ -0,0 +1,229 @@
+/*
+  WString.h - String library for Wiring & Arduino
+  ...mostly rewritten by Paul Stoffregen...
+  Copyright (c) 2009-10 Hernando Barragan.  All right reserved.
+  Copyright 2011, Paul Stoffregen, paul@pjrc.com
+
+  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
+*/
+
+#ifndef String_class_h
+#define String_class_h
+#ifdef __cplusplus
+
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <avr/pgmspace.h>
+
+// When compiling programs with this class, the following gcc parameters
+// dramatically increase performance and memory (RAM) efficiency, typically
+// with little or no increase in code size.
+//     -felide-constructors
+//     -std=c++0x
+
+class __FlashStringHelper;
+#define F(string_literal) (reinterpret_cast<const __FlashStringHelper *>(PSTR(string_literal)))
+
+// An inherited class for holding the result of a concatenation.  These
+// result objects are assumed to be writable by subsequent concatenations.
+class StringSumHelper;
+
+// The string class
+class String
+{
+	// use a function pointer to allow for "if (s)" without the
+	// complications of an operator bool(). for more information, see:
+	// http://www.artima.com/cppsource/safebool.html
+	typedef void (String::*StringIfHelperType)() const;
+	void StringIfHelper() const {}
+
+public:
+	// constructors
+	// creates a copy of the initial value.
+	// if the initial value is null or invalid, or if memory allocation
+	// fails, the string will be marked as invalid (i.e. "if (s)" will
+	// be false).
+	String(const char *cstr = "");
+	String(const String &str);
+	String(const __FlashStringHelper *str);
+       #if __cplusplus >= 201103L || defined(__GXX_EXPERIMENTAL_CXX0X__)
+	String(String &&rval);
+	String(StringSumHelper &&rval);
+	#endif
+	explicit String(char c);
+	explicit String(unsigned char, unsigned char base=10);
+	explicit String(int, unsigned char base=10);
+	explicit String(unsigned int, unsigned char base=10);
+	explicit String(long, unsigned char base=10);
+	explicit String(unsigned long, unsigned char base=10);
+	explicit String(float, unsigned char decimalPlaces=2);
+	explicit String(double, unsigned char decimalPlaces=2);
+	~String(void);
+
+	// memory management
+	// return true on success, false on failure (in which case, the string
+	// is left unchanged).  reserve(0), if successful, will validate an
+	// invalid string (i.e., "if (s)" will be true afterwards)
+	unsigned char reserve(unsigned int size);
+	inline unsigned int length(void) const {return len;}
+
+	// creates a copy of the assigned value.  if the value is null or
+	// invalid, or if the memory allocation fails, the string will be
+	// marked as invalid ("if (s)" will be false).
+	String & operator = (const String &rhs);
+	String & operator = (const char *cstr);
+	String & operator = (const __FlashStringHelper *str);
+       #if __cplusplus >= 201103L || defined(__GXX_EXPERIMENTAL_CXX0X__)
+	String & operator = (String &&rval);
+	String & operator = (StringSumHelper &&rval);
+	#endif
+
+	// concatenate (works w/ built-in types)
+
+	// returns true on success, false on failure (in which case, the string
+	// is left unchanged).  if the argument is null or invalid, the
+	// concatenation is considered unsucessful.
+	unsigned char concat(const String &str);
+	unsigned char concat(const char *cstr);
+	unsigned char concat(char c);
+	unsigned char concat(unsigned char c);
+	unsigned char concat(int num);
+	unsigned char concat(unsigned int num);
+	unsigned char concat(long num);
+	unsigned char concat(unsigned long num);
+	unsigned char concat(float num);
+	unsigned char concat(double num);
+	unsigned char concat(const __FlashStringHelper * str);
+
+	// if there's not enough memory for the concatenated value, the string
+	// will be left unchanged (but this isn't signalled in any way)
+	String & operator += (const String &rhs)	{concat(rhs); return (*this);}
+	String & operator += (const char *cstr)		{concat(cstr); return (*this);}
+	String & operator += (char c)			{concat(c); return (*this);}
+	String & operator += (unsigned char num)		{concat(num); return (*this);}
+	String & operator += (int num)			{concat(num); return (*this);}
+	String & operator += (unsigned int num)		{concat(num); return (*this);}
+	String & operator += (long num)			{concat(num); return (*this);}
+	String & operator += (unsigned long num)	{concat(num); return (*this);}
+	String & operator += (float num)		{concat(num); return (*this);}
+	String & operator += (double num)		{concat(num); return (*this);}
+	String & operator += (const __FlashStringHelper *str){concat(str); return (*this);}
+
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, const String &rhs);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, const char *cstr);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, char c);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, unsigned char num);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, int num);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, unsigned int num);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, long num);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, unsigned long num);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, float num);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, double num);
+	friend StringSumHelper & operator + (const StringSumHelper &lhs, const __FlashStringHelper *rhs);
+
+	// comparison (only works w/ Strings and "strings")
+	operator StringIfHelperType() const { return buffer ? &String::StringIfHelper : 0; }
+	int compareTo(const String &s) const;
+	unsigned char equals(const String &s) const;
+	unsigned char equals(const char *cstr) const;
+	unsigned char operator == (const String &rhs) const {return equals(rhs);}
+	unsigned char operator == (const char *cstr) const {return equals(cstr);}
+	unsigned char operator != (const String &rhs) const {return !equals(rhs);}
+	unsigned char operator != (const char *cstr) const {return !equals(cstr);}
+	unsigned char operator <  (const String &rhs) const;
+	unsigned char operator >  (const String &rhs) const;
+	unsigned char operator <= (const String &rhs) const;
+	unsigned char operator >= (const String &rhs) const;
+	unsigned char equalsIgnoreCase(const String &s) const;
+	unsigned char startsWith( const String &prefix) const;
+	unsigned char startsWith(const String &prefix, unsigned int offset) const;
+	unsigned char endsWith(const String &suffix) const;
+
+	// character acccess
+	char charAt(unsigned int index) const;
+	void setCharAt(unsigned int index, char c);
+	char operator [] (unsigned int index) const;
+	char& operator [] (unsigned int index);
+	void getBytes(unsigned char *buf, unsigned int bufsize, unsigned int index=0) const;
+	void toCharArray(char *buf, unsigned int bufsize, unsigned int index=0) const
+		{ getBytes((unsigned char *)buf, bufsize, index); }
+	const char* c_str() const { return buffer; }
+	char* begin() { return buffer; }
+	char* end() { return buffer + length(); }
+	const char* begin() const { return c_str(); }
+	const char* end() const { return c_str() + length(); }
+
+	// search
+	int indexOf( char ch ) const;
+	int indexOf( char ch, unsigned int fromIndex ) const;
+	int indexOf( const String &str ) const;
+	int indexOf( const String &str, unsigned int fromIndex ) const;
+	int lastIndexOf( char ch ) const;
+	int lastIndexOf( char ch, unsigned int fromIndex ) const;
+	int lastIndexOf( const String &str ) const;
+	int lastIndexOf( const String &str, unsigned int fromIndex ) const;
+	String substring( unsigned int beginIndex ) const { return substring(beginIndex, len); };
+	String substring( unsigned int beginIndex, unsigned int endIndex ) const;
+
+	// modification
+	void replace(char find, char replace);
+	void replace(const String& find, const String& replace);
+	void remove(unsigned int index);
+	void remove(unsigned int index, unsigned int count);
+	void toLowerCase(void);
+	void toUpperCase(void);
+	void trim(void);
+
+	// parsing/conversion
+	long toInt(void) const;
+	float toFloat(void) const;
+	double toDouble(void) const;
+
+protected:
+	char *buffer;	        // the actual char array
+	unsigned int capacity;  // the array length minus one (for the '\0')
+	unsigned int len;       // the String length (not counting the '\0')
+protected:
+	void init(void);
+	void invalidate(void);
+	unsigned char changeBuffer(unsigned int maxStrLen);
+	unsigned char concat(const char *cstr, unsigned int length);
+
+	// copy and move
+	String & copy(const char *cstr, unsigned int length);
+	String & copy(const __FlashStringHelper *pstr, unsigned int length);
+       #if __cplusplus >= 201103L || defined(__GXX_EXPERIMENTAL_CXX0X__)
+	void move(String &rhs);
+	#endif
+};
+
+class StringSumHelper : public String
+{
+public:
+	StringSumHelper(const String &s) : String(s) {}
+	StringSumHelper(const char *p) : String(p) {}
+	StringSumHelper(char c) : String(c) {}
+	StringSumHelper(unsigned char num) : String(num) {}
+	StringSumHelper(int num) : String(num) {}
+	StringSumHelper(unsigned int num) : String(num) {}
+	StringSumHelper(long num) : String(num) {}
+	StringSumHelper(unsigned long num) : String(num) {}
+	StringSumHelper(float num) : String(num) {}
+	StringSumHelper(double num) : String(num) {}
+};
+
+#endif  // __cplusplus
+#endif  // String_class_h
diff --git a/font.cpp b/font.cpp
new file mode 100644
index 0000000..68543d5
--- /dev/null
+++ b/font.cpp
@@ -0,0 +1,142 @@
+// 
+// 
+// 
+
+#include "font.h"
+
+static uint8_t characters[] = {
+    0x3A,0x5A,0x0A,0xCA, //!
+    0x31,0x7A,0x32,0xFB, //"
+    0x39,0x49,0x0B,0x7B,0x2C,0x68,0x18,0xDC, //#
+    0x10,0x53,0x5C,0x63,0x61,0x68,0x71,0x74,0x3A,0xCA, // $
+    0x7C,0x39,0x70,0x69,0x72,0x79,0x1B,0x52,0x4B,0x54,0xDB, //% 
+    0x0C,0x68,0x70,0x79,0x7A,0x73,0x6B,0x50,0x49,0x4A,0xDC, //&
+    0x3B,0xF2, //'
+    0x3B,0x69,0x59,0xCB,// (
+        0x39,0x6B,0x5B,0xC9, // )
+        0x11,0x73,0x24,0x60,0x31,0xD3, //*
+        0x20,0x64,0x32,0xD2, //+
+        0x12,0xC1, //,
+        0x20,0xE4, //-
+        0x0A,0xCA, // .
+        0xFC, // /
+        0x7C,0x34,0x54,0x4B,0x49,0x50,0x70,0x79,0x7B,0xF4, //0
+        0x29,0x7B,0xCB,
+        0x30,0x79,0x7B,0x74,0x6C,0x48,0xCC,
+        0x30,0x79,0x7B,0x74,0x6C,0x63,0x5C,0x54,0x4B,0x49,0xD0,
+        0x0B,0x7B,0x58,0xDC,
+        0x10,0x49,0x4B,0x54,0x5C,0x63,0x60,0x78,0xFC,
+        0x34,0x7B,0x79,0x70,0x50,0x49,0x4B,0x54,0x5C,0x63,0x61,0xD8,
+        0x7C,0xF8,
+        0x09,0x50,0x58,0x61,0x63,0x6C,0x74,0x7B,0x79,0x70,0x68,0x61,0x63,0x5C,0x54,0x4B,0xC9,
+        0x10,0x49,0x4B,0x54,0x74,0x7B,0x79,0x70,0x68,0x61,0x63,0xEC, //9
+        0x1A,0x5A,0x2A,0xEA, //:
+        0x01,0x52,0x22,0xE2,0x33,0x61,0xD3, //;
+        0x28,0x6C,0x18,0xDC, //<
+        0x31,0x63,0xD1, //=
+        0x30,0x79,0x7B,0x74,0x6C,0x5A,0x52,0x0A,0xCA, //>
+        0x68,0x71,0x73,0x6C,0x54,0x4B,0x4A,0x51,0x61,0x6A,0x63,0xCB, // ?
+        0x7A,0x4C,0x21,0xE3,  // @
+        0x78,0x7B,0x74,0x6C,0x63,0x60,0x63,0x5C,0x54,0x4B,0xC8, //A
+        0x34,0x7B,0x79,0x70,0x50,0x49,0x4B,0xD4,
+        0x78,0x7B,0x74,0x54,0x4B,0xC8,
+        0x3C,0x78,0x48,0x4C,0x20,0xE2,
+        0x78,0x7C,0x20,0xE2,
+        0x34,0x7B,0x79,0x70,0x50,0x49,0x4B,0x54,0x5C,0xDA,
+        0x78,0x3C,0x4C,0x20,0xE4,
+        0x09,0x4B,0x4A,0x7A,0x79,0xFB,
+        0x10,0x49,0x4B,0x54,0xFC,
+        0x78,0x3C,0x60,0xCC,0x38,0x48,0xCC,0x78,0x62,0x7C,0xCC,0x78,0x4C,0xFC,0x09,0x50,0x70,
+        0x79,0x7B,0x74,0x54,0x4B,0xC9,0x78,0x7B,0x74,0x6C,0x63,0xE0,0x09,0x50,0x70,0x79,
+        0x7B,0x74,0x54,0x4B,0x49,0x1A,0xCC,0x78,0x7B,0x74,0x6C,0x63,0x60,0xCC,0x10,0x49,
+        0x4B,0x54,0x5C,0x63,0x61,0x68,0x70,0x79,0x7B,0xF4,0x38,0x7C,0x3A,0xCA,0x38,0x50,
+        0x49,0x4B,0x54,0xFC,0x38,0x4A,0xFC,0x38,0x49,0x6A,0x4B,0xFC,0x7C,0x38,0xCC,0x38,
+        0x62,0x7C,0x22,0xCA,0x38,0x7C,0x48,0xCC,0x3B,0x79,0x49,0xCB,0x38,0xCC,0x39,0x7B,
+        0x4B,0xC9,0x31,0x7A,0xF3,0x00,0xC4,0x39,0xF2,0x29,0x6B,0x64,0x54,0x4B,0x49,0x50,
+        0x59,0x5B,0x54,0xCC,0x78,0x28,0x6B,0x64,0x54,0x4B,0xC8,0x2C,0x69,0x60,0x50,0x49,
+        0xCC,0x3C,0x4C,0x49,0x50,0x60,0x69,0xEC,0x18,0x5C,0x64,0x6B,0x69,0x60,0x50,0x49,
+        0xCC,0x0A,0x72,0x7B,0x7C,0x21,0xE3,0x00,0x43,0x4C,0x6C,0x69,0x60,0x58,0x51,0xD4,
+        0x78,0x28,0x6B,0x64,0xCC,0x09,0x4B,0x4A,0x6A,0x69,0x3A,0xFA,0x01,0x42,0x4B,0x6B,
+        0x6A,0x3B,0xFB,0x78,0x2C,0x58,0xCC,0x09,0x4B,0x4A,0x7A,0xF9,0x68,0x60,0x69,0x62,
+        0x4A,0x22,0x6B,0x64,0xCC,0x68,0x60,0x69,0x6B,0x64,0xCC,0x09,0x50,0x60,0x69,0x6B,
+        0x64,0x54,0x4B,0xC9,0x00,0x68,0x6B,0x64,0x5C,0x53,0xD0,0x04,0x6C,0x69,0x60,0x58,
+        0x51,0xD4,0x09,0x69,0x61,0x6A,0x6B,0xE4,0x4B,0x54,0x5B,0x59,0x60,0x69,0xEC,0x0C,
+        0x4B,0x52,0x7A,0x29,0xEB,0x28,0x50,0x49,0x4B,0x54,0xEC,0x28,0x4A,0xEC,0x28,0x49,
+        0x62,0x4B,0xEC,0x6C,0x28,0xCC,0x00,0x43,0x4C,0x6C,0x28,0x58,0x51,0xD4,0x28,0x6C,
+        0x48,0xCC,0x0B,0x52,0x5A,0x61,0x6A,0x72,0xFB,0x0A,0xFA,0x09,0x52,0x5A,0x63,0x6A,
+        0x72,0xF9,0x20,0x69,0x5B,0xE4,0x80,0x32,0xFB,0x2A,0x71,0x7A,0x73,0xEA,0x39,0x72,
+        0xFB,0x31,0x7A,0xF3,0x39,0x79,0x3B,0xFB                                        
+};
+
+
+bool isFontCharEnd(uint8_t b)
+{
+    return b & 0x80;
+};
+
+int getFontCharOffset(char ch)
+{
+    char actual = '!';			
+    for (int i = 0; i < sizeof(characters); i++)
+    {
+        if (actual == ch)
+            return i;
+        if (isFontCharEnd(characters[i]))
+        {
+            actual++;
+        }
+    }
+    return -1;
+};
+
+void drawFontString(Plotter *plotter, String s, int scale)
+{
+    
+    bool penUp = true;
+    plotter->pu();
+    for (int i = 0; i < s.length(); i++)
+    {
+        char ch = s[i];
+        //LOG(String("character:")+ch);
+        Point savedpos = plotter->getCurrentPos();
+        savedpos.y += scale;
+        if (ch < ' ' || ch > '\x7f')
+        {
+            continue;
+        }
+        if (ch != ' ')
+        {
+            int pos = getFontCharOffset(ch);
+            do
+            {
+                uint8_t instr = characters[pos] & 0x7f;
+                if ((instr & 0x40) > 0)
+                {
+                    if (penUp)
+                    {
+                        plotter->pd();
+                    }
+                    penUp = false;
+                }
+                else
+                {
+                    if (!penUp)
+                    {
+                        plotter->pu();
+                    }
+                    penUp = true;
+                }
+                int nx = instr & 0x7;
+                int ny = (instr & 0x38) >> 3; //absolutni pozice od 0,0
+                nx*=scale;
+                ny*=scale;
+                plotter->moveto(savedpos.x + nx, savedpos.y + ny);
+            } while(!isFontCharEnd(characters[pos++]));
+        }
+        //move
+        penUp = true;
+        plotter->pu();
+        plotter->moveto((6 * scale) + savedpos.x, savedpos.y - scale);
+    }
+};
+
diff --git a/font.h b/font.h
new file mode 100644
index 0000000..a8ba285
--- /dev/null
+++ b/font.h
@@ -0,0 +1,13 @@
+// font.h
+
+#ifndef _FONT_h
+#define _FONT_h
+
+#include "WString.h"
+#include "plotter.h"
+#include "point.h"
+
+void drawFontString(Plotter *plotter, String s, int scale = 6);
+
+#endif
+
diff --git a/hpglparser.cpp b/hpglparser.cpp
new file mode 100644
index 0000000..178cd16
--- /dev/null
+++ b/hpglparser.cpp
@@ -0,0 +1,124 @@
+// 
+// 
+// 
+
+#include "hpglparser.h"
+
+String buffer = "";
+unsigned int bufferPosition = 0;
+
+bool isWhitespace(const char ch)
+{
+    return ch == ' ' || ch == '\n';
+}
+
+void addToBuffer(char ch)
+{
+	buffer += ch;
+}
+
+void setBuffer(String in)
+{
+    buffer = in;
+}
+
+void purgeBuffer()
+{
+	buffer = buffer.substring(bufferPosition);
+	bufferPosition = 0;
+}
+bool isBufferEmpty()
+{
+	return bufferPosition<buffer.length();
+}
+
+bool isBufferFull()
+{
+	return (buffer.length()>40  && (!buffer.indexOf("LB"))>=0);
+}
+
+bool isNumber(char ch)
+{
+	return ch=='-' || isdigit(ch);
+}
+
+void skipWhitespaces()
+{
+	//preskoc non-alpha
+	while(bufferPosition<buffer.length() && isWhitespace(buffer[bufferPosition]))
+	{
+		bufferPosition++;
+	}
+}
+
+bool tryReadInt(uint16_t *i)
+{
+	skipWhitespaces();
+	char ch = buffer[bufferPosition];
+	if (isNumber(ch))
+	{
+		unsigned int p = bufferPosition;
+		while(bufferPosition<buffer.length() && isNumber(buffer[bufferPosition]))
+		{
+			bufferPosition++;
+		}
+		String s = buffer.substring(p,bufferPosition);
+		*i = s.toInt();
+		return true;
+	}
+	return false;
+}
+
+bool isSeparator(char ch)
+{
+	return isWhitespace(ch) || ch==',';
+}
+
+bool readSeparator()
+{
+	bool b = false;
+	while(bufferPosition<buffer.length() && isSeparator(buffer[bufferPosition]))
+	{
+		b = true;
+		bufferPosition++;
+	}
+	return b;
+}
+
+bool tryReadPoint(Point *pt)
+{
+	uint16_t i;
+	if (!tryReadInt(&i))
+		return false;
+	(*pt).x = i;
+	if (!readSeparator())
+		return false;
+	if (!tryReadInt(&i))
+		return false;
+	(*pt).y = i;
+        
+	return true;
+}
+
+String readHpglCmd()
+{
+	while(bufferPosition<buffer.length() && !isalpha(buffer[bufferPosition]))
+	{
+		bufferPosition++;
+	}
+	String cmd = buffer.substring(bufferPosition, bufferPosition + 2);
+	bufferPosition+=2;
+	return cmd;
+}
+
+String readStringUntil(char ch)
+{
+	unsigned int pos=bufferPosition;
+	while(bufferPosition<buffer.length() && buffer[bufferPosition]!=ch)
+	{
+		bufferPosition++;
+	}
+	String result = buffer.substring(pos, bufferPosition);
+	bufferPosition++;
+	return result;
+}
diff --git a/hpglparser.h b/hpglparser.h
new file mode 100644
index 0000000..84e8513
--- /dev/null
+++ b/hpglparser.h
@@ -0,0 +1,20 @@
+// hpglparser.h
+
+#ifndef _HPGLPARSER_h
+#define _HPGLPARSER_h
+
+#include "WString.h"
+#include "point.h"
+#include <stdio.h>
+
+String readHpglCmd();
+bool tryReadPoint(Point *pt);
+bool readSeparator();
+String readStringUntil(char ch);
+void addToBuffer(char ch);
+void setBuffer(String in);
+void purgeBuffer();
+bool isBufferEmpty();
+bool isBufferFull();
+#endif
+
diff --git a/main.cpp b/main.cpp
new file mode 100644
index 0000000..e335704
--- /dev/null
+++ b/main.cpp
@@ -0,0 +1,131 @@
+#include <stdint.h>
+#include <stdio.h>
+#include "point.h"
+#include "hpglparser.h"
+#include "plotter.h"
+#include "font.h"
+#include "serial.h"
+#include "pwm.h"
+#include "WString.h"
+
+
+bool processCmd(Serial *serial, Plotter *plotter)
+{
+	String cmd = readHpglCmd();
+	if (cmd=="DE")
+	{
+		plotter->demo();
+		serial->putString("ok");
+	} 
+	else if (cmd=="BP")
+	{
+		plotter->basicposition();
+		serial->putString("ok");
+	}
+	else if (cmd=="PU")
+	{
+		plotter->pu();
+		Point pt;
+		while(tryReadPoint(&pt))
+		{
+                        char buffer[64];
+                        unsigned int count = snprintf( buffer, 64, "going to X:%u Y:%u\n", pt.x, pt.y );
+                        serial->putString(buffer, count);
+			plotter->moveto(&pt);
+			if (!readSeparator())
+				break;
+		}
+		serial->putString("ok");
+	}
+	else if (cmd=="PD")
+	{
+		plotter->pd();
+		Point pt;
+		while(tryReadPoint(&pt))
+		{
+                        char buffer[64];
+                        unsigned int count = snprintf( buffer, 64, "going to X:%u Y:%u\n", pt.x, pt.y );
+                        serial->putString(buffer, count);
+			plotter->moveto(&pt);
+			if (!readSeparator())
+				break;
+		}
+		serial->putString("ok");
+	}
+	else if (cmd=="PA")
+	{
+		Point pt;
+		while(tryReadPoint(&pt))
+		{
+                        char buffer[64];
+                        unsigned int count = snprintf( buffer, 64, "going to X:%u Y:%u\n", pt.x, pt.y );
+                        serial->putString(buffer, count);
+			plotter->moveto(&pt);
+			if (!readSeparator())
+				break;
+		}
+		serial->putString("ok");		
+	}	
+	else if (cmd=="PR")
+	{
+		Point pt;
+		while(tryReadPoint(&pt))
+		{
+			Point currentPos = plotter->getCurrentPos();
+			plotter->moveto(currentPos.x + pt.x, currentPos.y + pt.y);			
+			if (!readSeparator())
+				break;
+		}
+		serial->putString("ok");		
+	}
+	else if (cmd=="LB")
+	{
+		//LOG("label detected");
+		String str = readStringUntil('x');
+		//TODO: scaling
+		drawFontString(plotter, str, 6);
+		serial->putString("ok");
+	}
+	else if (cmd=="" || "IN" || "SP" )
+	{
+		//skip empty command
+	}
+	else 
+	{
+		serial->putString("ERR ");
+		serial->putString(cmd.begin(), cmd.length());
+                serial->putChar(';');
+	}
+	return isBufferEmpty();
+}
+
+int main()
+{
+    DDRB =  0xFF; 
+    
+    sei();
+    Serial  serial;
+    
+    Plotter plotter(&PORTB, PB4, &serial);
+    serial.putString("UVOS plotter interface\n");
+    while(true)
+    {
+        //plotter.demo();
+        if(serial.dataIsWaiting())
+        {
+            char ch = serial.getChar();
+            if(ch != '\0')
+            {
+                addToBuffer(ch);
+                
+            }
+            if (ch==';')
+            {
+                    while(processCmd(&serial, &plotter));
+                    purgeBuffer();
+                    serial.putChar('\n');
+            }
+        }
+    }
+}
+
diff --git a/plotter.cpp b/plotter.cpp
new file mode 100644
index 0000000..63a2a75
--- /dev/null
+++ b/plotter.cpp
@@ -0,0 +1,91 @@
+#include "plotter.h"
+
+const int SCALEX=5;
+const int SCALEY=5;
+
+Plotter::Plotter(volatile unsigned char *penPort,  const char penPin, Serial* serial): _pwm( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001)
+{
+    _serial=serial;
+    _penPort=penPort;
+    _penPin=penPin;
+    basicposition();
+}
+
+Point Plotter::getCurrentPos()
+{
+	return currentPos;
+}
+
+
+void Plotter::demo()
+{
+	moveto(65535,0);
+	moveto(65535,42129);
+	moveto(0,42129);
+	moveto(0,0);
+	pd();
+        pu();
+}
+
+void Plotter::basicposition()
+{
+	moveto(0, 0);
+}
+
+void Plotter::pd()
+{
+        _delay_us(1000);
+	writePin(_penPort, _penPin, true);
+	_delay_us(1000);
+}
+
+void Plotter::pu()
+{
+        _delay_us(1000);
+	writePin(_penPort, _penPin, false);
+	_delay_us(1000);
+}
+
+void Plotter::moveto(Point *pt)
+{
+	moveto((*pt).x,(*pt).y);
+}
+
+void Plotter::moveto(const uint16_t nx, const uint16_t ny)
+{
+    int deltaX = nx - currentPos.x; //-5100
+    int deltaY = ny - currentPos.y; //0
+    
+    int steps;
+    int32_t StepSizeX=0;
+    int32_t StepSizeY=0;
+    
+    if( abs(deltaX) > abs(deltaY))
+    {
+        if(deltaX != 0) StepSizeX = 100*(deltaX/abs(deltaX));// -100
+        if(deltaY != 0) StepSizeY = ((int32_t)abs(deltaY)*100)/abs((int32_t)deltaX)*(deltaY/abs(deltaY));// 0
+        steps = deltaX/StepSizeX;
+    }
+    else
+    {
+        StepSizeX = (((int32_t)abs(deltaX)*100)/abs((int32_t)deltaY))*(deltaX/abs(deltaX));
+        StepSizeY = 100*(deltaY/abs(deltaY));
+        steps = deltaY/StepSizeY;
+    }
+    
+    for( int i = 0; i < steps; i++)
+    {
+        _pwm.setDutyA(65535-(currentPos.x+i*StepSizeX)*SCALEX);
+        _pwm.setDutyB(65535-(currentPos.y+i*StepSizeY)*SCALEY);
+        _delay_us(10000);
+    }
+
+    
+    _pwm.setDutyA(65535-nx*SCALEX);
+    _pwm.setDutyB(65535-ny*SCALEY);
+    if( abs((currentPos.x+steps*StepSizeX) - nx) > 10 || abs((currentPos.y+steps*StepSizeY) - ny) > 10   ) _delay_us(200000);
+    
+    currentPos.x = nx;
+    currentPos.y = ny;
+}
+
diff --git a/plotter.h b/plotter.h
new file mode 100644
index 0000000..4c5f64f
--- /dev/null
+++ b/plotter.h
@@ -0,0 +1,40 @@
+// plotter.h
+
+#ifndef _PLOTTER_h
+#define _PLOTTER_h
+
+#include<util/delay.h>
+#include "writepin.h"
+#include "point.h"
+#include "pwm.h"
+#include "serial.h"
+#include "writepin.h"
+#include "serial.h"
+#include <stdio.h>
+#include <stdlib.h> 
+
+class Plotter
+{
+private:
+    Pwm16b _pwm;
+    Point currentPos = {0, 0};
+    
+    volatile unsigned char *_penPort;
+    char _penPin;
+    Serial* _serial;
+    
+public:
+    Plotter(volatile unsigned char *penPort,  const char penPin, Serial* serial);
+    void demo();
+    void basicposition();
+    void pd();
+    void pu();
+    void moveto(Point *pt);
+    void moveto(uint16_t nx,uint16_t ny);
+
+    Point getCurrentPos();
+};
+
+
+#endif
+
diff --git a/point.h b/point.h
new file mode 100644
index 0000000..f0b18de
--- /dev/null
+++ b/point.h
@@ -0,0 +1,12 @@
+// point.h
+
+#ifndef _POINT_h
+#define _POINT_h
+#include <stdint.h>
+
+typedef struct _Point {
+	uint16_t x;
+	uint16_t y;
+} Point;
+
+#endif
diff --git a/pwm.cpp b/pwm.cpp
new file mode 100644
index 0000000..36e72f0
--- /dev/null
+++ b/pwm.cpp
@@ -0,0 +1,110 @@
+#include "pwm.h"
+
+//16bit
+
+Pwm16b::Pwm16b( volatile unsigned char *timerControlRegisterA, volatile unsigned char *timerControlRegisterB, volatile uint16_t *compareRegisterA, volatile uint16_t *compareRegisterB, volatile uint16_t *inputCaptureRegister, const uint8_t speed, const bool enableA, const bool enableB)
+{
+    
+    _timerControlRegisterA = timerControlRegisterA;
+    _compareRegisterA = compareRegisterA;
+    _compareRegisterB = compareRegisterB;
+    
+    _enableA =enableA;
+    _enableB =enableB;
+    
+    *_timerControlRegisterA = 0x00;
+    *timerControlRegisterB = 0x00;
+    *inputCaptureRegister = 0xFFFF;
+    *timerControlRegisterB |= (1<<WGM13) | (1<<WGM12);
+    *timerControlRegisterB |= 0b00000111 & speed;
+    *_timerControlRegisterA|= (1<<WGM11);
+ 
+    
+    if(_enableA) *_timerControlRegisterA|= (1<<COM1A1);
+    if(_enableB) *_timerControlRegisterA|= (1<<COM1B1);
+    
+    *_compareRegisterA = 0;
+    *_compareRegisterB = 0;
+    
+    on();
+}
+
+void Pwm16b::off()
+{
+    //*_timerControlRegisterA &= 0b00000011;
+}
+
+void Pwm16b::on()
+{
+    off();
+    if(_enableA) *_timerControlRegisterA|=  (1<<COM1A1);
+    if(_enableB) *_timerControlRegisterA|=  (1<<COM1B1);
+}
+
+void Pwm16b::setDutyA(const uint16_t duty)
+{
+    *_compareRegisterA = duty;
+}
+
+void Pwm16b::setDutyB(const uint16_t duty)
+{
+    *_compareRegisterB = duty;
+}
+
+Pwm16b::~Pwm16b()
+{
+   off();
+}
+
+//8bit
+
+Pwm8b::Pwm8b( volatile unsigned char *timerControlRegisterA, volatile unsigned char *timerControlRegisterB, volatile unsigned char *compareRegisterA, volatile unsigned char *compareRegisterB, const uint8_t speed, const bool enableA, const bool enableB)
+{
+    _timerControlRegisterA = timerControlRegisterA;
+    _compareRegisterA = compareRegisterA;
+    _compareRegisterB = compareRegisterB;
+    
+    _enableA =enableA;
+    _enableB =enableB;
+    
+    *_timerControlRegisterA = 0x00;
+    *timerControlRegisterB = 0x00;
+    
+    //fast 8 bit PWM pwm A
+    if(_enableA) *_timerControlRegisterA|= (1<<COM0A1);
+    if(_enableB) *_timerControlRegisterA|=  (1<<COM0B1);
+    
+    *_timerControlRegisterA|= (1<<WGM01) | (1<<WGM00);
+    *timerControlRegisterB |= 0b00000111 & speed;
+    
+    *_compareRegisterA = 0; //0% pwm to start0
+    *_compareRegisterB = 0; //0% pwm to start0
+}
+
+void Pwm8b::off()
+{
+    *_timerControlRegisterA &= 0b00000011;
+}
+
+void Pwm8b::on()
+{
+    off();
+    if(_enableA) *_timerControlRegisterA|= (1<<COM0A1);
+    if(_enableB) *_timerControlRegisterA|=  (1<<COM0B1);
+    
+}
+
+void Pwm8b::setDutyA(const uint8_t duty)
+{
+    *_compareRegisterA = duty;
+}
+
+void Pwm8b::setDutyB(const uint8_t duty)
+{
+    *_compareRegisterB = duty;
+}
+
+Pwm8b::~Pwm8b()
+{
+   off();
+}
diff --git a/pwm.h b/pwm.h
new file mode 100644
index 0000000..1f344c8
--- /dev/null
+++ b/pwm.h
@@ -0,0 +1,43 @@
+#ifndef PWM_H
+#define PWM_H
+
+#include <avr/io.h>
+
+class Pwm16b //TC1 pwm on PB1 & PB2
+{
+private:
+    volatile unsigned char *_timerControlRegisterA; //TCCRxA
+    volatile uint16_t *_compareRegisterA;      //OCRxA
+    volatile uint16_t *_compareRegisterB;      //OCRxB
+    bool _enableA;
+    bool _enableB;
+    
+public:
+  Pwm16b( volatile unsigned char *timerControlRegisterA, volatile unsigned char *timerControlRegisterB, volatile uint16_t *compareRegisterA, volatile uint16_t *compareRegisterB, volatile uint16_t *inputCaptureRegister, const uint8_t speed = 0b00000011, const bool enableA = true, const bool enableB = true);
+  ~Pwm16b();
+  void setDutyA(const uint16_t duty);
+  void setDutyB(const uint16_t duty);
+  void off();
+  void on();
+};
+
+class Pwm8b
+{
+private:
+     volatile unsigned char *_timerControlRegisterA; //TCCRxA
+     volatile unsigned char *_compareRegisterA;      //OCRxA
+     volatile unsigned char *_compareRegisterB;      //OCRxB
+     
+     bool _enableA;
+     bool _enableB;
+     
+public:
+  Pwm8b( volatile unsigned char *timerControlRegisterA, volatile unsigned char *timerControlRegisterB, volatile unsigned char *compareRegisterA, volatile unsigned char *compareRegisterB, const uint8_t speed = 0b00000011, const bool enableA = true, const bool enableB = true);
+  ~Pwm8b();
+  void setDutyA(const uint8_t duty);
+  void setDutyB(const uint8_t duty);
+  void off();
+  void on();
+};
+
+#endif
diff --git a/serial.cpp b/serial.cpp
new file mode 100644
index 0000000..fd4880b
--- /dev/null
+++ b/serial.cpp
@@ -0,0 +1,92 @@
+#include "serial.h"
+
+char rxBuffer[BUFFER_SIZE];
+volatile uint16_t interruptIndex = 0;
+
+ISR (USART_RX_vect) //I have seen worse interrupt sintax
+{
+	rxBuffer[interruptIndex % BUFFER_SIZE] = UDR0;
+	interruptIndex++;
+}
+
+Serial::Serial() 
+{
+    UBRR0H = UBRRH_VALUE;
+    UBRR0L = UBRRL_VALUE;
+    UCSR0C = _BV(UCSZ01) | _BV(UCSZ00); 
+    UCSR0B = _BV(RXEN0) | _BV(TXEN0);   //Enable RX and TX
+    UCSR0B |= (1 << RXCIE0); //Enable Rx interuppt
+}
+
+void Serial::putChar(const char c)
+{
+    loop_until_bit_is_set(UCSR0A, UDRE0);
+    UDR0 = c;
+}
+
+void Serial::putString(const char* in, const unsigned int length)
+{
+    for(unsigned int i = 0; i < length && in[i] != '\0'; i++)
+    {
+        putChar(in[i]);
+    }
+}
+
+void Serial::putString(const char in[])
+{
+    for(unsigned int i = 0; i < strlen(in); i++)
+    {
+        putChar(in[i]);
+    }
+}
+
+bool Serial::dataIsWaiting()
+{
+    return (interruptIndex > _rxIndex);
+}
+
+char Serial::getChar()
+{
+    if( _rxIndex >= (32768) - 2*BUFFER_SIZE ) flush(); //may explode only occasionaly
+    if(dataIsWaiting())
+    {
+        _rxIndex++;
+        return rxBuffer[(_rxIndex -1) % BUFFER_SIZE];
+    }
+    else return '\0';
+}
+
+unsigned int Serial::getString(char* buffer, const int bufferLength)
+{
+    int i = 0;
+    for(; i <= (interruptIndex-_rxIndex) && i <= BUFFER_SIZE && rxBuffer[(_rxIndex+i) % BUFFER_SIZE] != _terminator; i++);
+    
+    if( i < (interruptIndex-_rxIndex) && i > 0)
+    {
+       int j = 0;
+       for(; j < i &&  j < bufferLength-1 ; j++)
+       {
+           buffer[j] = getChar();
+       }
+       buffer[j+1]='\0';
+       _rxIndex++;
+    }
+    else
+    {
+         i = 0;
+         if( _rxIndex >= (32768) - 2*BUFFER_SIZE ) flush();
+    }
+
+    if (rxBuffer[(_rxIndex+i) % BUFFER_SIZE] == _terminator) _rxIndex++;
+    
+    return i;
+}
+
+void Serial::flush()
+{
+    _rxIndex = 0;
+    interruptIndex = 0;
+    for(int i = 0; i < BUFFER_SIZE; i++) rxBuffer[i] = ' ';
+}
+
+void Serial::setTerminator(char terminator){_terminator = terminator;}
diff --git a/serial.h b/serial.h
new file mode 100644
index 0000000..810ef8d
--- /dev/null
+++ b/serial.h
@@ -0,0 +1,31 @@
+#ifndef SERIAL_H
+#define SERIAL_H
+
+#define BAUD 38400
+#define BUFFER_SIZE 128
+
+#include <util/setbaud.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <string.h>
+
+class Serial
+{
+private:
+    char _terminator = '\n';
+    uint16_t _rxIndex=0;
+    
+public:
+    Serial();
+    void putChar(const char c);
+    void putString(const char* in, const unsigned int length);
+    void putString(const char in[]);
+    bool dataIsWaiting();
+    char getChar();
+    unsigned int getString(char* buffer, const int bufferLength);
+    void flush();    
+    void setTerminator(const char terminator);
+};
+
+#endif
+
diff --git a/writepin.h b/writepin.h
new file mode 100644
index 0000000..4ef214f
--- /dev/null
+++ b/writepin.h
@@ -0,0 +1,13 @@
+#ifndef WRITEPIN_H
+#define WRITEPIN_H
+#include <avr/io.h>
+
+inline void writePin(volatile unsigned char *port, const unsigned char pin, const bool state) //waste 2 cycles
+{
+    *port  &= ~(1 << pin);
+    if(state) *port |= (1 << pin);
+}
+
+inline bool readPin( volatile unsigned char *inPort, const unsigned char pin){ return (bool) (*inPort & (1 << pin));}
+
+#endif