/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

/* This is the ArNetworking example client. 
 * It connects to an ArNetworking server, and provides continuous 
 * information about the robot state (position, speed, server mode, etc.),
 * and, if the server supports the commands, lets you drive the robot with
 * the keyboard.  
 *
 * To see the example client in action, first run a server on the robot's 
 * onboard computer, for example, serverDemo, testServer, guiServer (from ARNL),
 * or ARAM. Make sure the robot computer is on a network, and run this
 * clientDemo with the hostname of the robot computer as an argument:
 *
 *    ./clientDemo -host myrobot
 *
 */

/*
   This version modified 10/10 to include joystick control and stereoScan command.
   This version modified 6/11 to include parameters for stereoscan and multiple robot

 */


#include <time.h>

#include "Aria.h"
#include "ArNetworking.h"

#define MAX_ROBOTS 10 /* max num of robots allowed*/

bool G_quiet = false;

ArJoyHandler myJoyHandler; // joystick handler, should be in inputHandler I suppose



/* This class provides callback functions which are called by the ArKeyHandler
 * object when the user presses keys. These callback functions adjust member
 * variables.  The sendInput() method sends a command with those values
 * to the server using the "ratioDrive" request.
 */

class InputHandler
{
public: 
  /** 
   * @param client  Our client networking object 
   * @param keyHandler  Key handler to register command callbacks with
   */
  InputHandler(ArClientBase *client, ArKeyHandler *keyHandler);
  virtual ~InputHandler(void);

  /// Up arrow key handler: drive the robot forward
  void up(void);

  /// Down arrow key handler: drive the robot backward
  void down(void);

  /// Left arrow key handler: turn the robot left 
  void left(void);

  /// Right arrow key handler: turn the robot right
  void right(void);

  /// Send drive request to the server with stored values
  void sendInput(void);

  /// Send a request to enable "safe drive" mode on the server
  void safeDrive();

  /// Send a request to disable "safe drive" mode on the server
  void unsafeDrive();

  void listData();

  void logTrackingTerse();
  void logTrackingVerbose();
  void resetTracking();

  /* dml 6/10/11 */
  void   setPanParameters(); // set the pan increment, num pans,and default tilt
  void   setTiltParameters(); // set the tilt increment, num tits and default pan
  void   setRobotNumber(); // set the robot being controlled
  void   setRobotTo(int n); // set the robot being controlled
  void   addNewRobot(); // open a link to a new robot
  ArClientBase *getClient();
  void   doStereoScan(); // do a scan

  void toggleSalience();
  void enableSalienceMove();
  void doStop();

  /* */

  bool dontCheck;

protected:
  ArClientBase *myClient;
  ArKeyHandler *myKeyHandler;


  int currentRobotNumber;
  int maxRobotSoFar;
  ArClientBase *robotNumberTable[MAX_ROBOTS];

  int panInc, numPans, defTilt;
  int tiltInc, numTilts, defPan;

  /// Set this to true in the constructor to print out debugging information
  bool myPrinting;

  /// Current translation value (a percentage of the  maximum speed)
  double myTransRatio;

  /// Current rotation ration value (a percentage of the maximum rotational velocity)
  double myRotRatio;

  time_t lastScan;
  time_t sinceScan;

  /** Functor objects, given to the key handler, which then call our handler
   * methods above */
  ///@{
  ArFunctorC<InputHandler> myUpCB;
  ArFunctorC<InputHandler> myDownCB;
  ArFunctorC<InputHandler> myLeftCB;
  ArFunctorC<InputHandler> myRightCB;
  ArFunctorC<InputHandler> mySafeDriveCB;
  ArFunctorC<InputHandler> myUnsafeDriveCB;
  ArFunctorC<InputHandler> myListDataCB;
  ArFunctorC<InputHandler> myLogTrackingTerseCB;
  ArFunctorC<InputHandler> myLogTrackingVerboseCB;
  ArFunctorC<InputHandler> myResetTrackingCB;
  /* dml 6/10/11 */
  ArFunctorC<InputHandler> mySetPanParametersCB;
  ArFunctorC<InputHandler> mySetTiltParametersCB;
  ArFunctorC<InputHandler> myAddNewRobotCB;
  ArFunctorC<InputHandler> mySetRobotNumberCB;
  ArFunctorC<InputHandler> myDoStereoScanCB;

  ArFunctorC<InputHandler> myToggleSalienceCB;
  ArFunctorC<InputHandler> myEnableSalienceMoveCB;
  ArFunctorC<InputHandler> myDoStopCB;
  
