void GazeboRosSkidSteerDrive::cmdVelCallback(
      const geometry_msgs::Twist::ConstPtr& cmd_msg) {

    boost::mutex::scoped_lock scoped_lock(lock);
    x_ = cmd_msg->linear.x;
    rot_ = cmd_msg->angular.z;
  }

int swd::database::save_parameter(const int& request_id, const std::string& path,
 const std::string& value, const int& total_whitelist_rules,
 const int& critical_impact, const int& threat) {
    ensure_connection();

    boost::unique_lock<boost::mutex> scoped_lock(dbi_mutex_);

    char *path_esc = strdup(path.c_str());
    dbi_conn_quote_string(conn_, &path_esc);

    char *value_esc = strdup(value.c_str());
    dbi_conn_quote_string(conn_, &value_esc);

    dbi_result res = dbi_conn_queryf(conn_, "INSERT INTO parameters "
     "(request_id, path, value, total_whitelist_rules, critical_impact, threat) "
     "VALUES (%i, %s, %s, %i, %i, %i)", request_id, path_esc, value_esc,
     total_whitelist_rules, critical_impact, threat);

    free(path_esc);
    free(value_esc);

    if (!res) {
        throw swd::exceptions::database_exception("Can't execute parameter query");
    }

    int id = dbi_conn_sequence_last(conn_, "parameters_id_seq");

    dbi_result_free(res);

    return id;
}

JNIEnv *ThingsManagerJVM::getEnv()
{
    std::unique_lock<std::mutex> scoped_lock(m_currentThreadMutex);

    if (NULL == m_jvm)
    {
        LOGE("Failed to get JVM");
        return NULL;
    }

    JNIEnv *env = NULL;
    jint ret = m_jvm->GetEnv((void **)&env, JNI_CURRENT_VERSION);
    switch (ret)
    {
        case JNI_OK:
            return env;
        case JNI_EDETACHED:
            if (0 > m_jvm->AttachCurrentThread(&env, NULL))
            {
                LOGE("Failed to attach current thread to env");
                return NULL;
            }
            return env;
        case JNI_EVERSION:
            LOGE("JNI version not supported");
        default:
            LOGE("Failed to get the environment");
            return NULL;
    }
}

void CInfoConsole::AddLineHelper (int priority, const char *text)
{
	if (priority > verboseLevel)
		return;

	PUSH_CODE_MODE;
	ENTER_MIXED;
	boost::recursive_mutex::scoped_lock scoped_lock(infoConsoleMutex);

	char text2[50];
	if(strlen(text)>42){
		memcpy(text2,text,42);
		text2[42]=0;
	} else
		strcpy(text2,text);
	(*filelog) << text2  << "\n";
	filelog->flush();
	InfoLine l;
	if((int)data.size()>(numLines-1)){
		data[1].time+=data[0].time;
		data.pop_front();
	}
	data.push_back(l);
	data.back().text=text2;
	data.back().time=lifetime-lastTime;
	lastTime=lifetime;

	if(strlen(text)>42){
		AddLine("%s",&text[42]);
	}
	POP_CODE_MODE;
}

void EventManager::freePool(BaseEventPool* pool)
{
	boost::recursive_mutex::scoped_lock scoped_lock(mEventPoolMutex);
	
	pool->clear();
	mPoolQueue.push_back(pool);
}

  // Update the controller
  void YoubotArmController::UpdateChild()
  {
    boost::mutex::scoped_lock scoped_lock(lock);

    if(cmd_mode_ == CMD_POSITION)
    {
        arm_joints_[0]->SetAngle(0, math::Angle(cmd_.q1));
        arm_joints_[1]->SetAngle(0, math::Angle(cmd_.q2));
        arm_joints_[2]->SetAngle(0, math::Angle(cmd_.q3));
        arm_joints_[3]->SetAngle(0, math::Angle(cmd_.q4));
        arm_joints_[4]->SetAngle(0, math::Angle(cmd_.q5));
    }
    else if(cmd_mode_ == CMD_VELOCITY)
    {
        arm_joints_[0]->SetVelocity(0, cmd_.q1);
        arm_joints_[1]->SetVelocity(0, cmd_.q2);
        arm_joints_[2]->SetVelocity(0, cmd_.q3);
        arm_joints_[3]->SetVelocity(0, cmd_.q4);
        arm_joints_[4]->SetVelocity(0, cmd_.q5);
    }
    else if(cmd_mode_ == CMD_TORQUE)
    {
        arm_joints_[0]->SetForce(0, cmd_.q1);
        arm_joints_[1]->SetForce(0, cmd_.q2);
        arm_joints_[2]->SetForce(0, cmd_.q3);
        arm_joints_[3]->SetForce(0, cmd_.q4);
        arm_joints_[4]->SetForce(0, cmd_.q5);
    }
  }

