void BitsetSaver::save(
    const std::vector<bool> &bits)
{
    char byte = 0;
    for (size_t i = 0; i < bits.size(); ++ i)
    {
        const unsigned shift = i % 8;
        const char positionHasNeighbors = bits[i];
        byte |= positionHasNeighbors << shift;
        if (7 == shift)
        {
            if (!os_.write(&byte, sizeof(byte)))
            {
                const boost::format message = boost::format("Failed to write bits into %s: %s") % filePath_.string() % strerror(errno);
                BOOST_THROW_EXCEPTION(common::IoException(errno, message.str()));
            }
            byte = 0;
        }
    }

    if (!os_.write(&byte, sizeof(byte)))
    {
        const boost::format message = boost::format("Failed to write final bits byte into %s: %s") % filePath_.string() % strerror(errno);
        BOOST_THROW_EXCEPTION(common::IoException(errno, message.str()));
    }
}

void FastaDumperOptions::postProcess(bpo::variables_map &vm)
{
    eagle::common::OptionsHelper check(vm);

    if (fastaFiles.empty())
    {
        throw bpo::validation_error(bpo::validation_error::at_least_one_value_required, "", "positional");
    }

    check.addPathOptions(fastaFiles,"positional");
    check.inputPathsExist();

    if ( 1 == fastaFiles.size() && bfs::is_directory(fastaFiles[0]) )
    {
        mode = WHOLE_DIR;
    } else {
        for (unsigned int i = 0; i < fastaFiles.size(); i++)
        {
            if (bfs::is_directory(fastaFiles[i]))
            {
                const boost::format message = boost::format("\n   *** FASTA file #%d has an invalid value: ***"
                                                            "\n   ***       It should point to a file, but a directory already exists with name %s ***\n")
                                                            % i % fastaFiles[i];
                BOOST_THROW_EXCEPTION(eagle::common::InvalidOptionException(message.str()));
            }
        }
        mode = SAFE_MODE;
    }

    check.inRange<unsigned long>(std::make_pair(size,"size"),1);  // 1 <= size < inf
                                                                  // (size == 0) not allowed, so internally used to represent 'until the end'
}

static std::vector<std::string > expandUseBasesMask (
    const std::vector<unsigned int> &readLengths,
    const std::string &useBasesMask,
    const boost::filesystem::path &baseCallsDirectory)
{
    std::vector<std::string > result;
    std::string::const_iterator parseIt(useBasesMask.begin());
    const std::string::const_iterator parseEnd(useBasesMask.end());
    UseBasesMaskGrammar<std::string::const_iterator> parser(readLengths);
    if (!boost::spirit::qi::parse(parseIt, parseEnd, parser, result) ||
        parseEnd != parseIt)
    {
        const boost::format message = boost::format("\n   *** Could not parse the use-bases-mask '%s' for '%s' at: %s ***\n") %
                useBasesMask % baseCallsDirectory.string() % useBasesMask.substr(parseIt - useBasesMask.begin());
        BOOST_THROW_EXCEPTION(common::InvalidOptionException(message.str()));
    }

    ISAAC_THREAD_CERR << "use bases mask: " << boost::algorithm::join(result, ",") << "\n";
    ISAAC_THREAD_CERR << "reads parsed: " << parser.currentRead_ << "\n";

    if (result.size() != readLengths.size())
    {
        const boost::format message = boost::format("\n   *** use-bases-mask '%s' is incompatible with number of reads (%d) in %s ***\n") %
                useBasesMask % readLengths.size() % baseCallsDirectory.string();
        BOOST_THROW_EXCEPTION(common::InvalidOptionException(message.str()));
    }

    return result;
}

BitsetSaver::BitsetSaver(const boost::filesystem::path filePath) :
    filePath_(filePath),
    os_(filePath.c_str())
{
    if (!os_)
    {
        const boost::format message = boost::format("Failed to open file %s for writing: %s") % filePath_ % strerror(errno);
        BOOST_THROW_EXCEPTION(common::IoException(errno, message.str()));
    }
}

