/** allocs a scoped_block for a new key
     ** doesn't support reallocing on an existing key, maybe in the future */
    void alloc(boost::scoped_ptr< variable_store::scoped_block > & p,
               const Key & key,
               size_t block_size)
    {
        boost::mutex::scoped_lock lock(m_mutex);
        // don't support reallocing for now
        typename hm_key_location::const_iterator it = m_key_location.find(key);

        if (it != m_key_location.end())
            throw std::invalid_argument("realloc not supported");

        p.reset( new variable_store::scoped_block(m_variable_store, block_size) );
        m_key_location[key] = location(p->block_size(), p->index());
    }

LogBackend& LogBackend::instance()
{
	static boost::scoped_ptr< LogBackend >	m_t;
	static boost::mutex			m_mutex;
	static bool				m_initialized = false;

	if ( !m_initialized )	{
		boost::lock_guard< boost::mutex > lock( m_mutex );
		if ( !m_initialized )	{
			m_t.reset( new LogBackend() );
			m_initialized = true;
		}
	}
	return *m_t;
}

 /// Solve the problem
 void p_solve(const p_optimizeMethod& m)
 {
   switch (m) {
   case (Maximize):
     p_impl->maximize();
     break;
   case (Minimize):
     p_impl->minimize();
     break;
   default:
     BOOST_ASSERT(false);
   }
 }

    virtual void handle_start()
    {
        tcp::resolver resolver(main_pool().io_service());
        tcp::endpoint endpoint = *resolver.resolve(addr_query_);

        acceptor_.reset(new tcp::acceptor(main_pool().io_service()));
        acceptor_->open(endpoint.protocol());
        acceptor_->set_option(tcp::acceptor::reuse_address(true));
        acceptor_->bind(endpoint);
        acceptor_->listen();
        start_accept();

        std::cout << "Server started" << std::endl;
    }

bool CameraV4LPublisher::getImage(cv::Mat& img, bool emptyBuffer)
{
  bool ok = false;
  if ( emptyBuffer )
  {
    for (int i = 0; i < 4; ++i) //v4l stores a buffer of 4 images
      _capture->grab();
  }
  if ( _capture->grab() )
    if ( _capture->retrieve(img) )
      ok = true;

  return ok;
}

        /*! @brief configure the detector state
         *
         * The configure method is called once when the ECTO cell is launched,
         * and is designed to initialise the detector state and load models,
         * parameters, etc.
         *
         * @param params the parameters made available through the config file and
         * python bindings
         * @param inputs for initializing inputs, if necessary
         * @param outputs for initializing outputs, if necessary
         */
        void
        configure(const tendrils& params, const tendrils& inputs, const tendrils& outputs) {

            // create the model object and deserialize it
            FileStorageModel model;
            model.deserialize(*model_file_);

            // create the visualizer
            visualizer_.reset(new Visualize(model.name()));

            // create the PartsBasedDetector and distribute the model parameters
            detector_.reset(new PartsBasedDetector<float>);
            detector_->distributeModel(model);
        }

  VisualServo() : arm_("left"),
		planning_group_name_(arm_+"_arm"),
		camera_frame_(arm_+"_hand_camera")
  {
    move_group_.reset(new move_group_interface::MoveGroup(planning_group_name_));
    move_group_->setPlanningTime(30.0);
    move_group_->setPlannerId("RRTstark");
    while(ros::ok())
      {
	// marker_pose_subscriber_ = node_.subscribe("ar_pose_marker",50, &VisualServo::marker_pose_callback, this);
	// target_publisher_ = node_.advertise<geometry_msgs::Pose>("/visual_servo_target", 10);
	servo_to_tag();
      }
  }

	void reset()
	{
		fallback.reset();
		effects.clear();
		if(target)
			target.Release();
		if(offsetTarget)
			offsetTarget.Release();

		vbuffer.reset();
		ibuffer.reset();
		vshader.reset();
		pshader.reset();
	}

		~FiberControl(){
			AUTO(it, g_stack_pool.begin());
			for(;;){
				if(it == g_stack_pool.end()){
					stack.reset();
					break;
				}
				if(!*it){
					stack.swap(*it);
					break;
				}
				++it;
			}
		}