bool
GazeboRosIMU::SetAccelBiasCallback(thinc_sim_gazebo_plugins::SetBias::Request &req, thinc_sim_gazebo_plugins::SetBias::Response &res)
{
  boost::mutex::scoped_lock scoped_lock(lock);
  accelModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z));
  return true;
}

void WorkerQueue::push(ServerWorker* worker){
  //mutex lock (scoped)
  boost::mutex::scoped_lock scoped_lock(mutex);
  worker->add();
  this->workers_list.push_back(worker);

}

Config* Config::Instance(){
	boost::mutex::scoped_lock scoped_lock(instanceMutex);

	if(theSingleInstance == NULL)
			theSingleInstance = new Config();
	return theSingleInstance;
}

void BCLog::Logger::LogPrintStr(const std::string &str)
{
    std::string strTimestamped = LogTimestampStr(str);

    if (m_print_to_console) {
        // print to console
        fwrite(strTimestamped.data(), 1, strTimestamped.size(), stdout);
        fflush(stdout);
    }
    if (m_print_to_file) {
        std::lock_guard<std::mutex> scoped_lock(m_file_mutex);

        // buffer if we haven't opened the log yet
        if (m_fileout == nullptr) {
            m_msgs_before_open.push_back(strTimestamped);
        }
        else
        {
            // reopen the log file, if requested
            if (m_reopen_file) {
                m_reopen_file = false;
                m_fileout = fsbridge::freopen(m_file_path, "a", m_fileout);
                if (!m_fileout) {
                    return;
                }
                setbuf(m_fileout, nullptr); // unbuffered
            }

            FileWriteStr(strTimestamped, m_fileout);
        }
    }
}

    int StatsTable::FindCounter(const std::string& name)
    {
        // Note: the API returns counters numbered from 1..N, although
        // internally, the array is 0..N-1.  This is so that we can return
        // zero as "not found".
        if(!impl_)
        {
            return 0;
        }

        // Create a scope for our auto-lock.
        {
            AutoLock scoped_lock(counters_lock_);

            // Attempt to find the counter.
            CountersMap::const_iterator iter;
            iter = counters_.find(name);
            if(iter != counters_.end())
            {
                return iter->second;
            }
        }

        // Counter does not exist, so add it.
        return AddCounter(name);
    }

bool OpenDebugLog()
{
    boost::call_once(&DebugPrintInit, debugPrintInitFlag);
    boost::mutex::scoped_lock scoped_lock(*mutexDebugLog);

    assert(fileout == nullptr);
    assert(vMsgsBeforeOpenLog);
    fs::path pathDebug = GetDebugLogPath();

    fileout = fsbridge::fopen(pathDebug, "a");
    if (!fileout) {
        return false;
    }

    setbuf(fileout, nullptr); // unbuffered
    // dump buffered messages from before we opened the log
    while (!vMsgsBeforeOpenLog->empty()) {
        FileWriteStr(vMsgsBeforeOpenLog->front(), fileout);
        vMsgsBeforeOpenLog->pop_front();
    }

    delete vMsgsBeforeOpenLog;
    vMsgsBeforeOpenLog = nullptr;
    return true;
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboPanel::Update()
{
  boost::mutex::scoped_lock scoped_lock(lock);

  if ( terminated_ )
    {
      return;
    }

  common::Time current_time = world_->GetSimTime();

  // check score
  double angle = joint_->GetAngle(0).Radian();
  if ( fmod(current_time.Double(), 1) == 0 ) {
    ROS_INFO_STREAM("Current time is " << current_time.Double() << ", Current angle is = " << angle);
  }

  // void Entity::GetNearestEntityBelow(double &_distBelow,  std::string &_entityName)
  //
  std::stringstream ss;
  std_msgs::String msg_time;
  ss << 20*60 - current_time.Double();
  msg_time.data = ss.str();
  pub_time_.publish(msg_time);
  if ( fabs(angle) > 3.14)
    {
      std_msgs::String msg_score, msg_time;
      msg_score.data = "Mission Completed";
      ROS_INFO_STREAM("Remaining time is " << msg_time.data << "[sec], Score is " << msg_score.data);
      pub_score_.publish(msg_score);
      terminated_ = true;
    }
}

void GazeboRosCarDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg ) {

    boost::mutex::scoped_lock scoped_lock ( lock );
    base_speed_cmd_ = cmd_msg->linear.x;
    base_omega_cmd_ = cmd_msg->angular.z;

}