void vfeSession::AppendStatusMessage (const boost::format& fmt, int RecommendedPause)
{
  boost::mutex::scoped_lock lock(m_MessageMutex);

  m_StatusQueue.push (StatusMessage (*this, fmt.str(), RecommendedPause));
  m_StatusLineMessage = fmt.str();
  if (m_MaxStatusMessages != -1)
    while (m_StatusQueue.size() > m_MaxStatusMessages)
      m_StatusQueue.pop();
  NotifyEvent(stStatusMessage);
}

 boost::format CustomFormat(const char* s)
 {
   using namespace boost::io;
   boost::format format(s);
   format.exceptions(all_error_bits ^ (too_many_args_bit | too_few_args_bit));
   return format;
 }

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_saver", ros::init_options::AnonymousName);
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  //std::string topic = nh.resolveName("image");
//  std::string topic = "/ardrone/image_raw";
  std::string topic = "/ardrone/kinect/image_raw";
  std::string topic_depth = "/ardrone/kinect/depth/image_raw";

  Callbacks callbacks;
  //callbacks.control ="10000000";
  //callbacks.path = "/home/jay/data/";
  //obtain saving location
  std::string saving_location = nh.resolveName("generated_set");
  //if(saving_location.compare("generated_set")) saving_location = "remote_images/set_online";
  callbacks.path = "/home/jay/data/"+saving_location;
  boost::filesystem::path dir(callbacks.path);
  boost::filesystem::remove_all(dir);
  if(boost::filesystem::create_directory(dir)) {
    callbacks.path_depth = callbacks.path+"/depth";
    boost::filesystem::path dir_depth(callbacks.path_depth);
    if(boost::filesystem::create_directory(dir_depth)) {
      std::cout << "Success in creating: "<<callbacks.path_depth << "\n";
    }
    callbacks.path = callbacks.path+"/RGB";
    boost::filesystem::path dir_rgb(callbacks.path);
    if(boost::filesystem::create_directory(dir_rgb)) {
      std::cout << "Success in creating: "<<callbacks.path << "\n";
    }
  }else{
    std::cout <<"Failed to make saving direction "<<callbacks.path <<"\n";
  }
  
  // Useful when CameraInfo is being published
  /*image_transport::CameraSubscriber sub_image_and_camera = it.subscribeCamera(topic, 1,
                                                                              &Callbacks::callbackWithCameraInfo,
                                                                              &callbacks);*/
  // Useful when CameraInfo is not being published
  image_transport::Subscriber sub_image = it.subscribe(
      topic, 1, boost::bind(&Callbacks::callbackWithoutCameraInfo, &callbacks, _1));
  //depth
  image_transport::Subscriber sub_image_depth = it.subscribe(
      topic_depth, 1, boost::bind(&Callbacks::callbackWithoutCameraInfoWithDepth, &callbacks, _1));

  // Make subscriber to cmd_vel in order to set the name.
  ros::Subscriber takeOff = nh.subscribe("/ardrone/takeoff",1,&Callbacks::callbackTakeoff, &callbacks);
  ros::Subscriber subControl = nh.subscribe("/dagger_vel",1,&Callbacks::callbackCmd, &callbacks);
  // [hover, back, forward, turn right, turn left, down, up, clockwise, ccw]
  // Adapt name instead of left0000.jpg it should be 00000-gt1.jpg when receiving control 1 ~ straight
  ros::NodeHandle local_nh("~");
  std::string format_string;
  local_nh.param("filename_format", format_string, std::string("%s/%010i-gt%s.%s"));
  local_nh.param("encoding", encoding, std::string("bgr8"));
  local_nh.param("save_all_image", save_all_image, true);
  g_format.parse(format_string);
  ros::ServiceServer save = local_nh.advertiseService ("save", service);

  ros::spin();
} 

	hpgl_exception::hpgl_exception(
		const std::string & a_where, 
		const boost::format & what)
			:m_where(a_where), m_what(what.str())
		{
			m_message = m_where + ": " + m_what;
			//std::cerr << m_message << std::endl;
		}

 error::error(
     bool          _status,
     long long     _code,
     boost::format _msg,
     std::string   _file,
     int           _line,
     std::string   _fcn ) :
     error::error( _status, _code, _msg.str(), _file, _line, _fcn ) {}

