int main(int argc, char **argv)
{
  std::string str;
  int ret;
  int successes = 0, failures = 0;
  int action;
  bool exitOnFailure = true;
  
  ArSerialConnection con;
  ArRobot robot;
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);
  srand(time(NULL));
  robot.runAsync(false);
// if (!exitOnFailure)
//    ArLog::init(ArLog::None, ArLog::Terse);
  //else
  //ArLog::init(ArLog::None);
  while (1)
  {
    if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
	(ret = con.open()) != 0)
    {
      str = con.getOpenMessage(ret);
      printf("Open failed: %s\n", str.c_str());
      ++failures;
      if (exitOnFailure)
      {
	printf("Failed\n");
	exit(0);
      }
      else
      {
	ArUtil::sleep(200);
	robot.unlock();
	continue;
      }
    }
    robot.lock();
    robot.setDeviceConnection(&con);
    robot.unlock();
    ArUtil::sleep((rand() % 5) * 100);
    if (robot.asyncConnect())
    {
      robot.waitForConnectOrConnFail();
      robot.lock();
      if (!robot.isConnected())
      {
	if (exitOnFailure)
	{
	  printf("Failed after %d tries.\n", successes);
	  exit(0);
	}
	printf("Failed to connect successfully");
	++failures;
      }
      robot.comInt(ArCommands::SONAR, 0);
      robot.comInt(ArCommands::SOUNDTOG, 0);
      //robot.comInt(ArCommands::PLAYLIST, 0);
      robot.comInt(ArCommands::ENCODER, 1);
      ArUtil::sleep(((rand() % 20) + 3) * 100);
      ++successes;
      // okay, now try to leave it in a messed up state
      action = rand() % 8;
      robot.dropConnection();
      switch (action) {
      case 0:
	printf("Discon  0 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	break;
      case 1:
	printf("Discon  1 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	break;
      case 2:
	printf("Discon  2 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	ArUtil::sleep(100);
	robot.com(2);
	break;
      case 3:
	printf("Discon 10 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(10);
	break;
      case 4:
	printf("Discon    ");
	robot.disconnect();
	break;
      default:
	printf("Leave     ");
	break;
      }
      robot.unlock();
    }
    else
    {
      if (exitOnFailure)
      {
	printf("Failed after %d tries.\n", successes);
	exit(0);
      }
      printf("Failed to start connect ");
      ++failures;
    }
    if ((rand() % 2) == 0)
    {
      printf(" ! RadioDisconnect ! ");
      con.write("|||\15", strlen("!!!\15"));
      
      ArUtil::sleep(100);
      con.write("WMD\15", strlen("WMD\15"));
      ArUtil::sleep(200);
    }
    if ((rand() % 2) == 0)
    {
      printf(" ! ClosePort !\n");
      con.close();
    }
    else
      printf("\n");
    printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
	   (float)successes/(float)(successes+failures)*100);

    ArUtil::sleep((rand() % 2)* 1000);
  }
  return 0; 
}
int main(int argc, char **argv)
{
  int ret;
  std::string str;
  ArSerialConnection con;
  ArSickPacket sick;
  ArSickPacket *packet;
  ArSickPacketReceiver receiver(&con);
  ArTime start;
  unsigned int value;
  int numReadings;
  ArTime lastReading;
  ArTime packetTime;

  start.setToNow();

  // open the connection, if it fails, exit
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  start.setToNow();

  printf("Waiting for laser to power on\n");
  sick.empty();
  sick.uByteToBuf(0x10);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  while (start.secSince() < 70 && 
	 ((packet = receiver.receivePacket(100)) == NULL
	  ||  (packet->getID() != 0x90)));
  if (packet != NULL)
    printf("Laser powered on\n");
  else
    exit(1);

  printf("Changing baud\n");
  sick.empty();
  sick.byteToBuf(0x20);
  sick.byteToBuf(0x40);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  ArUtil::sleep(10);
  if (!con.setBaud(38400))
  {
    printf("Could not set baud, exiting\n");
  }
  
  
  /*packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();
  */
  sick.empty();
  sick.uByteToBuf(0x3B);
  sick.uByte2ToBuf(180);
  sick.uByte2ToBuf(100);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();

  sick.empty();
  sick.byteToBuf(0x20);
  sick.byteToBuf(0x24);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();



  printf("Starting to report back from port, it took %ld ms to get here:\n",
	 start.mSecSince());
  start.setToNow();
  while (start.secSince() < 6)
  {
    packetTime.setToNow();
    packet = receiver.receivePacket();
    if (packet != NULL)
      printf("####### %ld ms was how long the packet took\n", packetTime.mSecSince());
    if (packet != NULL)
    {
      if (packet->getLength() < 10)
	packet->log();
      else if (packet->getID() == 0x90)
      {
	char strBuf[512];
	packet->log();
	//printf("%x\n", packet->bufToUByte());
	packet->bufToStr(strBuf, 512);
	printf("0x%x %s\n", packet->getID(), strBuf);
	sick.empty();
	sick.uByteToBuf(0x3B);
	sick.uByte2ToBuf(180);
	sick.uByte2ToBuf(100);
	sick.finalizePacket();
	con.write(sick.getBuf(), sick.getLength());
	packet = receiver.receivePacket(100);
	sick.empty();
	sick.uByteToBuf(0x20);
	sick.uByteToBuf(0x24);
	sick.finalizePacket();
	con.write(sick.getBuf(), sick.getLength());
      }
      else
      {
	value = packet->bufToUByte2();
	numReadings = value & 0x3ff;
	printf("%ld ms after last reading.\n", lastReading.mSecSince());
	/*
	printf("Reading number %d, complete %d, unit: %d %d:\n", value & 0x3ff, !(bool)(value & ArUtil::BIT13), (bool)(value & ArUtil::BIT14), (bool)(value & ArUtil::BIT15));
	for (i = 0; i < numReadings; i++)
	{
	  value = packet->bufToUByte2();
	  if (value & ArUtil::BIT13)
	    printf("D");
	  printf("%d ", value & 0x1fff);
	}
	printf("\n");
	*/
	lastReading.setToNow();
      }
    }
    else
    {
      //printf("No packet\n");
    }
  }
}