diff --git a/hpglparser.cpp b/hpglparser.cpp
index 178cd16..88a5e19 100644
--- a/hpglparser.cpp
+++ b/hpglparser.cpp
@@ -4,121 +4,145 @@
 
 #include "hpglparser.h"
 
-String buffer = "";
-unsigned int bufferPosition = 0;
+HpglParser::HpglParser(Plotter* plotter): _plotter(plotter)
+{
+}
 
-bool isWhitespace(const char ch)
+bool HpglParser::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)
+bool HpglParser::isSeparator(char ch)
 {
 	return isWhitespace(ch) || ch==',';
 }
 
-bool readSeparator()
+void HpglParser::findCoordinats()
 {
-	bool b = false;
-	while(bufferPosition<buffer.length() && isSeparator(buffer[bufferPosition]))
-	{
-		b = true;
-		bufferPosition++;
-	}
-	return b;
+    bool wasPAM = (cmdMode == PA_M);
+    if(buffer[bufferIndex] == ';') cmdMode = 0; 
+    
+    buffer[bufferIndex] = '\0';
+    if(!xSet)
+    {
+        _nextPoint.x=atoi(buffer);
+        if(cmdMode != 0) xSet = true;
+        bufferIndex = -1;
+    }
+    else
+    {
+        _nextPoint.y=atoi(buffer);
+        if(wasPAM) _plotter->moveto(&_nextPoint);
+        else 
+        {
+            Point currentPos = _plotter->getCurrentPos();
+            _plotter->moveto(currentPos.x + _nextPoint.x, currentPos.y + _nextPoint.y);
+        }
+        xSet = false;
+        bufferIndex = -1;
+    }
 }
 
-bool tryReadPoint(Point *pt)
+void HpglParser::findCommand()
 {
-	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;
+    switch (buffer[bufferIndex])
+    {
+        case 'I': break;
+        case 'N':
+        {
+            cmdMode = SKIP_M;
+            bufferIndex = -1;
+            break;
+        }
+        case 'P':
+        {
+            if(buffer[bufferIndex-1] == 'S') 
+            { 
+                cmdMode = SKIP_M;
+                bufferIndex = -1;
+            }
+            else if(buffer[bufferIndex-1] == 'B') 
+            { 
+                _plotter->moveto(0,0);
+                cmdMode = SKIP_M;
+                bufferIndex = -1;
+            }
+            break;
+        }
+        case 'U':
+        {
+            if(buffer[bufferIndex-1] == 'P') 
+            { 
+                cmdMode = PA_M;
+                _plotter->pu();
+                bufferIndex = -1;
+            }
+            break;
+        }
+        case 'D':
+        {
+            if(buffer[bufferIndex-1] == 'P') 
+            { 
+                cmdMode = PA_M;
+                _plotter->pd();
+                bufferIndex = -1;
+            }
+            break;
+        }
+        case 'A':
+        {
+            if(buffer[bufferIndex-1] == 'P') 
+            { 
+                cmdMode = PA_M;
+                bufferIndex = -1;
+            }
+            else cmdMode = ERROR_M;
+            break;
+        }
+        case 'R':
+        {
+            if(buffer[bufferIndex-1] == 'P') 
+            { 
+                cmdMode = PR_M;
+                bufferIndex = -1;
+            }
+            else cmdMode = ERROR_M;
+            break;
+        }
+    }
 }
 
-String readHpglCmd()
+int HpglParser::add(const char ch)
 {
-	while(bufferPosition<buffer.length() && !isalpha(buffer[bufferPosition]))
-	{
-		bufferPosition++;
-	}
-	String cmd = buffer.substring(bufferPosition, bufferPosition + 2);
-	bufferPosition+=2;
-	return cmd;
+    buffer[bufferIndex]=ch;
+
+    //sense
+    
+    if(cmdMode == 0 && bufferIndex > 0 && !isWhitespace(ch) )
+    {
+        findCommand();
+    }
+    else if((cmdMode == PA_M || cmdMode == PR_M) && (isSeparator(ch) || ch==';') )
+    {
+        findCoordinats();
+    }
+    else if(((cmdMode == SKIP_M || cmdMode == ERROR_M) && ch==';') || (cmdMode == 0 && ch==';')  )
+    {
+        xSet=false;
+        bufferIndex = -1;
+        cmdMode=0;
+    }
+    
+    bufferIndex++;
+    
+    if(bufferIndex > 64) 
+    {
+        xSet=false;
+        bufferIndex = 0;
+        cmdMode = 0;
+        return 2;
+    }
+    return (bufferIndex == 0 && cmdMode == 0) ? 1 : 0;
 }
 