  /* */

  ///@}
};

InputHandler::InputHandler(ArClientBase *client, ArKeyHandler *keyHandler) : 
  myClient(client), myKeyHandler(keyHandler),
  myPrinting(false), myTransRatio(0.0), myRotRatio(0.0) ,

  /* Initialize functor objects with pointers to our handler methods: */
  myUpCB(this, &InputHandler::up),
  myDownCB(this, &InputHandler::down),
  myLeftCB(this, &InputHandler::left),
  myRightCB(this, &InputHandler::right),
  mySafeDriveCB(this, &InputHandler::safeDrive),
  myUnsafeDriveCB(this, &InputHandler::unsafeDrive),
  myListDataCB(this, &InputHandler::listData),
  myLogTrackingTerseCB(this, &InputHandler::logTrackingTerse),
  myLogTrackingVerboseCB(this, &InputHandler::logTrackingVerbose),
  myResetTrackingCB(this, &InputHandler::resetTracking),
  /* dml 6/10 /11 */
  mySetPanParametersCB(this, &InputHandler::setPanParameters),
  mySetTiltParametersCB(this, &InputHandler::setTiltParameters),
  myAddNewRobotCB(this, &InputHandler::addNewRobot),
  mySetRobotNumberCB(this, &InputHandler::setRobotNumber),
  myDoStereoScanCB(this,&InputHandler::doStereoScan),

  myToggleSalienceCB(this,&InputHandler::toggleSalience),
  myEnableSalienceMoveCB(this,&InputHandler::enableSalienceMove),
  myDoStopCB(this,&InputHandler::doStop)
  /* */
{

  /* Add our functor objects to the key handler, associated with the appropriate
   * keys: */
  myKeyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB);
  myKeyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB);
  myKeyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB);
  myKeyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB);
  myKeyHandler->addKeyHandler('s', &mySafeDriveCB);
  myKeyHandler->addKeyHandler('u', &myUnsafeDriveCB);
  myKeyHandler->addKeyHandler('l', &myListDataCB);
  myKeyHandler->addKeyHandler('t', &myLogTrackingTerseCB);
  myKeyHandler->addKeyHandler('v', &myLogTrackingVerboseCB);
  myKeyHandler->addKeyHandler('r', &myResetTrackingCB);

  /* dml 6/10/11 */
  myKeyHandler->addKeyHandler('P', &mySetPanParametersCB);
  myKeyHandler->addKeyHandler('T', &mySetTiltParametersCB);
  myKeyHandler->addKeyHandler('N', &myAddNewRobotCB);
  myKeyHandler->addKeyHandler('S', &mySetRobotNumberCB);
  myKeyHandler->addKeyHandler('G',&myDoStereoScanCB);
  /*   */
  myKeyHandler->addKeyHandler('Z',&myToggleSalienceCB);
  myKeyHandler->addKeyHandler('M',&myEnableSalienceMoveCB);
  myKeyHandler->addKeyHandler('.',&myDoStopCB);

  dontCheck=false;
  currentRobotNumber=0; maxRobotSoFar=1;
  for (int i=0; i<MAX_ROBOTS; i++) robotNumberTable[i]=0;  
  robotNumberTable[0]=client;
  panInc=20; numPans=3; defTilt=20;
  tiltInc=0; numTilts=1; defPan=-1;

  lastScan=time(NULL);
  sinceScan= -10;
}

InputHandler::~InputHandler(void)
{
  // should free up the robot table
}

ArClientBase *InputHandler::getClient() { return myClient; }

void InputHandler::up(void)
{
  if (myPrinting)
    printf("Forwards\n");
  myTransRatio = 100;
}

void InputHandler::down(void)
{
  if (myPrinting)
    printf("Backwards\n");
  myTransRatio = -100;
}

void InputHandler::left(void)
{
  if (myPrinting)
    printf("Left\n");
  myRotRatio = 10;
}

void InputHandler::right(void)
{
  if (myPrinting)
    printf("Right\n");
  myRotRatio = -10;
}

void InputHandler::safeDrive()
{
  /* Construct a request packet. The data is a single byte, with value 
   * 1 to enable safe drive, 0 to disable. */
  ArNetPacket p;
  p.byteToBuf(1);

  /* Send the packet as a single request: */
  if(myPrinting)
    printf("Sending setSafeDrive 1.\n");
  myClient->requestOnce("setSafeDrive",&p);
  if(myPrinting)
    printf("\nSent enable safe drive.\n");
}

