#include <stdio.h>
#include <libplayerc/playerc.h>

typedef struct {
	playerc_client_t *client;
	playerc_position2d_t *position2d;
	playerc_laser_t *laser;
	char tabela[16]; //aqui vai a tabela... n sei o tamanho certo dela
	int fitness; //opcional, depende de como vc implementar
} individuo;

int inicia_individuo(individuo* p, char* host, int porta) {
	p->client = playerc_client_create(NULL, host, porta);
	if (playerc_client_connect(p->client) != 0)
		return -1;

	p->position2d = playerc_position2d_create(p->client, 0);
	if (playerc_position2d_subscribe(p->position2d, PLAYERC_OPEN_MODE) != 0)
	{
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}

	p->laser = playerc_laser_create(p->client, 0);
	if (playerc_laser_subscribe(p->laser, PLAYERC_OPEN_MODE))
		return -1;

	//ativa motores
	playerc_position2d_enable(p->position2d, 1);

	return 0; //ok
}

void destroi_individuo(individuo* p) {
	playerc_position2d_unsubscribe(p->position2d);
	playerc_position2d_destroy(p->position2d);
	playerc_laser_destroy(p->laser);
	playerc_client_disconnect(p->client);
	playerc_client_destroy(p->client);

	p->position2d = NULL;
	p->client = NULL;
	p->laser = NULL;
}

int main(int argc, const char **argv)
{
	int i;

	individuo p[3]; //3 robos

	//inicializa todos eles
	for (i=0;i<3;i++) {
		inicia_individuo(&p, "127.0.0.1", 6665+i);
	}

	while(1)
	{
		for (i=0; i<3; i++) {
			//controla cada robo
		    playerc_client_read(p[i]->client);
			//<FAZER>Controle</FAZER>
		  playerc_position2d_set_cmd_vel(p[i]->position2d, 3.0, 0, 0.0, 1);
		
		}
	}

	for (i=0;i<3;i++) {
		destroi_individuo(&p);
	}

  return 0;
}