void HeartbeatArrival(uint16_t sourceId, const std::uint8_t *data, std::uint8_t len)
{
	if (len<5)
		return;
	CANExtended::DeviceState state = static_cast<CANExtended::DeviceState>(data[0]);
	if (state != CANExtended::Operational)
		return;
	auto unit = unitManager->FindUnit(sourceId&0x7f);
	bool updated = (unit!=nullptr) && (unit->UpdateStatus()==StorageUnit::Updated);
	
	if (unit == nullptr || updated)
	{
		StorageBasic basic(CanEx);
		basic.DeviceId = sourceId;
		basic.SensorNum = data[2];
		basic.Version = (data[3]<<8)|data[4];
		switch (data[1])
		{
			case UNIT_TYPE_INDEPENDENT:
				unit.reset(new IndependentUnit(basic));
				break;
			case UNIT_TYPE_UNITY:
				unit.reset(new UnityUnit(basic));
				break;
			case UNIT_TYPE_UNITY_RFID:
				unit.reset(new RfidUnit(basic));
				break;
			default:
				CanEx->Sync(sourceId, DeviceSync::SyncLive,  CANExtended::Trigger);
				return;
		}
		unit->ReadCommandResponse.bind(ethEngine.get(), &NetworkEngine::DeviceReadResponse);
		unit->WriteCommandResponse.bind(ethEngine.get(), &NetworkEngine::DeviceWriteResponse);
		CanEx->RegisterDevice(unit);
		if (updated)
		{	
			unitManager->Recover(sourceId&0x7f, unit);
#ifdef DEBUG_PRINT
			cout<<"#Recovered Device 0x"<<std::hex<<sourceId<<std::dec<<endl;
#endif
		}
		else
		{
			unitManager->Add(sourceId&0x7f, unit);
#ifdef DEBUG_PRINT
			cout<<"#Added Device 0x"<<std::hex<<sourceId<<std::dec<<endl;
#endif
		}
	}
	//CanEx->Sync(sourceId, DeviceSync::SyncLive, CANExtended::AutoSync); //Confirm & Start AutoSync
	CanEx->Sync(sourceId, DeviceSync::SyncLive, CANExtended::Trigger);
	if (updated)
		unitManager->SyncUpdate();
}

		//-----------------------------------------------------------------------------
		void Editor::Initialize(Main::IGame* pGame)
		{
				m_pGame = pGame;

				Core::IGameStateManager* pStateMan = m_pGame->GetGameStateManager();
				Main::GameStateTest::Create(pStateMan, "StateTest");

				pStateMan->Change(pStateMan->FindByName("StateTest"));

				m_pGrid.reset(m_pGame->GetEngine()->CreateGrid());
				m_pGrid->Initialze(100.f, 1.f);

				m_pCamManipulator.reset(m_pGame->GetEngine()->CreateCameraManipulator());
				m_pCamManipulator->Set(Ogre::Vector3(-50.f, 0.f, 50.f),Ogre::Vector3(0,0,0));
		}

~OmniNamesFixture() {
  BOOST_TEST_MESSAGE("== Test teardown [BEGIN]: Stopping OmniNames ==");
  bf::remove_all(logdir);

  if (processNamingService) {
    try {
     processNamingService->terminate();
     processNamingService->wait();
    } catch (...) {
      BOOST_TEST_MESSAGE("== Problem while ending OmniNames ==");
    }
  }
  boost::this_thread::sleep(boost::posix_time::milliseconds(SLEEP_TIME));
  BOOST_TEST_MESSAGE("== Test teardown [END]: Stopping OmniNames ==");
}