void InputHandler::unsafeDrive()
{
  /* Construct a request packet. The data is a single byte, with value 
   * 1 to enable safe drive, 0 to disable. */
  ArNetPacket p;
  p.byteToBuf(0);

  /* Send the packet as a single request: */
  if(myPrinting)
    printf("Sending setSafeDrive 0.\n");
  myClient->requestOnce("setSafeDrive",&p);
  if(myPrinting)
    printf("\nSent disable safe drive command. Your robot WILL run over things if you're not careful.\n");
}

void InputHandler::listData()
{
  myClient->logDataList();
}

void InputHandler::logTrackingTerse()
{
  myClient->logTracking(true);
}

void InputHandler::logTrackingVerbose()
{
  myClient->logTracking(false);
}

void InputHandler::resetTracking()
{
  myClient->resetTracking();
}

/* dml 6/10/11 */

void InputHandler::doStereoScan()
{
  myClient->requestOnce("doStereoScan", NULL);
  ArUtil::sleep(500);
}



void InputHandler::toggleSalience()
{
  myClient->requestOnce("doSalience", NULL);
  ArUtil::sleep(500);
}

void InputHandler::enableSalienceMove()
{
  myClient->requestOnce("doSalienceMove", NULL);
  ArUtil::sleep(500);
}
void InputHandler::doStop()
{
  myClient->requestOnce("doSTOP", NULL);
}

void InputHandler::setPanParameters()
{
  if(!myClient->dataExists("setStereoPan")) return;

  myKeyHandler->restore();
  G_quiet=true;

  printf("Enter pan increment, number of pans, and default tilt during panning\n");
  scanf("%d %d %d",&panInc, &numPans, &defTilt);

  ArNetPacket packet;
  packet.doubleToBuf(double(panInc));
  packet.doubleToBuf(double(numPans));
  packet.doubleToBuf(double(defTilt)); 
  dontCheck=false;

  if (myPrinting)
    printf("Sending\n");
  myClient->requestOnce("setStereoPan", &packet);
  printf("Setting pan parameters.\n");
  myKeyHandler->takeKeys();
  G_quiet=false;

}

void InputHandler::setTiltParameters()
{
  if(!myClient->dataExists("setStereoTilt")) return;

  myKeyHandler->restore();
  G_quiet=true;

  printf("Enter tilt increment, number of tilts, and default pan during tilting\n");
  scanf("%d %d %d",&tiltInc, &numTilts, &defPan);

  ArNetPacket packet;
  packet.doubleToBuf(double(tiltInc));
  packet.doubleToBuf(double(numTilts));
  packet.doubleToBuf(double(defPan)); 
  dontCheck=false;
  if (myPrinting)
    printf("Sending\n");
  myClient->requestOnce("setStereoTilt", &packet);
  printf("Setting tilt parameters.\n");
  myKeyHandler->takeKeys();
  G_quiet=false;

}

void InputHandler::addNewRobot()
{ char ip[100];
  int myArgc=5; char *myArgv[6];

  myKeyHandler->restore();
  G_quiet=true;

  myArgv[0]="./clientDemo";
  myArgv[1]="-host";

  printf("What is the IP address of the new robot\n");
  scanf("%s",ip);

  myArgv[2]=ip;
  myArgv[3]="-port";
  myArgv[4]="7272";
  myArgv[5]=0;

  ArClientBase *client = new ArClientBase;

  ArArgumentParser parser(&myArgc,myArgv);
  parser.loadDefaultArguments();
  ArClientSimpleConnector clientConnector(&parser);
  clientConnector.parseArgs();

  /* Connect our client object to the remote server: */
  if (!clientConnector.connectClient(client))
  {
    if (client->wasRejected())
      printf("Server '%s' rejected connection, exiting\n", client->getHost());
    else
      printf("Could not connect to server '%s', exiting\n", client->getHost());
    return;
  } 

  printf("Connected to server on new robot[%d]=%s.\n",maxRobotSoFar,ip);
  client->runAsync();
  robotNumberTable[maxRobotSoFar]=client;
  maxRobotSoFar++;
  myKeyHandler->takeKeys();
  G_quiet=false;

}


