/*
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"

/*
  This demo starts up the robot in its own thread, then runs a bunch of the 
  direct motion commands... note that if you want to stop direct motion 
  commands and let actions take over, you should use clearDirectMotion, see
  the docs for details.  Also note how it locks the robot before doing anything
  with the robot, then unlocks it when done, this is vital for threaded code.
  You shouldn't really use threads if you don't understand them or don't need
  them.
*/

/*
  This is a connection handler, fairly simple, but quite useful, esp when
  the robot is running in another thread.  Its not really needed here
  since blockingConnect is used.  But it'd still be useful if there is an
  error after connected
*/
class ConnHandler
{
public:
  // Constructor
  ConnHandler(ArRobot *robot);
  // Destructor, its just empty
  ~ConnHandler(void) {}
  // to be called if the connection was made
  void connected(void);
  // to call if the connection failed
  void connFail(void);
  // to be called if the connection was lost
  void disconnected(void);
protected:
  // robot pointer
  ArRobot *myRobot;
  // the functor callbacks
  ArFunctorC<ConnHandler> myConnectedCB;
  ArFunctorC<ConnHandler> myConnFailCB;
  ArFunctorC<ConnHandler> myDisconnectedCB;
};

/*
  The constructor, note its use of contructor chaining to initalize the 
  callbacks.
*/
ConnHandler::ConnHandler(ArRobot *robot) :
  myConnectedCB(this, &ConnHandler::connected),  
  myConnFailCB(this, &ConnHandler::connFail),
  myDisconnectedCB(this, &ConnHandler::disconnected)

{
  // set the robot poitner
  myRobot = robot;

  // add the callbacks to the robot
  myRobot->addConnectCB(&myConnectedCB, ArListPos::FIRST);
  myRobot->addFailedConnectCB(&myConnFailCB, ArListPos::FIRST);
  myRobot->addDisconnectOnErrorCB(&myDisconnectedCB, ArListPos::FIRST);
}

// just exit if the connection failed
void ConnHandler::connFail(void)
{
  printf("Failed to connect.\n");
  myRobot->stopRunning();
  Aria::shutdown();
  return;
}

// turn on motors, and off sonar, and off amigobot sounds, when connected
void ConnHandler::connected(void)
{
  printf("Connected\n");
  myRobot->comInt(ArCommands::SONAR, 0);
  myRobot->comInt(ArCommands::ENABLE, 1);
  myRobot->comInt(ArCommands::SOUNDTOG, 0);
}

// lost connection, so just exit
void ConnHandler::disconnected(void)
{
  printf("Lost connection\n");
  exit(0);
}



int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  int dist;
  ArTime start;
  ArPose startPose;
  bool vel2 = false;

  // connection to the robot
  ArSerialConnection con;
  // the robot
  ArRobot robot;
  // the connection handler from above
  ConnHandler ch(&robot);
  
  // init area with a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

  if (argc != 2 || (dist = atoi(argv[1])) == 0)
    {
      printf("Usage: %s <distInMM>\n", argv[0]);
      exit(0);
    }
  if (dist < 1000)
    {
      printf("You must go at least a meter\n");
      exit(0);
    }
  // open the connection with the defaults, exit if failed
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  // set the robots connection
  robot.setDeviceConnection(&con);
  // try to connect, if we fail, the connection handler should bail
  if (!robot.blockingConnect())
  {
    // this should have been taken care of by the connection handler
    // but just in case
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }
  // run the robot in its own thread, so it gets and processes packets and such
  robot.runAsync(false);

  // just a big long set of printfs, direct motion commands and sleeps,
  // it should be self-explanatory

  robot.lock();

  /*
  robot.setAbsoluteMaxTransVel(2000);
  robot.setTransVelMax(2000);
  robot.setTransAccel(1000);
  robot.setTransDecel(1000);
  robot.comInt(82, 30); // rotkp
  robot.comInt(83, 200); // rotkv
  robot.comInt(84, 0); // rotki
  robot.comInt(85, 30); // transkp
  robot.comInt(86, 450); // transkv
  robot.comInt(87, 4); // transki

  */
  printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
  if (vel2)
    robot.setVel2(2200, 2200);
  else
    robot.setVel(2200);
  robot.unlock();
  start.setToNow();
  startPose = robot.getPose();
  while (1)
  {
    robot.lock();
    printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
	   robot.getVel(), robot.getX(), robot.getY(), 
	   startPose.findDistanceTo(robot.getPose()),
	   robot.getTh());
    if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
    {
      printf("\nFinished distance\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 10000)
    {
      printf("\nDistance timed out\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  
  if (vel2)
    robot.setVel2(0, 0);
  else
    robot.setVel(0);
  start.setToNow();
  while (1)
    {
      robot.lock();
      if (vel2)
	robot.setVel2(0, 0);
      else
	robot.setVel(0);
      if (fabs(robot.getVel()) < 20)
	{
	  printf("Stopped\n");
	  robot.unlock();
	  break;
	}
      if (start.mSecSince() > 2000)
	{
	  printf("\nStop timed out\n");
	  robot.unlock();
	  break;
	}
      robot.unlock();
      ArUtil::sleep(50);
    }
  robot.lock();
  robot.disconnect();
  robot.unlock();
  // shutdown and ge tout
  Aria::shutdown();
  return 0;
}