sound::InputStream*
Sound_as::attachAuxStreamerIfNeeded()
{
    media::AudioInfo* audioInfo =  _mediaParser->getAudioInfo();
    if (!audioInfo) return 0;

    // the following may throw an exception
    _audioDecoder.reset(_mediaHandler->createAudioDecoder(*audioInfo).release());

    // start playing ASAP, a call to ::start will just change _startTime
#ifdef GNASH_DEBUG_SOUND_AS
    log_debug("Attaching the aux streamer");
#endif
    return _soundHandler->attach_aux_streamer(getAudioWrapper, (void*) this);
}

    boost::shared_ptr<mongo::DBClientCursor> query(const std::string &json, int limit = 0, int skip = 0) {
        try {
            mongo::DBClientCursor *ptr = conn_->get()->query(ns_, mongo::Query(json), limit, skip).release();

            if (!ptr)
                throw conn_->get()->getLastError();

            return boost::shared_ptr<mongo::DBClientCursor>(ptr);
        } catch(mongo::DBException &de) {
            std::string err_msg = "Mongodb Plugin: ";
            err_msg += de.toString();
            err_msg += "\n";
            throw mapnik::datasource_exception(err_msg);
        }
    }

static void update_timestamp( bool force = false )
{
    static const boost::posix_time::time_duration timeout = boost::posix_time::seconds( 60 );
    static boost::posix_time::ptime last = boost::posix_time::microsec_clock::universal_time();
    boost::posix_time::ptime now = boost::posix_time::microsec_clock::universal_time();
    if( !force && ( now - last ) < timeout ) { return; }
    if( verbose ) { std::cerr << "sick-ldmrs-stream: setting time to " << boost::posix_time::to_iso_string( now ) << "..." << std::endl; }
    std::pair< comma::uint32, comma::uint32 > ntp = snark::timing::to_ntp_time( now );
    clear_fault();
    if( !protocol->write( sick::ibeo::commands::set_ntp_seconds( ntp.first ) ).ok() ) { COMMA_THROW( comma::exception, "failed to set NTP seconds" ); }
    clear_fault();
    if( !protocol->write( sick::ibeo::commands::set_ntp_fractions( ntp.second ) ).ok() ) { COMMA_THROW( comma::exception, "failed to set NTP fractions" ); }
    last = now;
    if( verbose ) { std::cerr << "sick-ldmrs-stream: set time to " << boost::posix_time::to_iso_string( now ) << std::endl; }
}

 void train(DataView2D<FeatureType> samples, 
            DataView2D<int> labels, 
            int* sample_idxes, 
            size_t n_samples, 
            SplitGenerator split_generator,
            RandomGen const& gen
            ) 
 {
     // copy generator
     RandomGen my_gen = gen;
     // initialize root node
     root_node.reset(new NodeT(0, params));
     // train root node (other notes get trained recursively)
     root_node->train(samples, labels, sample_idxes, n_samples, split_generator, my_gen);
 }

static void Traversal(void const *argument)  //Prevent missing status
{
	static bool forceReport = true;
	while(1)
	{
		if (!ethEngine->IsConnected())
		{
			forceReport = true;
			continue;
		}
		unitManager->Traversal(forceReport);	//Update all units
		forceReport = false;
		osThreadYield();
	}
}

	virtual bool last() {
		current = end;
		while (current != begin) {
			--current;

			nested_current.reset((*current)->lines());

			if (nested_current->last()) {
				valid = true;
				return true;
			}
		}

		valid = false;
		return false;
	}

static bool clear_fault()
{
    boost::optional< sick::ibeo::fault > fault = protocol->last_fault();
    if( !fault ) { return true; }
    std::cerr << "sick-ldmrs-stream: " << *fault << std::endl;
    return !fault->fatal();
}

    float similarity_path(float const* sample_1, float const* sample_2) {
        NodeT const* node_1 = root_node.get();
        NodeT const* node_2 = root_node.get();
        int n_common = 0;
        int n_1 = 1;
        int n_2 = 1;

        while (node_1 == node_2) {
            if (node_1->is_leaf) {
                return 1;
            }
            n_common++;
            node_1 = node_1->split(sample_1);
            node_2 = node_2->split(sample_2);
        }
        while (!node_1->is_leaf) {
            node_1 = node_1->split(sample_1);
            n_1++;
        }
        while (!node_2->is_leaf) {
            node_2 = node_2->split(sample_2);
            n_2++;
        }
        return n_common/sqrt((n_common + n_1)*(n_common + n_2));
    }