#include "Plotter.h"
#define TMAX 26
#define CASASDECIMAIS 2

int passos_unidadex,passos_unidadey,passos_unidadez;
int posx,posy,posz;
int pux,puy,puz;

typedef struct {
	char codigo;
	int x,y,z,a,b,c,i,j,k,f;
} COMANDO;

COMANDO cmd;

void passox (bool direcao){
	if(direcao){
		pux++;
		if(pux>=passos_unidadex){
			posx++;
			pux=0;
		}
	} else {
		pux--;
		if(pux<0){
			posx--;
			pux=passos_unidadex-1;
		}
	}
}

void passoy (bool direcao){
	if(direcao){
		puy++;
		if(puy>=passos_unidadey){
			posy++;
			puy=0;
		}
	} else {
		puy--;
		if(puy<0){
			posy--;
			puy=passos_unidadey-1;
		}
	}
}

void passoz (bool direcao){
	if(direcao){
		puz++;
		if(puz>=passos_unidadez){
			posz++;
			puz=0;
		}
	} else {
		puz--;
		if(puz<0){
			posz--;
			puz=passos_unidadez-1;
		}
	}
}


void decodificar_comando(COMANDO *comando){
	switch(comando->codigo){
		case 0:
			while(comando->x != posx ){//&& comando->y!= posy && comando->z != posz){
				if(comando->x > posx)
					passox(1);
				if(comando->x < posx)
					passox(0);
			}

			break;
	}
}

void decodificar_linha(COMANDO *comando, char *linhac){
	char i=0;
	char t=0;
	char variavel;
	int valor;
	bool negativo=false;
	while(linhac[i]){
		switch(linhac[i]){
			case 'G':
				comando->codigo=0;
				if(linhac[i+1]=='-')
					i++;
				while(isdigit(linhac[++i])){
					comando->codigo=comando->codigo * 10;
					comando->codigo=comando->codigo + linhac[i]- '0';
				}
				break;
			default:
				if(isalpha(linhac[i])){
					variavel=linhac[i];
					valor=0;
					negativo=false;
					if(linhac[i+1]=='-'){
						negativo=true;
						i++;
					}
					while(isdigit(linhac[++i])){
						valor *= 10;
						valor=valor + linhac[i] - '0';
					}
					t=0;
					if(linhac[i]=='.'){
						while(isdigit(linhac[++i]) && t<CASASDECIMAIS){
							valor *= 10;
							valor=valor + linhac[i] - '0';
							t++;
						}
					}
					while(t<CASASDECIMAIS){
						valor *= 10;
						t++;
					}
					if(negativo) valor *= -1;
					switch(variavel){
						case 'X':
							comando->x=valor;
							break;
						case 'Y':
							comando->y=valor;
							break;
						case 'Z':
							comando->z=valor;
							break;
						case 'A':
							comando->a=valor;
							break;
						case 'B':
							comando->b=valor;
							break;
						case 'C':
							comando->c=valor;
							break;
						case 'I':
							comando->i=valor;
							break;
						case 'J':
							comando->j=valor;
							break;
						case 'K':
							comando->k=valor;
							break;
						case 'F':
							comando->f=valor;
							break;
					}
				} else {
					i++;
				}
				break;
		}
	}
}

void ler_linha(char *destino){
	char c;
	char i=0;
	bool letranumero=false;
	bool tirazeroaesquerda=true;
	while(i<TMAX){
		c=getc();
		letranumero=isalnum(c) || ispunct(c) || c=='-';
		if(c=='\n')break;
		if(c=='\r')break;
		if(letranumero && !( tirazeroaesquerda && c=='0')){
			destino[i++]=toupper(c);
			tirazeroaesquerda=isalpha(c);
		}
	}
	destino[i]=0;
}

void main(){
	char linha[TMAX+1];
	uart_init(1,25); 
	posx=posy=posz=0;
	passos_unidadex=passos_unidadey=passos_unidadez=1;
	pux=puy=puz=0;
	
	puts("Plotter 1.0\n");
	while(1){ 
		ler_linha(linha);
		puts(linha);
		decodificar_linha(&cmd,linha);


		sprintf(linha,"Comando: %d",cmd.codigo);
		puts(linha);
		sprintf(linha,"X: %d",cmd.x);
		puts(linha);
		sprintf(linha,"Y: %d",cmd.y);
		puts(linha);
		sprintf(linha,"Z: %d",cmd.z);
		puts(linha);
		decodificar_comando(&cmd);

	}
}