#include "Aria.h"

int main(int argc, char *argv[])
{
	int status = 0;
	ArRobot robot;
	ArSimpleConnector connector(&argc, argv);
	ArKeyHandler keyHandler;
	ArSonarDevice sonar;
	ArActionStallRecover recover;
	ArActionBumpers bumpers;
	ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
	ArActionAvoidFront avoidFrontFar("Avoid Front Far"); // 450(distance), 200(velocity)
	ArActionConstantVelocity constantVelocity("Constant Velocity", 400); // default velocity

	Aria::init();
	Aria::setKeyHandler(&keyHandler);
	robot.attachKeyHandler(&keyHandler);
	robot.addRangeDevice(&sonar);
	if(connector.connectRobot(&robot))
	{
		printf("Connected to robot... start to do stuffs!\n");
		robot.enableMotors(); //robot.comInt(ArCommands::ENABLE, 1);
		robot.addAction(&recover, 100); // highest priority!
		robot.addAction(&bumpers, 75);
		robot.addAction(&avoidFrontNear, 50);
		robot.addAction(&avoidFrontFar, 49);
		robot.addAction(&constantVelocity, 25); // move if nothing else...
		robot.run(true);
	}
	else
	{
		printf("Could not connect to robot... exiting!\n");
		status = 1;
	}
	Aria::shutdown();

	return status;
}