void InputHandler::setRobotNumber()
{ int r;
 
  myKeyHandler->restore();
  G_quiet=true;

  printf("Enter the robot number: 0 is the base robot, -1 means all robots\n");
  scanf("%d",&r);

  setRobotTo(r);

  myKeyHandler->takeKeys();
  G_quiet=false;
  ArUtil::sleep(500);

}

void InputHandler::setRobotTo(int r)
{ 
  if (r>=0 && r<MAX_ROBOTS)
    if (robotNumberTable[r]!=0) {
      currentRobotNumber=r;
      myClient=robotNumberTable[r];
      printf("Now controlling robot number %d.\n",r);

    } else printf("Robot %d not started yet\n",r);
  else if (r!=-1) printf("Bad robot number argument %d\n",r);
  else {
    printf("Broadcasting robot commands to all robots\n");
    currentRobotNumber=-1;
  }
  ArUtil::sleep(500);
}


void InputHandler::sendInput(void)
{
  /* This method is called by the main function to send a ratioDrive
   * request with our current velocity values. If the server does 
   * not support the ratioDrive request, then we abort now: */
  if(!myClient->dataExists("ratioDrive")) return;

  /* Construct a ratioDrive request packet.  It consists
   * of three doubles: translation ratio, rotation ratio, and an overall scaling
   * factor. */
  ArNetPacket packet;
  packet.doubleToBuf(myTransRatio);
  packet.doubleToBuf(myRotRatio);
  packet.doubleToBuf(80); // use half of the robot's maximum.
  if (myPrinting)
    printf("Sending\n");

  if (currentRobotNumber<0) 
    for (int i=0; i<maxRobotSoFar; i++)
     robotNumberTable[i]->requestOnce("ratioDrive", &packet);
  else myClient->requestOnce("ratioDrive", &packet);

  sinceScan = time(NULL) - lastScan;

  if ( myJoyHandler.getButton(2) )
    if ( (sinceScan>10) ) {/* send stereo scan  command */
      myClient->requestOnce("doStereoScan", NULL);
      lastScan=time(NULL);
    } //else printf("rejecting bounce %d\n",sinceScan);

  myRotRatio = myTransRatio = 0;
}/*  */


/** This class requests continual data updates from the server and prints them
 * out.
 */
class OutputHandler
{
public:
  OutputHandler(ArClientBase *client);
  virtual ~OutputHandler(void);
  
  /// This callback is called when an update on general robot state arrives
  void handleOutput(ArNetPacket *packet);

  /// This callback is called when an update on the battery configuration changes
  void handleBatteryInfo(ArNetPacket *packet);
  /// This is called when the physical robot information comes back
  void handlePhysicalInfo(ArNetPacket *packet);

  ArClientBase *getClient();
  void setClient(ArClientBase *c);

protected:

  /// The results from the data update are stored in these variables
  //@{
  double myX;
  double myY;
  double myTh;
  double myVel;
  double myRotVel;
  double myVoltage;
  char myStatus[256];
  char myMode[32];
  //@}
  ArClientBase *myClient;

  /** These functor objects are given to the client to receive updates when they
   * arrive from the server.
   */
  //@{
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleBatteryInfoCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandlePhysicalInfoCB;
  //@}
  
  /// A header for the columns in the data printout is sometimes printed
  bool myNeedToPrintHeader;
  /// Don't print any information until we get the battery info
  bool myGotBatteryInfo;
};

OutputHandler::OutputHandler(ArClientBase *client) :
  myClient(client),
  myHandleOutputCB(this, &OutputHandler::handleOutput),
  myHandleBatteryInfoCB(this, &OutputHandler::handleBatteryInfo),
  myHandlePhysicalInfoCB(this, &OutputHandler::handlePhysicalInfo),
  myNeedToPrintHeader(false),
  myGotBatteryInfo(false)
{
  /* Add a handler for battery info, and make a single request for it  */
  myClient->addHandler("physicalInfo", &myHandlePhysicalInfoCB);
  myClient->requestOnce("physicalInfo");


  /* Add a handler for battery info, and make a single request for it  */
  myClient->addHandler("batteryInfo", &myHandleBatteryInfoCB);
  myClient->requestOnce("batteryInfo");

  /* Add a handler for general info, and request it to be called every 1000 ms */
  myClient->addHandler("update", &myHandleOutputCB);
  myClient->request("update", 500);
}

OutputHandler::~OutputHandler(void)
{
  /* Halt the request for data updates */
  myClient->requestStop("update");
}


ArClientBase *OutputHandler::getClient() { return myClient; }