-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
index 84e8513..eadd598 100644
--- a/hpglparser.h
+++ b/hpglparser.h
@@ -3,18 +3,39 @@
 #ifndef _HPGLPARSER_h
 #define _HPGLPARSER_h
 
-#include "WString.h"
 #include "point.h"
-#include <stdio.h>
+#include "plotter.h"
+
+#define PA_M 1
+#define PR_M  2
+#define SKIP_M  3
+#define ERROR_M 4
+
+class HpglParser
+{
+private:
+    Plotter* _plotter;
+    char buffer[64];
+    int bufferIndex = 0;
+    
+    uint8_t cmdMode = 0;
+    uint8_t searchMode = 0;
+    
+    bool xSet = false;
+    Point _nextPoint;
+    
+    bool isWhitespace(const char ch);
+    bool isSeparator(char ch);
+    
+    void findCommand();
+    void findCoordinats();
+    
+public:
+    HpglParser(Plotter* plotter);
+    ~HpglParser(){}
+    
+    int add(const char ch);
+};
 
-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
index e335704..65f85ce 100644
--- a/main.cpp
+++ b/main.cpp
@@ -3,127 +3,34 @@
 #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;
+    Serial serial;
     
-    Plotter plotter(&PORTB, PB4, &serial);
-    serial.putString("UVOS plotter interface\n");
+    
+    Plotter plotter(&PORTB, PB4);
+    HpglParser parser(&plotter);
+    
+    //serial.putString("UVOS plotter interface\n");
     while(true)
     {
-        //plotter.demo();
         if(serial.dataIsWaiting())
         {
             char ch = serial.getChar();
-            if(ch != '\0')
+            int parseRetrun = parser.add(ch);
+            if(parseRetrun == 1) 
             {
-                addToBuffer(ch);
-                
+                serial.putString("ok\n");
             }
-            if (ch==';')
+            else if(parseRetrun == 2) 
             {
-                    while(processCmd(&serial, &plotter));
-                    purgeBuffer();
-                    serial.putChar('\n');
+                serial.putString("overrun!, ok\n");
             }
         }
     }
diff --git a/plotter.cpp b/plotter.cpp
index 63a2a75..41f3bd0 100644
--- a/plotter.cpp
+++ b/plotter.cpp
@@ -1,11 +1,10 @@
 #include "plotter.h"
 
-const int SCALEX=5;
-const int SCALEY=5;
+const int SCALEX=2;
+const int SCALEY=3;
 
-Plotter::Plotter(volatile unsigned char *penPort,  const char penPin, Serial* serial): _pwm( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001)
+Plotter::Plotter(volatile unsigned char *penPort,  const char penPin): _pwm( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001)
 {
-    _serial=serial;
     _penPort=penPort;
     _penPin=penPin;
     basicposition();
@@ -34,16 +33,20 @@ void Plotter::basicposition()
 
 void Plotter::pd()
 {
-        _delay_us(1000);
-	writePin(_penPort, _penPin, true);
-	_delay_us(1000);
+        if(!readPin(_penPort, _penPin))
+        {
+            writePin(_penPort, _penPin, true);
+            _delay_us(500);
+        }
 }
 
 void Plotter::pu()
 {
-        _delay_us(1000);
+    if(readPin(_penPort, _penPin))
+    {
 	writePin(_penPort, _penPin, false);
-	_delay_us(1000);
+	_delay_us(500);
+    }
 }
 
 void Plotter::moveto(Point *pt)
@@ -56,7 +59,7 @@ 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;
+    int steps = 0;
     int32_t StepSizeX=0;
     int32_t StepSizeY=0;
     
@@ -77,13 +80,20 @@ void Plotter::moveto(const uint16_t nx, const uint16_t ny)
     {
         _pwm.setDutyA(65535-(currentPos.x+i*StepSizeX)*SCALEX);
         _pwm.setDutyB(65535-(currentPos.y+i*StepSizeY)*SCALEY);
-        _delay_us(10000);
+        _delay_us(5000);
     }
 
     
     _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);