void Logger::log(
    const Level& level,
    const boost::format& message,
    const std::string& filename,
    const int& lineNumber
)
{
    log( level, message.str(), filename, lineNumber );
}

  bs_exception::bs_exception (const std::string &who, const boost::format &message)
  : who_ (who),
  what_ (who_ + ": " + message.str ()),
  m_err_ (user_defined)
  {
#ifdef BS_EXCEPTION_COLLECT_BACKTRACE
    what_ += detail::collect_backtrace ();
#endif
  }

void Trace::send(const boost::format& str, TraceLevelOptions level)
{
    if (not Trace::Pimpl::mTrace) {
        Trace::init();
    }

    boost::mutex::scoped_lock lock(Trace::Pimpl::mTrace->getMutex());

    Pimpl::mTrace->send(str.str(), level);
}

void chronometer_t::event_dumb(boost::format const& params) {
    if (is_dumb) {
        if (!log_ptr) {
            log_ptr = logger::instance();
        }
        if (log_ptr) {
            log_ptr->debug("raw/begin/" + name, params.str());
        }
    }
}

void Log::Critical( boost::format &fmt )
{
	int ConsoleLogLevel = sConfig.GetIntDefault("Log.ConsoleLogLevel",LOGLEVEL_INFO);
	int FileLogLevel = sConfig.GetIntDefault("Log.FileLogLevel",LOGLEVEL_WARNING);

	if (ConsoleLogLevel >= LOGLEVEL_CRITICAL || FileLogLevel >= LOGLEVEL_CRITICAL)
	{
		Critical(fmt.str());
	}
}

void Log::Debug( boost::format &fmt )
{
	int ConsoleLogLevel = sConfig.GetIntDefault("Log.ConsoleLogLevel",LOGLEVEL_INFO);
	int FileLogLevel = sConfig.GetIntDefault("Log.FileLogLevel",LOGLEVEL_WARNING);

	if (ConsoleLogLevel >= LOGLEVEL_DEBUG || FileLogLevel >= LOGLEVEL_DEBUG)
	{
		Debug(fmt.str());
	}
}

void Log::Error( boost::format &fmt )
{
	int ConsoleLogLevel = sConfig.GetIntDefault("Log.ConsoleLogLevel",LOGLEVEL_INFO);
	int FileLogLevel = sConfig.GetIntDefault("Log.FileLogLevel",LOGLEVEL_WARNING);

	if (ConsoleLogLevel >= LOGLEVEL_ERROR || FileLogLevel >= LOGLEVEL_ERROR)
	{
		Error(fmt.str());
	}
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_saver", ros::init_options::AnonymousName);
  ros::NodeHandle nh;
  g_format.parse("left%04i.%s");
  image_transport::ImageTransport it(nh);
  std::string topic = nh.resolveName("image");
  image_transport::CameraSubscriber sub = it.subscribeCamera(topic, 1, &callback);

  ros::spin();
}

  bool saveImage(cv::Mat image, std::string &filename, bool depth = false) {
    if (!image.empty()) {
      try {
        filename = (g_format).str();
      } catch (...) { g_format.clear(); }
      try {
        filename = (g_format % path).str();
      } catch (...) { g_format.clear(); }
      try {
        filename = (g_format % path % count_).str();
      } catch (...) { g_format.clear(); }
      try { 
        filename = (g_format % path % count_ % "jpg").str();
      } catch (...) { g_format.clear(); }
      try {
	if(!depth){
	  filename = (g_format % path % count_ % control % "jpg").str();
	}else{
	  filename = (g_format % path_depth % count_ % control % "jpg").str();
	}
      } catch (...) { g_format.clear(); }
      
      if ( save_all_image || save_image_service ) {
        try{
	  cv::imwrite(filename, image);
	  ROS_INFO("Saved image %s", filename.c_str());

	  save_image_service = false;
	}catch(runtime_error& ex){
	  fprintf(stderr, "Exception converting image to PNG format: %s\n", ex.what());
	  return false;
	}
      } else {
        return false;
      }
    } else {
      ROS_WARN("Couldn't save image, no data!");
      return false;
    }
    return true;
  }