void OutputHandler::setClient(ArClientBase *c) { 
  if (c==NULL) return;
 
  myClient->requestStop("update");
  myClient = c;
  printf("\nSwitching Robot telemetry Source.\n");
  myClient->requestOnce("physicalInfo");
  myClient->requestOnce("batteryInfo");
  myClient->addHandler("update", &myHandleOutputCB);
  myClient->request("update", 500);
}


void OutputHandler::handleOutput(ArNetPacket *packet)
{
  /* Extract the data from the update packet. Its format is status and
   * mode (null-terminated strings), then 6 doubles for battery voltage, 
   * x position, y position and orientation (theta) (from odometry), current
   * translational velocity, and current rotational velocity. Translation is
   * always milimeters, rotation in degrees.
   */
  memset(myStatus, 0, sizeof(myStatus));
  memset(myMode, 0, sizeof(myMode));
  packet->bufToStr(myStatus, sizeof(myStatus));
  packet->bufToStr(myMode, sizeof(myMode));
  myVoltage = ( (double) packet->bufToByte2() )/10.0;
  myX = (double) packet->bufToByte4();
  myY = (double) packet->bufToByte4();
  myTh = (double) packet->bufToByte2();
  myVel = (double) packet->bufToByte2();
  myRotVel = (double) packet->bufToByte2();

  if(myNeedToPrintHeader && !G_quiet) 
  {
    printf("\n%6s|%6s|%6s|%6s|%6s|%6s|%15s|%20s|\n",
      "x","y","theta", "vel", "rotVel", "volts", "mode","status");
    fflush(stdout);
    myNeedToPrintHeader = false;
  }
  if (myGotBatteryInfo && !G_quiet)
    printf("%6.0f|%6.0f|%6.1f|%6.1f|%6.1f|%6.1f|%15s|%20s|\r",
	   myX, myY, myTh, myVel, myRotVel, myVoltage, myMode, myStatus);
  
  fflush(stdout);
}

void OutputHandler::handleBatteryInfo(ArNetPacket *packet)
{
  /* Get battery configuration parameters: when the robot will begin beeping and 
   * warning about low battery, and when it will automatically disconnect and
   * shutdown. */
  double lowBattery = packet->bufToDouble();
  double shutdown = packet->bufToDouble();
  if (!G_quiet)
   printf("Low battery voltage: %6g       Shutdown battery voltage: %6g\n", lowBattery, shutdown);
  fflush(stdout);
  myNeedToPrintHeader = true;
  myGotBatteryInfo = true;
}


void OutputHandler::handlePhysicalInfo(ArNetPacket *packet)
{
  /* Get phyiscal configuration parameters: */
  char robotType[512];
  char robotSubtype[512];
  int width;
  int lengthFront;
  int lengthRear;

  packet->bufToStr(robotType, sizeof(robotType));
  packet->bufToStr(robotSubtype, sizeof(robotSubtype));
  width = packet->bufToByte2();
  lengthFront = packet->bufToByte2();
  lengthRear = packet->bufToByte2();

  if (!G_quiet)
   printf("Type: %s Subtype: %s Width %d: LengthFront: %d LengthRear: %d\n",
	 robotType, robotSubtype, width, lengthFront, lengthRear);
  fflush(stdout);
}


/* Key handler for the escape key: shutdown all of Aria. */
void escape(void)
{
  printf("esc pressed, shutting down aria\n");
  Aria::shutdown();
}