+    _delay_us(10000);
+    
+    if(steps < 50) for(int i = 0; i < steps; i++)_delay_us(11000);
+    else if(steps < 5) _delay_us(10000*5);
+    else _delay_us(200000);
+    
+    //_delay_us(500000);
+    //else _delay_us(10000);
     
     currentPos.x = nx;
     currentPos.y = ny;
diff --git a/plotter.h b/plotter.h
index 4c5f64f..a8227aa 100644
--- a/plotter.h
+++ b/plotter.h
@@ -7,9 +7,7 @@
 #include "writepin.h"
 #include "point.h"
 #include "pwm.h"
-#include "serial.h"
 #include "writepin.h"
-#include "serial.h"
 #include <stdio.h>
 #include <stdlib.h> 
 
@@ -21,10 +19,9 @@ private:
     
     volatile unsigned char *_penPort;
     char _penPin;
-    Serial* _serial;
     
 public:
-    Plotter(volatile unsigned char *penPort,  const char penPin, Serial* serial);
+    Plotter(volatile unsigned char *penPort,  const char penPin);
     void demo();
     void basicposition();
     void pd();
diff --git a/serial.cpp b/serial.cpp
index fd4880b..28f567d 100644
--- a/serial.cpp
+++ b/serial.cpp
@@ -1,12 +1,26 @@
 #include "serial.h"
 
 char rxBuffer[BUFFER_SIZE];
-volatile uint16_t interruptIndex = 0;
+volatile uint32_t interruptIndex = 0;
+volatile uint32_t _rxIndex = 0;
+
+bool stopped = false;
 
 ISR (USART_RX_vect) //I have seen worse interrupt sintax
 {
 	rxBuffer[interruptIndex % BUFFER_SIZE] = UDR0;
-	interruptIndex++;
+        /*if (interruptIndex - BUFFER_SIZE > 0 && _rxIndex - BUFFER_SIZE > 0)
+        {
+            interruptIndex -= BUFFER_SIZE;
+            _rxIndex -= BUFFER_SIZE;
+        }*/
+        if(serialFlowControl && !stopped && interruptIndex - _rxIndex > BUFFER_SIZE - 64)
+        {
+            loop_until_bit_is_set(UCSR0A, UDRE0);
+            UDR0 = 0x13;
+            stopped = true;
+        }
+	if(interruptIndex - _rxIndex < BUFFER_SIZE )interruptIndex++;
 }
 
 Serial::Serial() 
@@ -47,10 +61,15 @@ bool Serial::dataIsWaiting()
 
 char Serial::getChar()
 {
-    if( _rxIndex >= (32768) - 2*BUFFER_SIZE ) flush(); //may explode only occasionaly
     if(dataIsWaiting())
     {
         _rxIndex++;
+        if(serialFlowControl && stopped && interruptIndex - _rxIndex < 64)
+        {
+            loop_until_bit_is_set(UCSR0A, UDRE0);
+            UDR0 = 0x11;
+            stopped = false;
+        }
         return rxBuffer[(_rxIndex -1) % BUFFER_SIZE];
     }
     else return '\0';
@@ -58,12 +77,12 @@ char Serial::getChar()
 
 unsigned int Serial::getString(char* buffer, const int bufferLength)
 {
-    int i = 0;
+    unsigned 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;
+       unsigned int j = 0;
        for(; j < i &&  j < bufferLength-1 ; j++)
        {
            buffer[j] = getChar();
@@ -71,12 +90,8 @@ unsigned int Serial::getString(char* buffer, const int bufferLength)
        buffer[j+1]='\0';
        _rxIndex++;
     }
-    else
-    {
-         i = 0;
-         if( _rxIndex >= (32768) - 2*BUFFER_SIZE ) flush();
-    }
-
+    else i = 0;
+    
     if (rxBuffer[(_rxIndex+i) % BUFFER_SIZE] == _terminator) _rxIndex++;
     
     return i;
diff --git a/serial.h b/serial.h
index 810ef8d..63dc090 100644
--- a/serial.h
+++ b/serial.h
@@ -1,19 +1,21 @@
 #ifndef SERIAL_H
 #define SERIAL_H
 
-#define BAUD 38400
-#define BUFFER_SIZE 128
+#define BAUD 19200
+#define BUFFER_SIZE 1024
 
 #include <util/setbaud.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <string.h>
 
+const bool serialFlowControl = false;
+
 class Serial
 {
 private:
     char _terminator = '\n';
-    uint16_t _rxIndex=0;
+    
     
 public:
     Serial();