#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');
            }
        }
    }
}