/*
Adept MobileRobots Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
Copyright (C) 2011, 2012, 2013 Adept Technology

     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 
Adept MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
*/
#include "Aria.h"

/* Example showing how two ArRobot objects can be used in one program.
 
  Normally, a program will only connect to one robot, and will only have
  one ArRobot object.   However, it is possible to connect to more than
  one robot from the same program -- for example, this may be useful for doing 
  multirobot experiments in a simulator, or to control remote robots with
  wireless-serial bridge devices from an offboard computer. This program 
  demonstrates this by connecting to two robots over TCP connections. 
  (Old systems with dedicated serial-radio links for each robot would require 
  modifying this program to use serial connections instead.)

  Specify the hostnames of each robot with -rh1 and -rh2 command line arguments
  (defaults are "localhost" for each). Specify port numbers for each with
  -rp1 and -rp2 command line arguments (defaults are 8101 for each if 
  the hostnames are different, or 8101 and 8102 if the hostnames are the same).

  This program will just have two robots wander around, avoiding obstacles
  detected by the sonar, or otherwise moving forward at constant velocity.
  First it has one robot wander for a period of time, then it has the 
  second robot wander around.
*/

int main(int argc, char** argv)
{
  int ret;
  std::string str;
  int curRobot;

  // get hostnames and port numbers for connecting to the robots.
  ArArgumentParser argParser(&argc, argv);
  char* host1 = argParser.checkParameterArgument("-rh1");
  if(!host1) host1 = "localhost";
  char* host2 = argParser.checkParameterArgument("-rh2");
  if(!host2) host2 = "localhost";

  int port1 = 8101;
  int port2 = 8101;
  if(strcmp(host1, host2) == 0 )
  {
    // same host, it must be using two ports (but can still override below with -rp2)
    port2++;
  }

  bool argSet = false;
  argParser.checkParameterArgumentInteger("-rp1", &port1, &argSet);  
  if(!argSet) argParser.checkParameterArgumentInteger("-rrtp1", &port1);
  argSet = false;
  argParser.checkParameterArgumentInteger("-rp2", &port2, &argSet);
  if(!argSet) argParser.checkParameterArgumentInteger("-rrtp2", &port2);
  
  if(!argParser.checkHelpAndWarnUnparsed())
  {
    ArLog::log(ArLog::Terse, "Usage: twoRobotWander [-rh1 <hostname1>] [-rh2 <hostname2>] [-rp1 <port1>] [-rp2 <port2>]\n"\
      "\t<hostname1> Is the network host name of the first robot."\
          " Default is localhost (for the simulator).\n"\
      "\t<hostname2> Is the network host name of the second robot."\
          " Default is localhost (for the simulator).\n"\
      "\t<port1> Is the TCP port number of the first robot. Default is 8101.\n"\
      "\t<port2> Is the TCP port number of the second robot. Default is 8102 if"\
        " both robots have the same hostname, or 8101 if they differ.\n\n");
    return 1;
  }

  

  //
  // The first robot's objects
  //
  // the first robot connection
  ArTcpConnection con1;
  // the first robot
  ArRobot robot1;
  // sonar, must be added to the first robot
  ArSonarDevice sonar1;
  // the actions we'll use to wander for the first robot
  ArActionStallRecover recover1;
  ArActionBumpers bumpers1;
  ArActionAvoidFront avoidFront1;
  ArActionConstantVelocity constantVelocity1("Constant Velocity", 400);

  //
  // The second robot's objects
  //
  // the second robot connection
  ArTcpConnection con2;
  // the second robot
  ArRobot robot2;
  // sonar, must be added to the second robot
  ArSonarDevice sonar2;
  // the actions we'll use to wander for the second robot
  ArActionStallRecover recover2;
  ArActionBumpers bumpers2;
  ArActionAvoidFront avoidFront2;
  ArActionConstantVelocity constantVelocity2("Constant Velocity", 400);

  // mandatory init
  Aria::init();

  //
  // Lets get robot 1 going
  //

  // open the connection, if this fails exit
  ArLog::log(ArLog::Normal, "Connecting to first robot at %s:%d...", host1, port1);
  if ((ret = con1.open(host1, port1)) != 0)
  {
    str = con1.getOpenMessage(ret);
    printf("Open failed to robot 1: %s\n", str.c_str());
    Aria::exit(1);
    return 1;
  }
  
  // add the sonar to the robot
  robot1.addRangeDevice(&sonar1);
  
  // set the device connection on the robot
  robot1.setDeviceConnection(&con1);
  
  // try to connect, if we fail exit
  if (!robot1.blockingConnect())
  {
    printf("Could not connect to robot 1... exiting\n");
    Aria::exit(1);
    return 1;
  }

  // turn on the motors, turn off amigobot sounds
  robot1.comInt(ArCommands::ENABLE, 1);
  robot1.comInt(ArCommands::SOUNDTOG, 0);

  // add the actions
  robot1.addAction(&recover1, 100);
  robot1.addAction(&bumpers1, 75);
  robot1.addAction(&avoidFront1, 50);
  robot1.addAction(&constantVelocity1, 25);


  //
  // Lets get robot 2 going
  //

  // open the connection, if this fails exit
  ArLog::log(ArLog::Normal, "Connecting to second robot at %s:%d...", host2, port2);
  if ((ret = con2.open(host2, port2)) != 0)
  {
    str = con2.getOpenMessage(ret);
    printf("Open failed to robot 2: %s\n", str.c_str());
    Aria::exit(1);
    return 1;
  }
  
  // add the sonar to the robot
  robot2.addRangeDevice(&sonar2);
  
  // set the device connection on the robot
  robot2.setDeviceConnection(&con2);
  
  // try to connect, if we fail exit
  if (!robot2.blockingConnect())
  {
    printf("Could not connect to robot 2... exiting\n");
    Aria::exit(1);
    return 1;
  }

  // turn on the motors, turn off amigobot sounds
  robot2.comInt(ArCommands::ENABLE, 1);
  robot2.comInt(ArCommands::SOUNDTOG, 0);

  // add the actions
  robot2.addAction(&recover2, 100);
  robot2.addAction(&bumpers2, 75);
  robot2.addAction(&avoidFront2, 50);
  robot2.addAction(&constantVelocity2, 25);
  
  // start the robots running. true so that if we lose connection to either
  // robot, the run stops.
  robot1.runAsync(true);
  robot2.runAsync(true);

  // As long as both robot loops are running, lets alternate between the
  // two wandering around.
  curRobot=1;
  while (robot1.isRunning() && robot2.isRunning())
  {
    ArUtil::sleep(10000);
    if (curRobot == 1)
    {
      robot1.stop();
      robot2.clearDirectMotion();
      curRobot=2;
    }
    else
    {
      robot2.stop();
      robot1.clearDirectMotion();
      curRobot=1;
    }
  }
  
  // now exit
  Aria::exit(0);
  return 0;
}