int main(int argc, char **argv)
{
  /* Aria initialization: */
  Aria::init();

  /* Create our client object. This is the object which connects with a remote
   * server over the network, and which manages all of our communication with it
   * once connected by sending data "requests".  Requests may be sent once, or
   * may be repeated at any frequency. Requests and replies to requsets contain 
   * payload "packets", into which various data types may be packed (when making a 
   * request), and from which they may also be extracted (when handling a reply). 
   * See the InputHandler and OutputHandler classes above for
   * examples of making requests and reading/writing the data in packets.
   */
  ArClientBase client;

  /* Aria components use this to get options off the command line: */
  ArArgumentParser parser(&argc, argv);

  /* This will be used to connect our client to the server, including
   * various bits of handshaking (e.g. sending a password, retrieving a list
   * of data requests and commands...)
   * It will get the hostname from the -host command line argument: */
  ArClientSimpleConnector clientConnector(&parser);

  parser.loadDefaultArguments();

  /* Check for -help, and unhandled arguments: */
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    exit(0);
  }

  
  /* Connect our client object to the remote server: */
  if (!clientConnector.connectClient(&client))
  {
    if (client.wasRejected())
      printf("Server '%s' rejected connection, exiting\n", client.getHost());
    else
      printf("Could not connect to server '%s', exiting\n", client.getHost());
    exit(1);
  } 

  printf("Connected to server.\n");


  // initialize the joystick handler
  myJoyHandler.init();
  // set the values we'll get back out of the joystick handler
  myJoyHandler.setSpeeds(100, 700);

  // see if we have the joystick, and let the user know
  if (myJoyHandler.haveJoystick())    {
      printf("Joystick detected.\n\n");
      printf("button 2 is stereoScan; 4 means robot1; 5 is robot2; 3 is both.\n");
    }
  // we don't have a joystick, exit
  else   {
      printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
    }
  /*****************************************************************************************/

  /* Create a key handler and also tell Aria about it */
  ArKeyHandler keyHandler;

  Aria::setKeyHandler(&keyHandler);

  /* Global escape-key handler to shut everythnig down */
  ArGlobalFunctor escapeCB(&escape);
  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);

  /* Now that we're connected, we can run the client in a background thread, 
   * sending requests and receiving replies. When a reply to a request arrives,
or the server makes a request of the client, a handler functor is invoked. 
   * The handlers for this program are registered with the client by the 
   * InputHandler and OutputHandler classes (in their constructors, above) */
  client.runAsync();

  /* Create the InputHandler object and request safe-drive mode */
  InputHandler inputHandler(&client, &keyHandler);
  inputHandler.safeDrive();

  /* Use ArClientBase::dataExists() to see if the "ratioDrive" request is available on the 
   * currently connected server.  */
  if(!client.dataExists("ratioDrive") )
      printf("Warning: server does not have ratioDrive command, can not use drive commands!\n");
  else
    printf("Keys are:\nUP: Forward\nDOWN: Backward\nLEFT: Turn Left\nRIGHT: Turn Right\n");
  printf("s: Enable safe drive mode (if supported).\nu: Disable safe drive mode (if supported).\nl: list all data requests on server\n\nDrive commands use 'ratioDrive'.\nt: logs the network tracking tersely\nv: logs the network tracking verbosely\nr: resets the network tracking\n\n");
  printf("S: set robot number\nN: make new robot\nP: set pan params for stereoScan\nT: set tilt params for stereoScan\nG: do a stereoScan\n");
  printf("Z: enable salience\nM: do Salience move \n'.': STOP salience move\n");
  /* Create the OutputHandler object. It will begin printing out data from the
   * server. */
  OutputHandler outputHandler(&client);


  /* Begin capturing keys into the key handler. Callbacks will be called
   * asyncrosously from this main thread when pressed.  */

  /* While the client is still running (getRunningWithLock locks the "running"
   * flag until it returns), check keys on the key handler (which will call
   * our callbacks), then tell the input handler to send drive commands. 
   * Sleep a fraction of a second as well to avoid using
   * too much CPU time, and give other threads time to work.
  */

  while (client.getRunningWithLock())
    { double x,y,z;
    keyHandler.checkKeys();

    /* joystick driving control */


       if (myJoyHandler.getButton(4)) 
	 inputHandler.setRobotTo(0);

       if (myJoyHandler.getButton(3)) 
	 inputHandler.setRobotTo(-1);

       if (myJoyHandler.getButton(5)) 
	 inputHandler.setRobotTo(1);

       if (myJoyHandler.getButton(8)) 
	 inputHandler.toggleSalience();

       if (myJoyHandler.getButton(9)) 
	 inputHandler.enableSalienceMove();

       if (myJoyHandler.getButton(10)) 
	 inputHandler.doStop();

    
    if (myJoyHandler.getButton(1)) {// joystick command overrides keyboard)
      myJoyHandler.getDoubles(&x,&y,&z); 
      if (y>0.33) inputHandler.up(); else if (y<-0.33) inputHandler.down();
      if (x>0.33) inputHandler.right(); else if (x<-0.33) inputHandler.left();
    }

    inputHandler.sendInput();

    if (inputHandler.getClient()!=outputHandler.getClient())
      outputHandler.setClient( inputHandler.getClient() ); // make sure both point to same robot
    ArUtil::sleep(100);
    }

  /* The client stopped running, due to disconnection from the server, general
   * Aria shutdown, or some other reason. */
  Aria::shutdown();
  return 0;
}