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

using namespace PlayerCc;

int main(int argc, char *argv[])
{

	PlayerClient robot("localhost");
	Position2dProxy pos(&robot, 0);

	pos.SetMotorEnable(true); // Not needed for simulation, but needed for 
							  // real robots.

	double xtarget = -2.0;
	double ytarget = -1.0;
	double theta_target = 0.0;
	double distance = hypot(xtarget - pos.GetXPos(), ytarget - pos.GetYPos());

	pos.GoTo(xtarget, ytarget, theta_target); // Go forth, little robot.

	while (distance > 0.0625 && !pos.GetStall()) {
		robot.Read(); // Get the latest data from the server.
		distance = hypot(xtarget - pos.GetXPos(), ytarget - pos.GetYPos());
	}

	if (pos.GetStall()) { // If the wheels stopped turning ...
		printf("Problem: robot motors have stalled.\n");
	} else {
		printf("Robot has reached target.\n");
	}

	pos.SetSpeed(0, 0, 0); // Stop the robot
}