#include <iostream>
#include <string>
#include "SerialPort.h"
#include "message.h"
#include "data.h"

int main (int argc, char *argv[])
{
	if(argc < 3)
	{
		cout << "Argumentos insuficientes!" << endl;
		return -1;
	}

	Message port;
	int i;
	signed char joint[20];
	signed char val[3];

	if(port.initcom() < 0)
		return 0;
	
	cout << "1\n" << endl;
	port.testcom();
	
	port.readcanstate();

//	int j=0;
//	while(j < 9)
//	{
	port.readjoint(joint, (unsigned char)8, (unsigned char)0);
//	j++;
//	}
	
	//activacao dos PWM
	for( i=0;i<3;i++)
	{
		val[i]=0;
	}
	val[0]=1;
	port.applyjoint((unsigned char)8, (unsigned char)3, val);
	
	// activacao das velocidades 
	for( i=0;i<3;i++)
	{
		val[i]=10;
	}
	val[2]=0;
	port.applyjoint((unsigned char)8, (unsigned char)1, val);
	
	// ir para a posicao inicial 
/*	for( i=0;i<3;i++)
	{
		val[i]=20;
	}*/
	val[0]=atoi(argv[1]);
	val[1]=atoi(argv[2]);
	val[2]=0;
	port.applyjoint((unsigned char)8, (unsigned char)0, val);
//	}
//	port.readjoint(&joint, HEAD,PARAM_POSITION);
//	cout << "Joint: " << joint << endl;

	port.killcom();

	return EXIT_SUCCESS;
}