int CNet::SendData(unsigned char *data, int length,int connection)
{
    if(playbackDemo && !serverNet)
        return 1;

    if(length<=0)
        logOutput.Print("Errenous send length in SendData %i",length);

    Connection* c=&connections[connection];
    if(c->active) {
        if(c->localConnection) {
            boost::mutex::scoped_lock scoped_lock(netMutex);
            Connection* lc=c->localConnection;
            if(lc->readyLength+length>=NETWORK_BUFFER_SIZE) {
                logOutput.Print("Overflow when sending to local connection %i %i %i %i %i",imServer,connection,lc->readyLength,length,NETWORK_BUFFER_SIZE);
                return 0;
            }
            memcpy(&lc->readyData[lc->readyLength],data,length);
            lc->readyLength+=length;
        } else {
            if(c->outgoingLength+length>=NETWORK_BUFFER_SIZE) {
                logOutput.Print("Overflow when sending to remote connection %i %i %i %i %i",imServer,connection,c->outgoingLength,length,NETWORK_BUFFER_SIZE);
                return 0;
            }
            memcpy(&c->outgoingData[c->outgoingLength],data,length);
            c->outgoingLength+=length;
        }
    }
    return 1;
}

// ============================================================================
// AcquisitionTask::set_state
// ============================================================================
void AcquisitionTask::set_state(Tango::DevState state) 
{
    {
        yat::MutexLock scoped_lock(m_status_lock);
        m_state = state;
    }
}

void swd::log::open_file(std::string file) {
	/* Use mutex to avoid race conditions. */
	boost::unique_lock<boost::mutex> scoped_lock(mutex_);

	/* Set the file. */
	file_ = file;
}

//this is called all the way from the receiver threads, we should process this swiftly
//and keep in mind that it is called crazily and concurently from lots of threads
//we MUST copy the data too
//also, this function can be called by threads owning clients_mutex !!!
void Broadcaster::QueueMessage(int type, int uid, unsigned int streamid, unsigned int len, const char *data) {
    if (!m_is_running) {
        return;
    }
    queue_entry_t msg = {type, uid, streamid, len, ""};
    memcpy(msg.data, data, len);

    {
        MutexLocker scoped_lock(m_queue_mutex);
        if (m_msg_queue.empty()) {
            m_packet_drop_counter = 0;
            m_is_dropping_packets = (++m_packet_good_counter > 3) ? false : m_is_dropping_packets;
        } else if (type == RoRnet::MSG2_STREAM_DATA_DISCARDABLE) {
            auto search = std::find_if(m_msg_queue.begin(), m_msg_queue.end(), [&](const queue_entry_t& m)
                    { return m.type == RoRnet::MSG2_STREAM_DATA_DISCARDABLE && m.uid == uid && m.streamid == streamid; });
            if (search != m_msg_queue.end()) {
                // Found outdated discardable streamdata -> replace it
                (*search) = msg;
                m_packet_good_counter = 0;
                m_is_dropping_packets = (++m_packet_drop_counter > 3) ? true : m_is_dropping_packets;
                Messaging::StatsAddOutgoingDrop(sizeof(RoRnet::Header) + msg.datalen); // Statistics
                return;
            }
        }
        m_msg_queue.push_back(msg);
    }

    m_queue_cond.signal();
}

////////////////////////////////////////////////////////////////////////////////
// returns true always, imu is always calibrated in sim
bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
                                        std_srvs::Empty::Response &res)
{
  boost::mutex::scoped_lock scoped_lock(lock);
  rateModel.reset();
  return true;
}

sp<IAppOpsService> AppOpsManager::getService()
{

    std::lock_guard<Mutex> scoped_lock(mLock);
    int64_t startTime = 0;
    sp<IAppOpsService> service = mService;
    while (service == NULL || !IInterface::asBinder(service)->isBinderAlive()) {
        sp<IBinder> binder = defaultServiceManager()->checkService(_appops);
        if (binder == NULL) {
            // Wait for the app ops service to come back...
            if (startTime == 0) {
                startTime = uptimeMillis();
                ALOGI("Waiting for app ops service");
            } else if ((uptimeMillis()-startTime) > 10000) {
                ALOGW("Waiting too long for app ops service, giving up");
                service = NULL;
                break;
            }
            sleep(1);
        } else {
            service = interface_cast<IAppOpsService>(binder);
            mService = service;
        }
    }
    return service;
}