void ImageNodelet::onInit()
{
  ros::NodeHandle nh = getNodeHandle();
  ros::NodeHandle local_nh = getPrivateNodeHandle();

  // Command line argument parsing
  const std::vector<std::string>& argv = getMyArgv();
  // First positional argument is the transport type
  std::string transport;
  local_nh.param("image_transport", transport, std::string("raw"));
  for (int i = 0; i < (int)argv.size(); ++i)
  {
    if (argv[i][0] != '-')
    {
      transport = argv[i];
      break;
    }
  }
  NODELET_INFO_STREAM("Using transport \"" << transport << "\"");
  // Internal option, should be used only by the image_view node
  bool shutdown_on_close = std::find(argv.begin(), argv.end(),
                                     "--shutdown-on-close") != argv.end();

  // Default window name is the resolved topic name
  std::string topic = nh.resolveName("image");
  local_nh.param("window_name", window_name_, topic);

  bool autosize;
  local_nh.param("autosize", autosize, false);
  
  std::string format_string;
  local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
  filename_format_.parse(format_string);

  cv::namedWindow(window_name_, autosize ? cv::WND_PROP_AUTOSIZE : 0);
  cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this);
  
#ifdef HAVE_GTK
  // Register appropriate handler for when user closes the display window
  GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
  if (shutdown_on_close)
    g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
  else
    g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_);
#endif

  // Start the OpenCV window thread so we don't have to waitKey() somewhere
  startWindowThread();

  image_transport::ImageTransport it(nh);
  image_transport::TransportHints hints(transport, ros::TransportHints(), getPrivateNodeHandle());
  sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "image_view", ros::init_options::AnonymousName);
  if (ros::names::remap("image") == "image") {
    ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
             "\t$ rosrun image_view image_view image:=<image topic> [transport]");
  }

  ros::NodeHandle nh;
  ros::NodeHandle local_nh("~");

  // Default window name is the resolved topic name
  std::string topic = nh.resolveName("image");
  local_nh.param("window_name", g_window_name, topic);

  std::string format_string;
  local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
  g_filename_format.parse(format_string);

  // Handle window size
  bool autosize;
  local_nh.param("autosize", autosize, false);
  cv::namedWindow(g_window_name, autosize ? (CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED) : 0);
  cv::setMouseCallback(g_window_name, &mouseCb);

  // Start the OpenCV window thread so we don't have to waitKey() somewhere
  cv::startWindowThread();

  // Handle transport
  // priority:
  //    1. command line argument
  //    2. rosparam '~image_transport'
  std::string transport;
  local_nh.param("image_transport", transport, std::string("raw"));
  ros::V_string myargv;
  ros::removeROSArgs(argc, argv, myargv);
  for (size_t i = 1; i < myargv.size(); ++i) {
    if (myargv[i][0] != '-') {
      transport = myargv[i];
      break;
    }
  }
  ROS_INFO_STREAM("Using transport \"" << transport << "\"");
  image_transport::ImageTransport it(nh);
  image_transport::TransportHints hints(transport, ros::TransportHints(), local_nh);
  image_transport::Subscriber sub = it.subscribe(topic, 1, imageCb, hints);

  ros::spin();

  cv::destroyWindow(g_window_name);
  return 0;
}