text:
// Copyright 2021, Yang Luo"
// 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 3 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, see <http://www.gnu.org/licenses/>.
//
// @Author
// Yang Luo, PHD
// Shenyang Institute of Automation, Chinese Academy of Sciences.
// email: luoyang@sia.cn

#include <include/hardware_interface.h>

#include <boost/core/ignore_unused.hpp>

namespace rocos {

    HardwareInterface::~HardwareInterface() = default;

    HardwareInterface::HWType rocos::HardwareInterface::getHardwareType() {
        return _type;
    }

    std::string HardwareInterface::getHardwareTypeString(HardwareInterface::HWType type) {
        std::string str;
        switch (type) {
            case HW_UNKNOWN:
                str = "Unknown";
                break;
            case HW_ETHERCAT:
                str = "EtherCAT";
                break;
            case HW_SIM:
                str = "Simulation";
                break;
            default:
                break;
        }
        return str;
    }

    Statusword HardwareInterface::getStatusword(int id) {
        return Statusword();
    }

    uint16_t HardwareInterface::getStatuswordRaw(int id) {
        return Statusword().getRawStatusword();
    }

    void HardwareInterface::setTargetPositionRaw(int id, int32_t pos) {
        boost::ignore_unused(id, pos);
    }

    void HardwareInterface::setTargetVelocityRaw(int id, int32_t vel) {
        boost::ignore_unused(id, vel);
    }

    void HardwareInterface::setTargetTorqueRaw(int id, int16_t tor) {
        boost::ignore_unused(id, tor);
    }

    void HardwareInterface::setControlwordRaw(int id, uint16_t ctrlwd) {
        boost::ignore_unused(id, ctrlwd);
    }

    void HardwareInterface::setModeOfOperationRaw(int id, int8_t mode) {
        boost::ignore_unused(id, mode);
    }

    void HardwareInterface::setModeOfOperation(int id, ModeOfOperation modeOfOperation) {
        boost::ignore_unused(id, modeOfOperation);
    }

    int32_t HardwareInterface::getActualPositionRaw(int id) {
        return 0;
    }

    int32_t HardwareInterface::getActualVelocityRaw(int id) {
        return 0;
    }

    int16_t HardwareInterface::getActualTorqueRaw(int id) {
        return 0;
    }

    int16_t HardwareInterface::getLoadTorqueRaw(int id) {
        boost::ignore_unused(id);
        return 0;
    }

    DriveState HardwareInterface::getDriverState(int id) {
        boost::ignore_unused(id);
        return DriveState();
    }

    Timestamp HardwareInterface::getTimestamp() {
        return rocos::Timestamp();
    }

    double HardwareInterface::getMinCycleTime() {
        return 0;
    }

    double HardwareInterface::getMaxCycleTime() {
        return 0;
    }

    double HardwareInterface::getAvgCycleTime() {
        return 0;
    }

    double HardwareInterface::getCurrCycleTime() {
        return 0;
    }

    int32_t HardwareInterface::getSlaveNumber() {
        return 0;
    }

    void HardwareInterface::waitForSignal(int id) {
        usleep(1000);
    }

    std::string HardwareInterface::getSlaveName(int id) {
        return std::string();
    }

}

ext: cc

max_stars_repo_name: rocos-sia/rocos-app


text:
// File: WMReal.cpp
// Authors: Francesco Giacomini <Francesco.Giacomini@cnaf.infn.it>
//          Marco Cecchi <Marco.Cecchi@cnaf.infn.it>
// Copyright (c) Members of the EGEE Collaboration. 2009.
// See http://www.eu-egee.org/partners/ for details on the copyright holders.

// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//     http://www.apache.org/licenses/LICENSE-2.0
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// $Id: wm_real.cpp,v 1.1.2.4.2.7.2.3.2.2.2.7.4.3 2012/03/29 13:31:42 mcecchi Exp $

#include <syslog.h>
#include <algorithm>
#include <boost/bind.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/regex.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/random.hpp>

#include "glite/lb/producer.h"

#include "glite/wms/common/configuration/Configuration.h"
#include "common/src/configuration/WMConfiguration.h"
#include "common/src/configuration/CommonConfiguration.h"
#include "common/src/configuration/NSConfiguration.h"
#include "common/src/configuration/JCConfiguration.h"
#include "common/src/configuration/ICEConfiguration.h"

#include "glite/wms/common/logger/logger_utils.h"

#include "common/src/utilities/boost_fs_add.h"
#include "glite/wms/common/utilities/scope_guard.h"

#include "purger/src/purger.h"
#include "glite/security/proxyrenewal/renewal.h"

#include "glite/jdl/PrivateAttributes.h"
#include "glite/jdl/JobAdManipulation.h"
#include "glite/jdl/PrivateAttributes.h"
#include "glite/jdl/PrivateAdManipulation.h"
#include "glite/jdl/ManipulationExceptions.h"
#include "glite/jdl/DAGAd.h"

#include "glite/wms/helper/Request.h"

#include "glite/jobid/JobId.h"
#include "common/src/utilities/manipulation.h"
#include "glite/wmsutils/classads/classad_utils.h"

#include "glite/security/proxyrenewal/renewal.h"

#include "plan.h"
#include "wm_real.h"
#include "lb_utils.h"
#include "submission_utils.h"

namespace utilities = glite::wms::common::utilities;
namespace configuration = glite::wms::common::configuration;
namespace jobid = glite::jobid;
namespace ca = glite::wmsutils::classads;
namespace jdl = glite::jdl;
namespace fs = boost::filesystem;

namespace glite {
namespace wms {
namespace manager {
namespace server {

namespace {

bool have_lbproxy()
{
  configuration::Configuration const& config(
    *configuration::Configuration::instance()
  );
  return config.common()->lbproxy();
}

std::string get_ice_input()
{
  configuration::Configuration const& config(
    *configuration::Configuration::instance()
  );
  return config.ice()->input();
}

std::string get_jc_input()
{
  configuration::Configuration const& config(
    *configuration::Configuration::instance()
  );
  return config.jc()->input();
}

fs::path
get_classad_file(std::string const& job_id)
{
  configuration::Configuration const& config(
    *configuration::Configuration::instance()
  );

  fs::path f_classad(
    utilities::normalize_path(config.jc()->submit_file_dir()),
    fs::native
  );

  fs::path reduced(
    utilities::get_reduced_part(job_id),
    fs::native
  );
  f_classad /= reduced;

  fs::path cname(
    "ClassAd." + utilities::to_filename(job_id),
    fs::native
  );
  f_classad /= cname;

  return f_classad;
}

std::string get_wm_input()
{
  configuration::Configuration const& config(
    *configuration::Configuration::instance()
  );
  return config.wm()->input();
}

} // {anonymous}

struct WMReal::Impl {
  Impl(boost::shared_ptr<utilities::JobDir> to_jc_,
       boost::shared_ptr<utilities::JobDir> to_ice_)
    : ice_output(get_ice_input()), to_ice(to_ice_),
      jc_output(get_jc_input()), to_jc(to_jc_) { }
  std::string ice_output;
  boost::shared_ptr<utilities::JobDir> to_ice;
  std::string jc_output;
  boost::shared_ptr<utilities::JobDir> to_jc;
};

WMReal::WMReal(
  boost::shared_ptr<utilities::JobDir> to_jc,
  boost::shared_ptr<utilities::JobDir> to_ice)
    : m_impl(new Impl(to_jc, to_ice)) { }

namespace {

classad::ClassAd
submit_command_create(std::auto_ptr<classad::ClassAd> job_ad, bool is_replan)
{
  classad::ClassAd result;

  result.InsertAttr("Protocol", std::string("1.0.0"));
  if (is_replan) {
    result.InsertAttr("Command", std::string("Reschedule"));
  } else {
    result.InsertAttr("Command", std::string("Submit"));
  }
  result.InsertAttr("Source", 2);
  std::auto_ptr<classad::ClassAd> args(new classad::ClassAd);
  args->Insert("JobAd", job_ad.get());
  job_ad.release();
  result.Insert("Arguments", args.get());
  args.release();

  return result;
}

}

void
WMReal::submit(std::auto_ptr<classad::ClassAd> planned_ad, ContextPtr context, bool is_replan)
{
  std::string const ce_id = jdl::get_ce_id(*planned_ad);
  log_match(context, ce_id);

  std::string ce_info_hostname;
  {
    bool no_throw;
    ce_info_hostname = jdl::get_ceinfo_host_name(*planned_ad, no_throw);
    if (!ce_info_hostname.empty()) {
      log_usertag(context, jdl::JDLPrivate::CE_INFO_HOST, ce_info_hostname);
    }
  }

  boost::regex const cream_ce_id(".+/cream-.+");
  bool const is_cream_ce = boost::regex_match(ce_id, cream_ce_id);

  std::string const output(
    is_cream_ce ? m_impl->ice_output : m_impl->jc_output
  );

  log_enqueued_start(context, output);

  std::string const sequence_code_(get_lb_sequence_code(context));
  jdl::set_lb_sequence_code(*planned_ad, sequence_code_);

  classad::ClassAd const cmd(submit_command_create(planned_ad, is_replan));
  std::string const ad_str = ca::unparse_classad(cmd);

  bool success = false;
  std::string error;

  edg_wlc_JobId c_jobid;
  int res = edg_wll_GetLoggingJob(context.get(), &c_jobid);
  if (!res) {
    jobid::JobId jobid(c_jobid);
    edg_wlc_JobIdFree(c_jobid);
    fs::path const cancel_token(get_cancel_token(jobid));
    if (fs::exists(cancel_token)) {
      // right before passing on to the next component in the submission chain
      log_cancelled(context);
      ::unlink(cancel_token.native_file_string().c_str());
      purger::Purger thePurger(have_lbproxy());
      thePurger.force_dag_node_removal();
      thePurger(jobid.toString());

      return;
    }

    // Logging jobid and destination on syslog
    std::string syslog_msg(
      "jobid=" + boost::lexical_cast<std::string>(jobid.toString()) += ", "
    );
    syslog_msg += "destination=" + ce_id;
    syslog(LOG_NOTICE, "%s", syslog_msg.c_str());
  } // no race condition if this check is missed, the cancel command
    // will be passed on next time the wm cancel request is considered

  if (is_cream_ce) {
    try {
      m_impl->to_ice->deliver(ad_str, boost::lexical_cast<std::string>(std::rand()));
      success = true;
    } catch (utilities::JobDirError const& e) {
      error = e.what();
    }
  } else { // ! cream CE
    try {
      m_impl->to_jc->deliver(ad_str, boost::lexical_cast<std::string>(std::rand()));
      success = true;
    } catch (utilities::JobDirError const& e) {
      error = e.what();
    }
  }

  if (success) {
    log_enqueued_ok(context, output, ad_str);
  } else {
    log_enqueued_fail(context, output, ad_str, error);
    throw CannotDeliverFatal(error);
  }
}

void
WMReal::submit(
  classad::ClassAd const& jdl,
  ContextPtr context,
  boost::shared_ptr<std::string> jw_template,
  bool is_replan
) {
  // we make a copy because we change the sequence code
  classad::ClassAd request_ad(jdl);

  std::string const sequence_code(get_lb_sequence_code(context));

  // before doing the planning, some helpers may need a
  // more recent sequence code than what appears in
  // the classad originally received
  jdl::set_lb_sequence_code(request_ad, sequence_code);

  std::auto_ptr<classad::ClassAd> planned_ad(Plan(request_ad, jw_template));
  submit(planned_ad, context, is_replan);
}

void
WMReal::submit_collection(
  classad::ClassAd const& jdl,
  ContextPtr context,
  PendingJobs& pending,
  boost::shared_ptr<std::string> jw_template
)
{
  ScheduledJobs jobs;
  do_bulk_mm(jdl, jobs, pending, jw_template);

  std::string const sequence_code(get_lb_sequence_code(context));

  ScheduledJobs::const_iterator job_b = jobs.begin();
  ScheduledJobs::const_iterator const job_e = jobs.end();

  for( ; job_b != job_e ; ++job_b ) {

    ClassAdPtr planned_ad = boost::tuples::get<planned_tag>(*job_b);

    std::string const brokerinfo_file(
      jdl::get_input_sandbox_path(*planned_ad) + "/.BrokerInfo"
    );

    ClassAdPtr brokerinfo(boost::tuples::get<brokerinfo_tag>(*job_b));
    std::ofstream bifs(brokerinfo_file.c_str());
    if (brokerinfo) {
      bifs << *brokerinfo << '\n';
    }
    bifs.close();

    jobid::JobId const job_id(
      jdl::get_edg_jobid(*planned_ad)
    );

    change_logging_job(context, sequence_code, job_id);

    change_logging_src(context, EDG_WLL_SOURCE_NETWORK_SERVER);
    if (
      !log_enqueued_ok(
        context,
        get_wm_input(),
        ca::unparse_classad(*planned_ad)
      )
    ) {
      Debug("log 'enqueued OK' failed for collection node");
    }
    change_logging_src(context, EDG_WLL_SOURCE_WORKLOAD_MANAGER);

    if (shallow_resubmission_is_enabled(*planned_ad)) {
      fs::path const token_file(get_reallyrunning_token(job_id, 0));
      create_token(token_file);
    }
    std::auto_ptr<classad::ClassAd> adapted_ad(
      static_cast<classad::ClassAd*>(boost::tuples::get<adapted_tag>(*job_b)->Copy())
    );
    submit(adapted_ad, context, 0 /* not a replan */);
  }

  if (pending.empty()) {
    return;
  }

  jdl::DAGAd dagad(jdl);
  jdl::DAGAdNodeIterator first_node, last_node;
  boost::tie(first_node, last_node) = dagad.nodes();

  PendingJobs::iterator p_first = pending.begin();
  PendingJobs::iterator const p_last = pending.end();

  change_logging_src(context, EDG_WLL_SOURCE_NETWORK_SERVER);

  for ( ; p_first != p_last; ++p_first) {

    std::string node_name = *p_first;
    jdl::DAGAdNodeIterator it = dagad.find(node_name);
    assert(it != last_node);
    jdl::DAGNodeInfo const& node_info = it->second;
    classad::ClassAd node_jdl(
      *node_info.description_ad()
    );
    std::string const id_str = jdl::get_edg_jobid(node_jdl);
    jobid::JobId const id(id_str);

    change_logging_job(context, "", id);
    if (
      !log_enqueued_ok(
        context,
        get_wm_input(),
        ca::unparse_classad(node_jdl)
    )) {
      Debug("log 'enqueued OK' failed for collection node");
    }
  }

  change_logging_src(context, EDG_WLL_SOURCE_WORKLOAD_MANAGER);
}

namespace {

classad::ClassAd
cancel_command_create(
  std::string const& job_id,
  std::string const& sequence_code,
  std::string const& user_x509_proxy
)
{
  classad::ClassAd result;

  result.InsertAttr("Protocol", std::string("1.0.0"));
  result.InsertAttr("Command", std::string("Cancel"));
  result.InsertAttr("Source", 2);

  std::auto_ptr<classad::ClassAd> args(new classad::ClassAd);
  args->InsertAttr("Force", false);

  std::string const ad_file = get_classad_file(job_id).native_file_string();
  std::ifstream ifs(ad_file.c_str());
  if (ifs) {
    classad::ClassAdParser parser;
    classad::ClassAd ad;
    if (parser.ParseClassAd(ifs, ad)) {
      bool good = false;
      std::string const log_file(jdl::get_log(ad, good));
      if (!log_file.empty() && good) {
        args->InsertAttr("LogFile", log_file);
      }
    }
  }

  args->InsertAttr("ProxyFile", user_x509_proxy);
  args->InsertAttr("SequenceCode", sequence_code);
  args->InsertAttr("JobId", job_id);

  result.Insert("Arguments", args.get());
  args.release();

  return result;
}

}

void
WMReal::cancel(jobid::JobId const& id, ContextPtr context)
{
  std::string const sequence_code(get_lb_sequence_code(context));

  classad::ClassAd cmd(
    cancel_command_create(
      id.toString(),
      sequence_code,
      get_user_x509_proxy(id)
    )
  );

  std::string const ad_str(ca::unparse_classad(cmd));

  try {
    m_impl->to_ice->deliver(ad_str);
  } catch (utilities::JobDirError const& e) {
  }

  try {
    m_impl->to_jc->deliver(ad_str);
  } catch (utilities::JobDirError const& e) {
  }
}

}}}}

ext: cpp

max_stars_repo_name: italiangrid/wms

max_issues_repo_name: italiangrid/wms

max_forks_repo_name: italiangrid/wms


text:
/*
 *  default_encoding_factory.hpp
 *
 *  Created on: 17-09-2015
 *      Author: Karol Dzitkowski
 */

#ifndef DDJ_DEFAULT_ENCODING_FACTORY_HPP_
#define DDJ_DEFAULT_ENCODING_FACTORY_HPP_

#include "compression/encoding.hpp"
#include "compression/encoding_type.hpp"
#include "core/not_implemented_exception.hpp"

#include "compression/delta/delta_encoding.hpp"
#include "compression/dict/dict_encoding.hpp"
#include "compression/none/none_encoding.hpp"
#include "compression/patch/patch_encoding_factory.hpp"
#include "compression/rle/rle_encoding.hpp"
#include "compression/scale/scale_encoding.hpp"
#include "compression/unique/unique_encoding.hpp"
#include "compression/afl/afl_encoding.hpp"
#include "compression/const/const_encoding.hpp"
#include "compression/float/float_encoding.hpp"
#include "compression/gfc/gfc_encoding.hpp"

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

namespace ddj {

class DefaultEncodingFactory
{

public:
	static boost::shared_ptr<EncodingFactory> Get(EncodingType encodingType, DataType dataType)
	{
		switch(encodingType)
		{
			case EncodingType::delta:
				return boost::make_shared<DeltaEncodingFactory>(dataType);
			case EncodingType::dict:
				return boost::make_shared<DictEncodingFactory>(dataType);
			case EncodingType::none:
				return boost::make_shared<NoneEncodingFactory>(dataType);
			case EncodingType::patch:
				switch(dataType)
				{
					case DataType::d_int:
						return boost::make_shared<PatchEncodingFactory<int>>(dataType, PatchType::lower);
					case DataType::d_time:
						return boost::make_shared<PatchEncodingFactory<time_t>>(dataType, PatchType::lower);
					case DataType::d_float:
						return boost::make_shared<PatchEncodingFactory<float>>(dataType, PatchType::lower);
					case DataType::d_double:
						return boost::make_shared<PatchEncodingFactory<double>>(dataType, PatchType::lower);
					case DataType::d_short:
						return boost::make_shared<PatchEncodingFactory<short>>(dataType, PatchType::lower);
					case DataType::d_char:
						return boost::make_shared<PatchEncodingFactory<char>>(dataType, PatchType::lower);
					default:
						throw NotImplementedException("Encoding of this type not implemented");
				}
				break;
			case EncodingType::scale:
				return boost::make_shared<ScaleEncodingFactory>(dataType);
			case EncodingType::rle:
				return boost::make_shared<RleEncodingFactory>(dataType);
			case EncodingType::unique:
				return boost::make_shared<UniqueEncodingFactory>(dataType);
			case EncodingType::afl:
				return boost::make_shared<AflEncodingFactory>(dataType);
			case EncodingType::gfc:
				return boost::make_shared<GfcEncodingFactory>(dataType);
			case EncodingType::constData:
				return boost::make_shared<ConstEncodingFactory>(dataType);
			case EncodingType::floatToInt:
				return boost::make_shared<FloatEncodingFactory>(dataType);
			default:
				throw NotImplementedException("Encoding of this type not implemented");
		}
	}
};

} /* namespace ddj */
#endif /* DDJ_DEFAULT_ENCODING_FACTORY_HPP_ */

ext: hpp

max_stars_repo_licenses: MIT Unlicense

max_issues_repo_licenses: MIT Unlicense

max_forks_repo_licenses: MIT Unlicense


text:
/* Author: Mincheul Kang */

#include <harmonious_sampling/HarmoniousLazyPRMstarMulti.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/goals/GoalSampleableRegion.h>
#include <ompl/util/GeometricEquations.h>
#include <ompl/geometric/planners/prm/ConnectionStrategy.h>
#include <ompl/tools/config/SelfConfig.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>

#include <boost/lambda/bind.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/foreach.hpp>
#include <fstream>
#include <queue>
#include <stack>
#include <utility>
#include <numeric>
#include <stdio.h>
#include <iostream>

#define foreach BOOST_FOREACH

namespace ompl {
    namespace magic {
/** \brief The number of nearest neighbors to consider by
      default in the construction of the PRM roadmap */
        static const unsigned int DEFAULT_NEAREST_NEIGHBORS_LAZY = 5;

/** \brief When optimizing solutions with lazy planners, this is the minimum
      number of path segments to add before attempting a new optimized solution
          extraction */
        static const unsigned int MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION = 5;
    }
}

ompl::geometric::HarmoniousLazyPRMstarMulti::HarmoniousLazyPRMstarMulti(const base::SpaceInformationPtr &si,
                                                                        base::HarmoniousSampler &hs,
                                                                        const std::vector<bool> &isContinuous,
                                                                        bool starStrategy) :
        base::Planner(si, "HarmoniousLazyPRMstarMulti"),
        starStrategy_(starStrategy),
        userSetConnectionStrategy_(false),
        maxDistance_(0.0),
        indexProperty_(boost::get(boost::vertex_index_t(), g_)),
        stateProperty_(boost::get(vertex_state_t(), g_)),
        radiusProperty_(boost::get(vertex_radius_t(), g_)),
        witnessProperty_(boost::get(vertex_witness_t(), g_)),
        costProperty_(boost::get(vertex_cost_t(), g_)),
        childrenProperty_(boost::get(vertex_children_t(), g_)),
        predecessorProperty_(boost::get(boost::vertex_predecessor_t(), g_)),
        colorProperty_(boost::get(boost::vertex_color_t(), g_)),
        weightProperty_(boost::get(boost::edge_weight_t(), g_)),
        vertexValidityProperty_(boost::get(vertex_flags_t(), g_)),
        edgeValidityProperty_(boost::get(edge_flags_t(), g_)),
        bestCost_(std::numeric_limits<double>::quiet_NaN()),
        iterations_(0),
        increaseIterations_(0),
        BisectionCC_(true),
        rewireFactor_(1.5),
        hs_(hs),
        isContinuous_(isContinuous)
{
    specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
    specs_.approximateSolutions = false;
    specs_.optimizingPaths = true;

    Planner::declareParam<bool>("BisectionCC", this,
                                &HarmoniousLazyPRMstarMulti::setBisectionCC, std::string("."));
    Planner::declareParam<double>("RewireFactor", this,
                                  &HarmoniousLazyPRMstarMulti::setRewireFactor, std::string("."));

    addPlannerProgressProperty("iterations INTEGER",
                               std::bind(&HarmoniousLazyPRMstarMulti::getIterationCount, this));
    addPlannerProgressProperty("best cost REAL",
                               std::bind(&HarmoniousLazyPRMstarMulti::getBestCost, this));
    addPlannerProgressProperty("milestone count INTEGER",
                               std::bind(&HarmoniousLazyPRMstarMulti::getMilestoneCountString, this));
    addPlannerProgressProperty("edge count INTEGER",
                               std::bind(&HarmoniousLazyPRMstarMulti::getEdgeCountString, this));
}

ompl::geometric::HarmoniousLazyPRMstarMulti::~HarmoniousLazyPRMstarMulti() {
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::setup() {
    Planner::setup();
    tools::SelfConfig sc(si_, getName());
    sc.configurePlannerRange(maxDistance_);

    if (!nn_) {
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nn_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!nnB_) {
        nnB_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nnB_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!nnM_) {
        nnM_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nnM_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!connectionStrategy_) {
//        if (starStrategy_)
//            connectionStrategy_ = KStarStrategy<Vertex>(std::bind(&HarmoniousLazyPRMstarMulti::milestoneCount, this), nn_, si_->getStateDimension());
//        else
//            connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
    }

    double dimDbl = static_cast<double>(si_->getStateDimension());
    // double prunedMeasure_ = si_->getSpaceMeasure();

    k_rrgConstant_ = rewireFactor_ * boost::math::constants::e<double>() + (boost::math::constants::e<double>() / dimDbl);

    // Setup optimization objective
    //
    // If no optimization objective was specified, then default to
    // optimizing path length as computed by the distance() function
    // in the state space.
    if (pdef_) {
        if (pdef_->hasOptimizationObjective()) {
            opt_ = pdef_->getOptimizationObjective();
        }
        else {
            opt_.reset(new base::PathLengthOptimizationObjective(si_));

            if (!starStrategy_) {
                opt_->setCostThreshold(opt_->infiniteCost());
            }
        }
    }
    else {
        OMPL_ERROR("%s: problem definition is not set, deferring setup completion...\n", getName().c_str());
        setup_ = false;
    }
    sampler_ = si_->allocStateSampler();
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::setRange(double distance) {
    maxDistance_ = distance;

    if (!userSetConnectionStrategy_) {
        connectionStrategy_.clear();
    }

    if (isSetup()) {
        setup();
    }
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::setMaxNearestNeighbors(unsigned int k) {
    if (starStrategy_) {
        throw Exception("Cannot set the maximum nearest neighbors for " + getName());
    }

    if (!nn_) {
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nn_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this,std::placeholders::_1, std::placeholders::_2));
    }

    if (!nnB_) {
        nnB_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nnB_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!nnM_) {
        nnM_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nnM_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!userSetConnectionStrategy_) {
        connectionStrategy_.clear();
    }

    if (isSetup()) {
        setup();
    }
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::setProblemDefinition(const base::ProblemDefinitionPtr &pdef) {
    Planner::setProblemDefinition(pdef);
    clearQuery();
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::clearQuery() {
    startM_.clear();
    goalM_.clear();
    pis_.restart();
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::clear() {
    Planner::clear();
    freeMemory();

    if (nn_) {
        nn_->clear();
    }

    if (nnB_) {
        nnB_->clear();
    }

    if (nnM_) {
        nnM_->clear();
    }

    clearQuery();

    iterations_ = 0;
    bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::freeMemory() {
    foreach (Vertex v, boost::vertices(g_)) {
        si_->freeState(stateProperty_[v]);
    }

    g_.clear();
}

// Add newly sampled vertex and its adjancency edges connected to neigh neighbors.
ompl::geometric::HarmoniousLazyPRMstarMulti::Vertex ompl::geometric::HarmoniousLazyPRMstarMulti::addMilestone(base::State *state, bool isM, bool isChecked) {
    Vertex m = boost::add_vertex(g_);
    stateProperty_[m] = state;
    radiusProperty_[m] = std::numeric_limits<double>::infinity();
    costProperty_[m] = std::numeric_limits<double>::infinity();
    childrenProperty_[m] = new std::vector<Vertex>();
    witnessProperty_[m] = NULL;
    predecessorProperty_[m] = NULL;
    colorProperty_[m] = 0;
    vertexValidityProperty_[m] = (isChecked) ? VALIDITY_TRUE : VALIDITY_UNKNOWN;

    std::vector<Vertex> neighbors;
    std::vector<double> neighbors_costs;

    if(isM){
        // manipulation regions
        unsigned long int bSize = nnB_->size();
        unsigned int max_number_of_neighbors = std::ceil(k_rrgConstant_ * log(static_cast<double>(milestoneCount()-bSize) + 1u));
        neighbors.reserve(max_number_of_neighbors);
        neighbors_costs.reserve(max_number_of_neighbors);

        nnM_->nearestK(m, max_number_of_neighbors, neighbors);

        foreach (Vertex n, neighbors) {
            const double weight = distanceFunctionHarmonious(n, m);

            ompl::base::Cost cost_weight(weight);
            const Graph::edge_property_type properties(cost_weight);

            neighborProperty_[m].push_back(type_neighbor(n, weight));
            neighborProperty_[n].push_back(type_neighbor(m, weight));

            // If collision-free or optimized well,
            const Edge &e = boost::add_edge(n, m, properties, g_).first;
            edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
        }

        // base regions
        max_number_of_neighbors = std::ceil(k_rrgConstant_ * log(static_cast<double>(bSize) + 1u));

        neighbors.reserve(max_number_of_neighbors);
        neighbors_costs.reserve(max_number_of_neighbors);

        nnB_->nearestK(m, max_number_of_neighbors, neighbors);

        foreach (Vertex n, neighbors) {
            const double weight = distanceFunctionHarmonious(n, m);

            ompl::base::Cost cost_weight(weight);
            const Graph::edge_property_type properties(cost_weight);

            neighborProperty_[m].push_back(type_neighbor(n, weight));
            neighborProperty_[n].push_back(type_neighbor(m, weight));

            // If collision-free or optimized well,
            const Edge &e = boost::add_edge(n, m, properties, g_).first;
            edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
        }

        nnM_->add(m);
    }
    else{
        unsigned int max_number_of_neighbors = std::ceil(k_rrgConstant_ * log(static_cast<double>(milestoneCount()) + 1u));

        neighbors.reserve(max_number_of_neighbors);
        neighbors_costs.reserve(max_number_of_neighbors);

        nn_->nearestK(m, max_number_of_neighbors, neighbors);

        foreach (Vertex n, neighbors) {
            const double weight = distanceFunctionHarmonious(n, m);

            ompl::base::Cost cost_weight(weight);
            const Graph::edge_property_type properties(cost_weight);

            neighborProperty_[m].push_back(type_neighbor(n, weight));
            neighborProperty_[n].push_back(type_neighbor(m, weight));

            // If collision-free or optimized well,
            const Edge &e = boost::add_edge(n, m, properties, g_).first;
            edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
        }

        nnB_->add(m);
    }

    nn_->add(m);

    return m;
}

ompl::base::PlannerStatus ompl::geometric::HarmoniousLazyPRMstarMulti::solve(const base::PlannerTerminationCondition &ptc) {
    // Initial checkup for start/goal configurations.
    checkValidity();

    // Add the valid start states as milestones
    while (const base::State *st = pis_.nextStart()) {
        Vertex st_vert = addMilestone(si_->cloneState(st), false);
        costProperty_[st_vert] = 0.0; // Initialize with 0 cost.
        startM_.push_back(st_vert);
    }

    if (startM_.size() == 0) {
        OMPL_ERROR("error-%s: There are no valid initial states!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    unsigned long int nrStartStates = boost::num_vertices(g_);
    OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);

    bestCost_ = opt_->infiniteCost();
    base::State *workState = si_->allocState();
    bool fullyOptimized = false;
    base::PathPtr bestSolution;

    base::RealVectorStateSpace::StateType *rstate = static_cast<base::RealVectorStateSpace::StateType*>(workState);

    Vertex startV = startM_[0];

    do{
        if(hs_.makeGoal(si_, workState)){
            goalM_.push_back(addMilestone(si_->cloneState(workState), true));
        }
    } while(goalM_.size() == 0 && ptc == false);

    bool isM = false;
    while (ptc == false) {
        ++iterations_;

        int r_val = std::rand() % 100;
        if(r_val < hs_.params_.prob_goal_ && goalM_.size() < hs_.params_.max_num_goal_){
            if(hs_.makeGoal(si_, workState)){
                goalM_.push_back(addMilestone(si_->cloneState(workState), true));
            }
            else{
                continue;
            }
        }
        else if(r_val < hs_.params_.prob_uniform_){
            hs_.uniformSampling(workState);
            if(hs_.getReachable(workState)){
                isM = true;
            }
            else{
                isM = false;
            }
        }
        else{
            hs_.biasedSampling(workState);

            if(hs_.getReachable(workState)){
                hs_.jointSampling(workState);
                isM = true;
            }
            else{
                hs_.setBasicPose(workState);
                isM = false;
            }
        }

        if (!si_->isValid(workState)) {
            continue;
        }

        // Add collision-free vertices.
        Vertex addedVertex = addMilestone(si_->cloneState(workState), isM);

        // DSPT update.
        Decrease(addedVertex);

        // Only support a single pair of start and goal node.
        base::PathPtr solution;

        for (int i = 0; i < (int)goalM_.size(); i++) {
            Vertex goalV = goalM_[i];
            do {
                if (predecessorProperty_[goalV] == NULL || bestCost_.value() <= costProperty_[goalV])
                    break;
                solution = constructSolution(startV, goalV);
            } while (!solution);

            if (solution) {
                base::Cost c(costProperty_[goalV]);

                if (opt_->isCostBetterThan(c, bestCost_)) {
                    bestSolution = solution;

                    bestCost_ = c;
                }
            }
        }
    }
    if (goalM_.empty()) {
        OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
        return base::PlannerStatus::INVALID_GOAL;
    }

    si_->freeState(workState);

    if (bestSolution) {
        base::PlannerSolution psol(bestSolution);
        psol.setPlannerName(getName());
        // If the solution was optimized, we mark it as such
        psol.setOptimized(opt_, bestCost_, fullyOptimized);
        pdef_->addSolutionPath(psol);
    }

    OMPL_INFORM("%s: Created %lu vertices and %lu edges.", getName().c_str(), boost::num_vertices(g_) - 1, boost::num_edges(g_));
    OMPL_INFORM("%s: Goalstates %lu ", getName().c_str(), goalM_.size());
    OMPL_INFORM("Cost: %.6f", bestCost_.value());

    return bestSolution ? base::PlannerStatus::EXACT_SOLUTION :
           base::PlannerStatus::TIMEOUT;
}

// outedge, inedge? - doesn't matter, need to scan all the neighbors.
void ompl::geometric::HarmoniousLazyPRMstarMulti::Decrease(const Vertex &v) {
    typedef std::pair<double, Vertex> weight_vertex;
    std::priority_queue<weight_vertex> pq;

    // Initialize cost of v, i.e., finding best parent vertex in G(g_).
    BGL_FORALL_OUTEDGES(v, e, g_, Graph) {
        Vertex w = target(e, g_);
        double weight = weightProperty_[e].value();

        if (costProperty_[v] > costProperty_[w] + weight) {
            predecessorProperty_[v] = w;
            costProperty_[v] = costProperty_[w] + weight;
        }
    }

    // No need to invoke cancelAdoption since v is newly sampled.
    if (predecessorProperty_[v] != NULL) {
        childrenProperty_[predecessorProperty_[v]]->push_back(v);
    }

    // At this point, v has a best parent. From now on construct its subtree of descendants.

    pq.push(weight_vertex(-costProperty_[v], v)); // Invert the cost value for mimicking min-heap.

    while (!pq.empty()) {
        weight_vertex top = pq.top();
        pq.pop();

        double cost = -top.first; // Invert the cost value to be like min-heap.
        Vertex vert = top.second;

        if (cost > costProperty_[vert]) {
            continue;
        }

        BGL_FORALL_OUTEDGES(vert, e, g_, Graph) {
            Vertex w = target(e, g_);
            double weight = weightProperty_[e].value();
            double cost_w = costProperty_[w];

            if (cost_w > cost + weight) {
                costProperty_[w] = cost + weight;
                cancelAdoption(w);

                predecessorProperty_[w] = vert;
                childrenProperty_[vert]->push_back(w);
                pq.push(weight_vertex(-costProperty_[w], w));
            }
        }
    }

    // Now, DSPT is stable.
}

#define RED (increaseIterations_ + 1) // I know, it's bad #define.

void ompl::geometric::HarmoniousLazyPRMstarMulti::Increase(const Vertex vs) {
    // <Step 1. Preparation.
    // white is used for color of each vertex without initialization.
    // For each iteration, white is increased by 1, thus we can use it as
    // equal to or less than 'white' means 'white' color, 'red' otherwise.
    increaseIterations_ += 1;

    std::vector<Vertex> reds;
    typedef std::pair<double, Vertex> weight_vertex;
    std::priority_queue<weight_vertex> pq; //  Max-heap by default.

    pq.push(weight_vertex(-costProperty_[vs], vs));  // It works as if it is min-heap.

    // <Step 2. Coloring
    while (!pq.empty()) {
        weight_vertex top = pq.top();
        pq.pop();

        double cost = -top.first;
        Vertex vert = top.second;

        if (cost > costProperty_[vert]) {
            continue;  // Instead of heap-improve
        }

        // Probability to get a pink node? almost impossible!
        /*
        // If there exist a non-red neighbor q of z such that Dist(q) + w_(q, z) = D(z)
        // set pink, that means it can keep the current cost, thus it is not necessary to
        // iterate its children.
        // Otherwise, set red and enqueue all the children of z.
        bool pink_flag = false;
        BGL_FORALL_OUTEDGES(vert, e, g_, Graph) {
          Vertex w = target(e, g_);
          double weight = weightProperty_[e].value();

          if (colorProperty_[w] != RED && costProperty_[w] + weight == cost) {
            // Actually, '<' should not be happened all the time.
            // And even '==' would very rarely occur, but possible.
            pink_flag = true;

            cancelAdoption(vert);
            predecessorProperty_[vert] = w;
            childrenProperty_[w]->push_back(vert);
            break; // If there exsits, take anyone among them.
          }
        }

        if (pink_flag) {
          continue;
        }*/

        colorProperty_[vert] = RED; // Set to 'red'
        reds.push_back(vert);
        // Even with multiple starting red nodes, there will be no re-visit since each starting node is
        // a root node of sub'tree' in DSPT. That is, if statement within for loop might be useless.
        // Someone would be curious, e.g., then why do we have to use priority queue in step2 ?
        // Just for 'pink' case. Yeap. We need to identify all the other parent candidates are red or not
        // prior to checking current node.
        std::vector<Vertex> *children = childrenProperty_[vert];

        for (unsigned int i = 0; i < children->size(); i++) {
            pq.push(weight_vertex(-costProperty_[(*children)[i]], (*children)[i]));
        }
    }

    // 'pq' is empty at here.

    // <Step 3-a. Find best non-red parent for each red node.
    for (unsigned int i = 0; i < reds.size(); i++) {
        // TODO : need to be verified
        // Cost/predecessor initialization.
        costProperty_[reds[i]] = std::numeric_limits<double>::infinity();
        cancelAdoption(reds[i]);

        BGL_FORALL_OUTEDGES(reds[i], e, g_, Graph) {
            Vertex w = target(e, g_);
            double weight = weightProperty_[e].value();

            if (colorProperty_[w] == RED) {
                continue;  // If red, put aside for a while.
            }

            if (costProperty_[reds[i]] > costProperty_[w] + weight) {
                costProperty_[reds[i]] = costProperty_[w] + weight;
                predecessorProperty_[reds[i]] = w;
            }
        }

        if (predecessorProperty_[reds[i]] != NULL) {
            childrenProperty_[predecessorProperty_[reds[i]]]->push_back(reds[i]);
        }

        pq.push(weight_vertex(-costProperty_[reds[i]], reds[i]));
    }

    // <Step 3-b. Propagate the changes; rewiring for 'red' nodes whether it can replace
    // existing parent node of near neighbors.
    while (!pq.empty()) {
        weight_vertex top = pq.top();
        pq.pop();

        double cost = -top.first;
        Vertex vert = top.second;

        if (costProperty_[vert] < cost) {
            continue;
        }

        BGL_FORALL_OUTEDGES(vert, e, g_, Graph) {
            Vertex w = target(e, g_);
            double weight = weightProperty_[e].value();

            if (colorProperty_[w] != RED) {
                continue;  // If not red, then skip.
            }

            if (cost + weight < costProperty_[w]) {
                costProperty_[w] = cost + weight;

                cancelAdoption(w);

                predecessorProperty_[w] = vert;
                childrenProperty_[vert]->push_back(w);
                pq.push(weight_vertex(-costProperty_[w], w));
            }
        }
    }

    // The end!
    // colorProperty_ is not necessary to be cleansed out, just increase variable, 'RED'.
}

// TODO : sync between children & edges.
void ompl::geometric::HarmoniousLazyPRMstarMulti::cancelAdoption(const Vertex &child) {
    if (predecessorProperty_[child] == NULL)
        return;

    std::vector<Vertex> *children = childrenProperty_[predecessorProperty_[child]];

    for (unsigned int i = 0; i < children->size(); i++) if ((*children)[i] == child) {
            std::swap((*children)[i], children->back());
            children->pop_back();
            break;
        }

    predecessorProperty_[child] = NULL;
}

// Vertex first.
ompl::base::PathPtr ompl::geometric::HarmoniousLazyPRMstarMulti::constructSolution(const Vertex &start, const Vertex &goal) {
    std::vector<Vertex> solution_path;

    // Construct a solution from DSPT.
    for (Vertex vert = goal; vert != NULL; vert = predecessorProperty_[vert])
        solution_path.push_back(vert);

    if (solution_path.empty() || solution_path.size() == 1)
        return base::PathPtr();

    // Goal = 0, Start = n - 1.
    // TODO : From goal or start ? which one is better?

    // auto from = solution_path.rbegin();
    std::vector<Vertex>::reverse_iterator from = solution_path.rbegin();
    for (std::vector<Vertex>::reverse_iterator to = from + 1; to != solution_path.rend(); ++to) {
        Edge e = boost::lookup_edge(*from, *to, g_).first; // Exhaustive search O(E) at worst case.
        unsigned int &evd = edgeValidityProperty_[e];

        if ((evd & VALIDITY_TRUE) == 0) { // Unknown
            bool result = true;
            // double weight = weightProperty_[e].value();

            result &= checkMotion(stateProperty_[*from], stateProperty_[*to]);

            if (result) {
                evd |= VALIDITY_TRUE;
            }
            else {
                boost::remove_edge(e, g_); // O(log(E/V)) time...

                cancelAdoption(*to);
                Increase(*to);
                return base::PathPtr();
            }
        }

        from = to;
    }

    PathGeometric *p = new PathGeometric(si_);

    // Feasible path is found, fetch optimized edges if possible.

    for (std::vector<Vertex>::const_reverse_iterator sol = solution_path.rbegin(); sol != solution_path.rend(); ++sol) {
        p->append(stateProperty_[*sol]);
    }

    return base::PathPtr(p);
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::getPlannerData(base::PlannerData &data) const {
    Planner::getPlannerData(data);
    // Caution : it handles directional information regardless of the search graph setting
    //           which is undirectional graph.
}

double ompl::geometric::HarmoniousLazyPRMstarMulti::distanceFunction(const base::State *a, const base::State *b) const {
    return si_->distance(a, b);
}

double ompl::geometric::HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious(const Vertex a, const Vertex b) const {
    double base = distanceFunctionBase(stateProperty_[a], stateProperty_[b]);
    double arm =  distanceFunctionJoints(stateProperty_[a], stateProperty_[b]);

    return base + arm;
}

double ompl::geometric::HarmoniousLazyPRMstarMulti::distanceFunctionBase(const base::State *a, const base::State *b) const {
    double dist = 0.0;
    int dim = si_->getStateDimension();

    std::vector<double> ca, cb;
    si_->getStateSpace()->copyToReals(ca, a);
    si_->getStateSpace()->copyToReals(cb, b);

    for (int i = 0; i < NUM_BASE_DOF; i++) {
        double dd = fabs(ca[i]-cb[i]);

        if(isContinuous_[i]) {
            if (dd > boost::math::constants::pi<double>()) {
                dd = 2.0 * boost::math::constants::pi<double>() - dd;
            }
        }

        dist += hs_.params_.weights_[i] * dd * hs_.params_.weights_[i] * dd;
    }

    return std::sqrt(dist);
}

double ompl::geometric::HarmoniousLazyPRMstarMulti::distanceFunctionJoints(const base::State *a, const base::State *b) const {
    double dist = 0.0;

    std::vector<double> ca, cb;
    si_->getStateSpace()->copyToReals(ca, a);
    si_->getStateSpace()->copyToReals(cb, b);

    for (int i = NUM_BASE_DOF; i < ca.size(); i++) {
        double dd = fabs(ca[i]-cb[i]);

        if(isContinuous_[i+3]){
            if (dd > boost::math::constants::pi<double>()) {
                dd = 2.0 * boost::math::constants::pi<double>() - dd;
            }
        }

        dist += hs_.params_.weights_[i] * dd * hs_.params_.weights_[i] * dd;
    }

    return std::sqrt(dist);
}

bool ompl::geometric::HarmoniousLazyPRMstarMulti::checkMotion(base::State *s1, base::State *s2) const {
//    /* Assume motion starts/ends in a valid configuration so v1/v2 are valid */
    bool result = true;
    int dim = si_->getStateDimension();
    int nd;

    int nd_j = validSegmentCount_compare(distanceFunctionJoints(s1, s2), hs_.params_.factor_joint_);
    int nd_b = validSegmentCount_compare(distanceFunctionBase(s1, s2), hs_.params_.factor_base_);
    if(nd_j > nd_b){
        nd = nd_j;
    }
    else{
        nd = nd_b;
    }

    if (nd > 1) {
        /* Temporary storage for the checked state */
        base::State *test = si_->allocState();
        std::queue<std::pair<unsigned int, unsigned int> > q;
        q.push(std::make_pair(1, nd - 1));

        while (!q.empty()) {
            std::pair<unsigned int, unsigned int> range = q.front();
            unsigned int mid;

            mid = (range.first + range.second) / 2;
            interpolate(s1, s2, (double)mid / (double)nd, test);
//            si_->getStateSpace()->interpolate(s1, s2, (double)mid / (double)nd, test);

            if (!si_->isValid(test)) {
                result = false;
                break;
            }

            q.pop();
            if (range.first < mid)
                q.push(std::make_pair(range.first, mid - 1));
            if (mid < range.second) {
                q.push(std::make_pair(mid + 1, range.second));
            } // if mid == first, no more recursion.
        }
    }

    return result;
}

unsigned int ompl::geometric::HarmoniousLazyPRMstarMulti::validSegmentCount_compare(const double dist,
                                                                                const double longestValidSegment) const {
    return si_->getStateSpace()->getValidSegmentCountFactor() * (unsigned int)ceil(dist / longestValidSegment);
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::interpolate(const base::State *from, const base::State *to, const double t, base::State *state) const
{
    const base::RealVectorStateSpace::StateType *rfrom = static_cast<const base::RealVectorStateSpace::StateType*>(from);
    const base::RealVectorStateSpace::StateType *rto = static_cast<const base::RealVectorStateSpace::StateType*>(to);
    const base::RealVectorStateSpace::StateType *rstate = static_cast<base::RealVectorStateSpace::StateType*>(state);
    for (unsigned int i = 0 ; i < isContinuous_.size() ; ++i){
        if(isContinuous_[i]){
            double diff = rto->values[i] - rfrom->values[i];
            if (fabs(diff) <= boost::math::constants::pi<double>())
                rstate->values[i] = rfrom->values[i] + diff * t;
            else
            {
                double &v = rstate->values[i];
                if (diff > 0.0)
                    diff = 2.0 * boost::math::constants::pi<double>() - diff;
                else
                    diff = -2.0 * boost::math::constants::pi<double>() - diff;
                v = rfrom->values[i] - diff * t;
                // input states are within bounds, so the following check is sufficient
                if (v > boost::math::constants::pi<double>())
                    v -= 2.0 * boost::math::constants::pi<double>();
                else
                if (v < -boost::math::constants::pi<double>())
                    v += 2.0 * boost::math::constants::pi<double>();
            }
        }
        else{
            rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
        }
    }
}

max_stars_repo_name: cheulkang/HarmoniousSampling

max_issues_repo_name: cheulkang/HarmoniousSampling

max_forks_repo_name: cheulkang/HarmoniousSampling


text:
/* structs.cpp */

#include "structs.h"
// #include <Eigen/Dense>

extern "C"
{

    // struct ET
    // {
    //     /**********************************************************
    //      *************** kinematic parameters *********************
    //      **********************************************************/
    //     int isjoint;
    //     int isflip;
    //     int jindex;
    //     int axis;
    //     double *T; /* link static transform */
    //     // Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>> Tm;
    //     double *qlim; /* joint limits */
    //     void (*op)(double *data, double eta);
    // };

} /* extern "C" */


text:
/**
 *  @file
 *  @copyright defined in evt/LICENSE.txt
 */
#pragma once
#include <boost/core/typeinfo.hpp>
#include <evt/chain/protocol.hpp>
#include <fc/exception/exception.hpp>

#define EVT_ASSERT(expr, exc_type, FORMAT, ...)            \
    FC_MULTILINE_MACRO_BEGIN                               \
    if(!(expr))                                            \
        FC_THROW_EXCEPTION(exc_type, FORMAT, __VA_ARGS__); \
    FC_MULTILINE_MACRO_END

#define EVT_THROW(exc_type, FORMAT, ...) throw exc_type(FC_LOG_MESSAGE(error, FORMAT, __VA_ARGS__));

/**
 * Macro inspired from FC_RETHROW_EXCEPTIONS
 * The main difference here is that if the exception caught isn't of type "chain_exception"
 * This macro will rethrow the exception as the specified "exception_type"
 */
#define EVT_RETHROW_EXCEPTIONS(exception_type, FORMAT, ...)                                                 \
    catch(chain_exception & e) {                                                                \
        FC_RETHROW_EXCEPTION(e, warn, FORMAT, __VA_ARGS__);                                                 \
    }                                                                                \
    catch(fc::exception & e) {                                                                              \
        exception_type new_exception(FC_LOG_MESSAGE(warn, FORMAT, __VA_ARGS__));                            \
        for(const auto& log : e.get_log()) {                                                                \
            new_exception.append_log(log);                                                                  \
        }                                                                                \
        throw new_exception;                                                                                \
    }                                                                                \
    catch(const std::exception& e) {                                                                        \
        exception_type fce(FC_LOG_MESSAGE(warn, FORMAT " (${what})", __VA_ARGS__("what", e.what())));       \
        throw fce;                                                                                \
    }                                                                                \
    catch(...) {                                                                                \
        throw fc::unhandled_exception(FC_LOG_MESSAGE(warn, FORMAT, __VA_ARGS__), std::current_exception()); \
    }

/**
 * Macro inspired from FC_CAPTURE_AND_RETHROW
 * The main difference here is that if the exception caught isn't of type "chain_exception"
 * This macro will rethrow the exception as the specified "exception_type"
 */
#define EVT_CAPTURE_AND_RETHROW(exception_type, ...)                                                               \
    catch(chain_exception & e) {                                                                       \
        FC_RETHROW_EXCEPTION(e, warn, "", FC_FORMAT_ARG_PARAMS(__VA_ARGS__));                                      \
    }                                                                                \
    catch(fc::exception & e) {                                                                                \
        exception_type new_exception(e.get_log());                                                                 \
        throw new_exception;                                                                                \
    }                                                                                \
    catch(const std::exception& e) {                                                                               \
        exception_type fce(FC_LOG_MESSAGE(warn, "${what}: ", FC_FORMAT_ARG_PARAMS(__VA_ARGS__)("what", e.what())), \
                           fc::std_exception_code, BOOST_CORE_TYPEID(decltype(e)).name(), e.what());               \
        throw fce;                                                                                \
    }                                                                                \
    catch(...) {                                                                                \
        throw fc::unhandled_exception(FC_LOG_MESSAGE(warn, "", FC_FORMAT_ARG_PARAMS(__VA_ARGS__)),                 \
                                      std::current_exception());                                                   \
    }

#define EVT_RECODE_EXC(cause_type, effect_type)    \
    catch(const cause_type& e) {                   \
        throw(effect_type(e.what(), e.get_log())); \
    }

namespace evt { namespace chain {

FC_DECLARE_EXCEPTION( chain_exception, 3000000, "blockchain exception" );
FC_DECLARE_DERIVED_EXCEPTION( database_query_exception,          chain_exception, 3010000, "database query exception" );
FC_DECLARE_DERIVED_EXCEPTION( block_validate_exception,          chain_exception, 3020000, "block validation exception" );
FC_DECLARE_DERIVED_EXCEPTION( transaction_exception,             chain_exception, 3030000, "transaction validation exception" );
FC_DECLARE_DERIVED_EXCEPTION( action_validate_exception,         chain_exception, 3040000, "message validation exception" );
FC_DECLARE_DERIVED_EXCEPTION( utility_exception,                 chain_exception, 3070000, "utility method exception" );
FC_DECLARE_DERIVED_EXCEPTION( undo_database_exception,           chain_exception, 3080000, "undo database exception" );
FC_DECLARE_DERIVED_EXCEPTION( unlinkable_block_exception,        chain_exception, 3090000, "unlinkable block" );
FC_DECLARE_DERIVED_EXCEPTION( black_swan_exception,              chain_exception, 3100000, "black swan" );
FC_DECLARE_DERIVED_EXCEPTION( chain_type_exception,              chain_exception, 3110000, "chain type exception" );
FC_DECLARE_DERIVED_EXCEPTION( missing_plugin_exception,          chain_exception, 3120000, "missing plugin exception" );
FC_DECLARE_DERIVED_EXCEPTION( wallet_exception,                  chain_exception, 3130000, "wallet exception" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_exception,                 chain_exception, 3140000, "tokendb exception" );
FC_DECLARE_DERIVED_EXCEPTION( misc_exception,                    chain_exception, 3150000, "Miscellaneous exception");
FC_DECLARE_DERIVED_EXCEPTION( authorization_exception,           chain_exception, 3160000, "Authorization exception");

FC_DECLARE_DERIVED_EXCEPTION( permission_query_exception,        database_query_exception, 3010001, "Permission Query Exception" );
FC_DECLARE_DERIVED_EXCEPTION( account_query_exception,           database_query_exception, 3010002, "Account Query Exception" );
FC_DECLARE_DERIVED_EXCEPTION( contract_table_query_exception,    database_query_exception, 3010003, "Contract Table Query Exception" );
FC_DECLARE_DERIVED_EXCEPTION( contract_query_exception,          database_query_exception, 3010004, "Contract Query Exception" );

FC_DECLARE_DERIVED_EXCEPTION( block_tx_output_exception,         block_validate_exception, 3020001, "transaction outputs in block do not match transaction outputs from applying block" );
FC_DECLARE_DERIVED_EXCEPTION( block_concurrency_exception,       block_validate_exception, 3020002, "block does not guarantee concurrent exection without conflicts" );
FC_DECLARE_DERIVED_EXCEPTION( block_lock_exception,              block_validate_exception, 3020003, "shard locks in block are incorrect or mal-formed" );
FC_DECLARE_DERIVED_EXCEPTION( block_resource_exhausted,          block_validate_exception, 3020004, "block exhausted allowed resources" );
FC_DECLARE_DERIVED_EXCEPTION( block_too_old_exception,           block_validate_exception, 3020005, "block is too old to push" );

FC_DECLARE_DERIVED_EXCEPTION( tx_missing_auth,                   transaction_exception, 3030001, "missing required authority" );
FC_DECLARE_DERIVED_EXCEPTION( tx_missing_sigs,                   transaction_exception, 3030002, "signatures do not satisfy declared authorizations" );
FC_DECLARE_DERIVED_EXCEPTION( invalid_committee_approval,        transaction_exception, 3030006, "committee account cannot directly approve transaction" );
FC_DECLARE_DERIVED_EXCEPTION( insufficient_fee,                  transaction_exception, 3030007, "insufficient fee" );
FC_DECLARE_DERIVED_EXCEPTION( tx_missing_recipient,              transaction_exception, 3030009, "missing required recipient" );
FC_DECLARE_DERIVED_EXCEPTION( checktime_exceeded,                transaction_exception, 3030010, "allotted processing time was exceeded" );
FC_DECLARE_DERIVED_EXCEPTION( tx_duplicate,                      transaction_exception, 3030011, "duplicate transaction" );
FC_DECLARE_DERIVED_EXCEPTION( tx_scheduling_exception,           transaction_exception, 3030013, "transaction failed during sheduling" );
FC_DECLARE_DERIVED_EXCEPTION( tx_unknown_argument,               transaction_exception, 3030014, "transaction provided an unknown value to a system call" );
FC_DECLARE_DERIVED_EXCEPTION( tx_resource_exhausted,             transaction_exception, 3030015, "transaction exhausted allowed resources" );
FC_DECLARE_DERIVED_EXCEPTION( page_memory_error,                 transaction_exception, 3030016, "error in WASM page memory" );
FC_DECLARE_DERIVED_EXCEPTION( unsatisfied_permission,            transaction_exception, 3030017, "Unsatisfied permission" );
FC_DECLARE_DERIVED_EXCEPTION( tx_msgs_auth_exceeded,             transaction_exception, 3030018, "Number of transaction messages per authorized account has been exceeded" );
FC_DECLARE_DERIVED_EXCEPTION( tx_msgs_code_exceeded,             transaction_exception, 3030019, "Number of transaction messages per code account has been exceeded" );
FC_DECLARE_DERIVED_EXCEPTION( wasm_execution_error,              transaction_exception, 3030020, "Runtime Error Processing WASM" );
FC_DECLARE_DERIVED_EXCEPTION( tx_decompression_error,            transaction_exception, 3030021, "Error decompressing transaction" );
FC_DECLARE_DERIVED_EXCEPTION( expired_tx_exception,              transaction_exception, 3030022, "Expired Transaction" );
FC_DECLARE_DERIVED_EXCEPTION( tx_exp_too_far_exception,          transaction_exception, 3030023, "Transaction Expiration Too Far" );
FC_DECLARE_DERIVED_EXCEPTION( invalid_ref_block_exception,       transaction_exception, 3030024, "Invalid Reference Block" );
FC_DECLARE_DERIVED_EXCEPTION( tx_apply_exception,                transaction_exception, 3030025, "Transaction Apply Exception" );
FC_DECLARE_DERIVED_EXCEPTION( wasm_serialization_error,          transaction_exception, 3030026, "Serialization Error Processing WASM" );
FC_DECLARE_DERIVED_EXCEPTION( tx_empty_region,                   transaction_exception, 3030027, "Transaction contains an empty region" );
FC_DECLARE_DERIVED_EXCEPTION( tx_empty_cycle,                    transaction_exception, 3030028, "Transaction contains an empty cycle" );
FC_DECLARE_DERIVED_EXCEPTION( tx_empty_shard,                    transaction_exception, 3030029, "Transaction contains an empty shard" );
FC_DECLARE_DERIVED_EXCEPTION( tx_receipt_inconsistent_status,    transaction_exception, 3030030, "Transaction receipt applied status does not match received status" );
FC_DECLARE_DERIVED_EXCEPTION( cfa_irrelevant_auth,               transaction_exception, 3030031, "context-free action should have no required authority" );
FC_DECLARE_DERIVED_EXCEPTION( tx_no_action,                      transaction_exception, 3030032, "transaction should have at least one normal action" );
FC_DECLARE_DERIVED_EXCEPTION( tx_no_auths,                       transaction_exception, 3030033, "transaction should have at least one required authority" );
FC_DECLARE_DERIVED_EXCEPTION( tx_receipt_inconsistent_cpu,       transaction_exception, 3030034, "Transaction receipt applied kcpu_usage does not match received kcpu_usage" );
FC_DECLARE_DERIVED_EXCEPTION( tx_receipt_inconsistent_net,       transaction_exception, 3030035, "Transaction receipt applied net_usage_words does not match received net_usage_words" );
FC_DECLARE_DERIVED_EXCEPTION( serialization_exception,           transaction_exception, 3030036, "serialization exception");
FC_DECLARE_DERIVED_EXCEPTION( deserialization_exception,         transaction_exception, 3030037, "deserialization exception");
FC_DECLARE_DERIVED_EXCEPTION( deadline_exception,                transaction_exception, 3030038, "transaction took too long");

FC_DECLARE_DERIVED_EXCEPTION( account_name_exists_exception,     action_validate_exception, 3040001, "account name already exists" );
FC_DECLARE_DERIVED_EXCEPTION( invalid_action_args_exception,     action_validate_exception, 3040002, "Invalid Action Arguments" );
FC_DECLARE_DERIVED_EXCEPTION( invalid_pts_address,               utility_exception, 3060001, "invalid pts address" );
FC_DECLARE_DERIVED_EXCEPTION( insufficient_feeds,                chain_exception, 37006, "insufficient feeds" );

FC_DECLARE_DERIVED_EXCEPTION( pop_empty_chain,                   undo_database_exception, 3070001, "there are no blocks to pop" );

FC_DECLARE_DERIVED_EXCEPTION( name_type_exception,               chain_type_exception, 3120001, "Invalid name" );
FC_DECLARE_DERIVED_EXCEPTION( public_key_type_exception,         chain_type_exception, 3120002, "Invalid public key" );
FC_DECLARE_DERIVED_EXCEPTION( private_key_type_exception,        chain_type_exception, 3120003, "Invalid private key" );
FC_DECLARE_DERIVED_EXCEPTION( authority_type_exception,          chain_type_exception, 3120004, "Invalid authority" );
FC_DECLARE_DERIVED_EXCEPTION( action_type_exception,             chain_type_exception, 3120005, "Invalid action" );
FC_DECLARE_DERIVED_EXCEPTION( transaction_type_exception,        chain_type_exception, 3120006, "Invalid transaction" );
FC_DECLARE_DERIVED_EXCEPTION( abi_type_exception,                chain_type_exception, 3120007, "Invalid ABI" );
FC_DECLARE_DERIVED_EXCEPTION( abi_not_found_exception,           chain_type_exception, 3120008, "No ABI found" );
FC_DECLARE_DERIVED_EXCEPTION( block_id_type_exception,           chain_type_exception, 3120009, "Invalid block ID" );
FC_DECLARE_DERIVED_EXCEPTION( transaction_id_type_exception,     chain_type_exception, 3120010, "Invalid transaction ID" );
FC_DECLARE_DERIVED_EXCEPTION( packed_transaction_type_exception, chain_type_exception, 3120011, "Invalid packed transaction" );
FC_DECLARE_DERIVED_EXCEPTION( asset_type_exception,              chain_type_exception, 3120012, "Invalid asset" );
FC_DECLARE_DERIVED_EXCEPTION( permission_type_exception,         chain_type_exception, 3120013, "Invalid permission" );
FC_DECLARE_DERIVED_EXCEPTION( group_type_exception,              chain_type_exception, 3120014, "Invalid group" );

FC_DECLARE_DERIVED_EXCEPTION( missing_chain_api_plugin_exception,                 missing_plugin_exception, 3130001, "Missing Chain API Plugin" );
FC_DECLARE_DERIVED_EXCEPTION( missing_wallet_api_plugin_exception,                missing_plugin_exception, 3130002, "Missing Wallet API Plugin" );
FC_DECLARE_DERIVED_EXCEPTION( missing_account_history_api_plugin_exception,       missing_plugin_exception, 3130003, "Missing Account History API Plugin" );
FC_DECLARE_DERIVED_EXCEPTION( missing_net_api_plugin_exception,                   missing_plugin_exception, 3130004, "Missing Net API Plugin" );

FC_DECLARE_DERIVED_EXCEPTION( wallet_exist_exception,            wallet_exception, 3140001, "Wallet already exists" );
FC_DECLARE_DERIVED_EXCEPTION( wallet_nonexistent_exception,      wallet_exception, 3140002, "Nonexistent wallet" );
FC_DECLARE_DERIVED_EXCEPTION( wallet_locked_exception,           wallet_exception, 3140003, "Locked wallet" );
FC_DECLARE_DERIVED_EXCEPTION( wallet_missing_pub_key_exception,  wallet_exception, 3140004, "Missing public key" );
FC_DECLARE_DERIVED_EXCEPTION( wallet_invalid_password_exception, wallet_exception, 3140005, "Invalid wallet password" );
FC_DECLARE_DERIVED_EXCEPTION( wallet_not_available_exception,    wallet_exception, 3140006, "No available wallet" );
FC_DECLARE_DERIVED_EXCEPTION( wallet_unlocked_exception,         wallet_exception, 3140007, "Already unlocked" );

FC_DECLARE_DERIVED_EXCEPTION( tokendb_domain_existed,            tokendb_exception, 3150001, "Domain already exists" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_group_existed,             tokendb_exception, 3150002, "Permission Group already exists" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_token_existed,             tokendb_exception, 3150003, "Token already exists" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_account_existed,           tokendb_exception, 3150004, "Account already exists" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_delay_existed,             tokendb_exception, 3150005, "Delay already exists" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_domain_not_found,          tokendb_exception, 3150006, "Not found specifc domain" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_group_not_found,           tokendb_exception, 3150007, "Not found specific permission group" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_token_not_found,           tokendb_exception, 3150008, "Not found specific token" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_account_not_found,         tokendb_exception, 3150009, "Not found specific account" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_delay_not_found,           tokendb_exception, 3150010, "Not found specific delay" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_rocksdb_fail,              tokendb_exception, 3150011, "Rocksdb internal error occurred" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_no_savepoint,              tokendb_exception, 3150012, "No savepoints anymore" );
FC_DECLARE_DERIVED_EXCEPTION( tokendb_seq_not_valid,             tokendb_exception, 3150013, "Seq for checkpoint is not valid" );

FC_DECLARE_DERIVED_EXCEPTION( unknown_block_exception,           misc_exception, 3100002, "unknown block" );
FC_DECLARE_DERIVED_EXCEPTION( unknown_transaction_exception,     misc_exception, 3100003, "unknown transaction" );
FC_DECLARE_DERIVED_EXCEPTION( fixed_reversible_db_exception,     misc_exception, 3100004, "corrupted reversible block database was fixed" );
FC_DECLARE_DERIVED_EXCEPTION( extract_genesis_state_exception,   misc_exception, 3100005, "extracted genesis state from blocks.log" );

FC_DECLARE_DERIVED_EXCEPTION( tx_duplicate_sig,                 authorization_exception, 3090001, "duplicate signature included" );
FC_DECLARE_DERIVED_EXCEPTION( tx_irrelevant_sig,                authorization_exception, 3090002, "irrelevant signature included" );
FC_DECLARE_DERIVED_EXCEPTION( unsatisfied_authorization,        authorization_exception, 3090003, "provided keys, permissions, and delays do not satisfy declared authorizations" );
FC_DECLARE_DERIVED_EXCEPTION( missing_auth_exception,           authorization_exception, 3090004, "missing required authority" );
FC_DECLARE_DERIVED_EXCEPTION( irrelevant_auth_exception,        authorization_exception, 3090005, "irrelevant authority included" );
FC_DECLARE_DERIVED_EXCEPTION( insufficient_delay_exception,     authorization_exception, 3090006, "insufficient delay" );

}} // evt::chain


text:
// Copyright (c) 2006, 2007 Julio M. Merino Vidal
// Copyright (c) 2008 Ilya Sokolov, Boris Schaeling
// Copyright (c) 2009 Boris Schaeling
// Copyright (c) 2010 Felipe Tanus, Boris Schaeling
// Copyright (c) 2011, 2012 Jeff Flinn, Boris Schaeling
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_PROCESS_WINDOWS_INITIALIZERS_PIPE_IN_HPP
#define BOOST_PROCESS_WINDOWS_INITIALIZERS_PIPE_IN_HPP

#include <boost/detail/winapi/process.hpp>
#include <boost/detail/winapi/handles.hpp>
#include <boost/process/detail/handler_base.hpp>
#include <cstdio>

namespace boost { namespace process { namespace detail { namespace windows {

struct pipe_in : public ::boost::process::detail::handler_base
{
    ::boost::detail::winapi::HANDLE_ handle;
    template<typename T>
    pipe_in(const T & p) : handle(p.native_source()) {}
    //template<typename CharT, typename Traits>
    //pipe_in(const boost::process::basic_pipe<CharT, Traits> & p) : handle(p.native_source()) {}
    //pipe_in(const boost::process::async_pipe & p)                : handle(p.native_source()) {}

    template <class WindowsExecutor>
    void on_setup(WindowsExecutor &e) const
    {
        boost::detail::winapi::SetHandleInformation(handle,
                boost::detail::winapi::HANDLE_FLAG_INHERIT_,
                boost::detail::winapi::HANDLE_FLAG_INHERIT_);

        e.startup_info.hStdInput = handle;
        e.startup_info.dwFlags  |= boost::detail::winapi::STARTF_USESTDHANDLES_;
        e.inherit_handles = true;
   }
};

}}}}

#endif

max_stars_repo_name: joerghall/sourcesfs

max_issues_repo_name: joerghall/sourcesfs

max_forks_repo_name: joerghall/sourcesfs


text:
#include "tsv.h"

#include <boost/algorithm/string.hpp>
#include <fstream>

using Eigen::VectorXd;
using std::string;
using std::vector;

ClassificationDataset load_tsv(const string& filename) {
    std::ifstream input(filename);
    if ( ! input) {
        std::ostringstream o;
        o << "Could not open file " << filename;
        throw std::runtime_error(o.str());
    }

    vector<VectorXd> x;
    vector<int> y;

    for (string line; std::getline(input, line); ) {
        // skip empty lines
        if (line.size() == 0)
            continue;

        // split line
        vector<string> strings;
        boost::split(strings, line, [](char c) { return c == '\t'; });

        // get point
        VectorXd v(3);
        for (unsigned i = 0; i < 3; ++i)
            v(i) = atof(strings[i].c_str());
        x.push_back(v);

        // get class
        y.push_back(strings[3] == "1");
    }

    return ClassificationDataset { x, y };
}

max_stars_repo_name: lfritz/data-mining-and-analysis

max_issues_repo_name: lfritz/data-mining-and-analysis

max_forks_repo_name: lfritz/data-mining-and-analysis


text:
/*
 * Copyright 2021 Assured Information Security, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <introvirt/core/memory/guest_ptr.hh>
#include <introvirt/windows/libraries/crypt32/functions/CryptProtectData.hh>

#include <boost/io/ios_state.hpp>

namespace introvirt {
namespace windows {
namespace crypt32 {

/* Input arguments */
GuestVirtualAddress CryptProtectData::pDataIn() const { return pDataIn_; }
void CryptProtectData::pDataIn(const GuestVirtualAddress& value) {
    set_address_argument(0, value);
    pDataIn_ = value;
}

GuestVirtualAddress CryptProtectData::pszDataDescr() const { return pszDataDescr_; }
void CryptProtectData::pszDataDescr(const GuestVirtualAddress& value) {
    set_address_argument(1, value);
    pszDataDescr_ = value;
}

GuestVirtualAddress CryptProtectData::pOptionalEntropy() const { return pOptionalEntropy_; }
void CryptProtectData::pOptionalEntropy(const GuestVirtualAddress& value) {
    set_address_argument(2, value);
    pOptionalEntropy_ = value;
}

GuestVirtualAddress CryptProtectData::pvReserved() const { return pvReserved_; }
void CryptProtectData::pvReserved(const GuestVirtualAddress& value) {
    set_address_argument(3, pvReserved_);
    pvReserved_ = value;
}

GuestVirtualAddress CryptProtectData::pPromptStruct() const { return pPromptStruct_; }
void CryptProtectData::pPromptStruct(const GuestVirtualAddress& value) {
    set_address_argument(4, value);
    pPromptStruct_ = value;
}

uint32_t CryptProtectData::dwFlags() const { return dwFlags_; }
void CryptProtectData::dwFlags(uint32_t value) {
    set_argument(5, value);
    dwFlags_ = value;
}

GuestVirtualAddress CryptProtectData::pDataOut() const { return pDataOut_; }
void CryptProtectData::pDataOut(const GuestVirtualAddress& value) {
    set_address_argument(6, value);
    pDataOut_ = value;
}

/* Helpers */
const CRYPTOAPI_BLOB* CryptProtectData::DataIn() const {
    if (!DataIn_ && pDataIn_)
        DataIn_ = CRYPTOAPI_BLOB::make_unique(pDataIn_, x64());
    return DataIn_.get();
}

CRYPTOAPI_BLOB* CryptProtectData::DataIn() {
    const auto* const_this = this;
    return const_cast<CRYPTOAPI_BLOB*>(const_this->DataIn());
}

const CRYPTOAPI_BLOB* CryptProtectData::OptionalEntropy() const {
    if (!OptionalEntropy_ && pOptionalEntropy_)
        OptionalEntropy_ = CRYPTOAPI_BLOB::make_unique(pOptionalEntropy_, x64());
    return OptionalEntropy_.get();
}

CRYPTOAPI_BLOB* CryptProtectData::OptionalEntropy() {
    const auto* const_this = this;
    return const_cast<CRYPTOAPI_BLOB*>(const_this->OptionalEntropy());
}

std::string CryptProtectData::DataDescr() const {
    if (pszDataDescr_) {
        auto result = map_guest_cstr(pszDataDescr());
        return std::string(result.get(), result.length());
    }
    return std::string();
}

const CRYPTPROTECT_PROMPTSTRUCT* CryptProtectData::PromptStruct() const {
    if (!PromptStruct_ && pPromptStruct_) {
        PromptStruct_ = CRYPTPROTECT_PROMPTSTRUCT::make_unique(pPromptStruct_, x64());
    }
    return PromptStruct_.get();
}

CRYPTPROTECT_PROMPTSTRUCT* CryptProtectData::PromptStruct() {
    const auto* const_this = this;
    return const_cast<CRYPTPROTECT_PROMPTSTRUCT*>(const_this->PromptStruct());
}

const CRYPTOAPI_BLOB* CryptProtectData::DataOut() const {
    if (!DataOut_ && pDataOut_)
        DataOut_ = CRYPTOAPI_BLOB::make_unique(pDataOut_, x64());
    return DataOut_.get();
}

CRYPTOAPI_BLOB* CryptProtectData::DataOut() {
    const auto* const_this = this;
    return const_cast<CRYPTOAPI_BLOB*>(const_this->DataOut());
}

bool CryptProtectData::result() const { return raw_return_value(); }

const std::string& CryptProtectData::function_name() const { return FunctionName; }
const std::string& CryptProtectData::library_name() const { return LibraryName; }
void CryptProtectData::write(std::ostream& os) const {
    boost::io::ios_flags_saver ifs(os);
    // TODO
}

CryptProtectData::CryptProtectData(Event& event) : WindowsFunctionCall(event, ArgumentCount) {
    pDataIn_ = get_address_argument(0);
    pszDataDescr_ = get_address_argument(1);
    pOptionalEntropy_ = get_address_argument(2);
    pvReserved_ = get_address_argument(3);
    pPromptStruct_ = get_address_argument(4);
    dwFlags_ = get_argument(5);
    pDataOut_ = get_address_argument(6);
}

CryptProtectData::~CryptProtectData() = default;

} // namespace crypt32
} // namespace windows
} // namespace introvirt

max_stars_repo_name: srpape/IntroVirt

max_issues_repo_name: srpape/IntroVirt

max_forks_repo_name: srpape/IntroVirt


text:
//
// Copyright (c) 2010-2015 Marat Abrarov (abrarov@gmail.com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef MA_ASYNC_CONNECT_HPP
#define MA_ASYNC_CONNECT_HPP

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif // defined(_MSC_VER) && (_MSC_VER >= 1200)

#include <boost/asio.hpp>
#include <ma/config.hpp>
#include <ma/detail/type_traits.hpp>
#include <ma/detail/utility.hpp>

#if defined(WIN32) && !defined(BOOST_ASIO_DISABLE_IOCP) \
    && !defined(MA_BOOST_ASIO_WINDOWS_CONNECT_EX) && (_WIN32_WINNT >= 0x0501)
#define MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX
#else
#undef  MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX
#endif

#if defined(MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX)
#include <mswsock.h>
#include <winsock2.h>
#include <cstddef>
#include <boost/numeric/conversion/cast.hpp>
#include <ma/bind_handler.hpp>
#include <ma/handler_alloc_helpers.hpp>
#include <ma/handler_invoke_helpers.hpp>
#include <ma/handler_cont_helpers.hpp>
#endif // defined(MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX)

namespace ma {

#if defined(MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX)

namespace detail {

#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable: 4512)
#endif // #if defined(_MSC_VER)

template <typename Handler>
class connect_ex_handler
{
private:
  typedef connect_ex_handler<Handler> this_type;

public:
  typedef void result_type;

  template <typename H>
  explicit connect_ex_handler(MA_FWD_REF(H) handler)
    : handler_(detail::forward<H>(handler))
  {
  }

#if defined(MA_HAS_RVALUE_REFS) \
    && (defined(MA_NO_IMPLICIT_MOVE_CONSTRUCTOR) || !defined(NDEBUG))

  connect_ex_handler(this_type&& other)
    : handler_(detail::move(other.handler_))
  {
  }

  connect_ex_handler(const this_type& other)
    : handler_(other.handler_)
  {
  }

#endif

#if !defined(NDEBUG)
  ~connect_ex_handler()
  {
  }
#endif

  friend void* asio_handler_allocate(std::size_t size, this_type* context)
  {
    return ma_handler_alloc_helpers::allocate(size, context->handler_);
  }

  friend void asio_handler_deallocate(void* pointer, std::size_t size,
      this_type* context)
  {
    ma_handler_alloc_helpers::deallocate(pointer, size, context->handler_);
  }

#if defined(MA_HAS_RVALUE_REFS)

  template <typename Function>
  friend void asio_handler_invoke(MA_FWD_REF(Function) function,
      this_type* context)
  {
    ma_handler_invoke_helpers::invoke(
        detail::forward<Function>(function), context->handler_);
  }

#else // defined(MA_HAS_RVALUE_REFS)

  template <typename Function>
  friend void asio_handler_invoke(Function& function, this_type* context)
  {
    ma_handler_invoke_helpers::invoke(function, context->handler_);
  }

  template <typename Function>
  friend void asio_handler_invoke(const Function& function, this_type* context)
  {
    ma_handler_invoke_helpers::invoke(function, context->handler_);
  }

#endif // defined(MA_HAS_RVALUE_REFS)

  friend bool asio_handler_is_continuation(this_type* context)
  {
    return ma_handler_cont_helpers::is_continuation(context->handler_);
  }

  void operator()(const boost::system::error_code& error,
      std::size_t /*bytes_transferred*/)
  {
    handler_(error);
  }

  void operator()(const boost::system::error_code& error,
      std::size_t /*bytes_transferred*/) const
  {
    handler_(error);
  }

private:
  Handler handler_;
}; // class connect_ex_handler

#if defined(_MSC_VER)
#pragma warning(pop)
#endif // #if defined(_MSC_VER)

template <typename Handler>
inline connect_ex_handler<typename detail::decay<Handler>::type>
make_connect_ex_handler(MA_FWD_REF(Handler) handler)
{
  typedef typename detail::decay<Handler>::type handler_type;
  return connect_ex_handler<handler_type>(detail::forward<Handler>(handler));
}

template <typename Socket>
boost::system::error_code bind_to_any(Socket& socket,
    const typename Socket::endpoint_type::protocol_type& protocol)
{
  typedef typename Socket::endpoint_type endpoint_type;

  static const boost::system::error_code ignored(WSAEINVAL,
      boost::asio::error::get_system_category());

  boost::system::error_code error;
  socket.bind(endpoint_type(protocol, 0), error);

  if (ignored == error)
  {
    error.clear();
  }

  return error;
}

} // namespace detail

#endif // defined(MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX)

template <typename Socket, typename Handler>
void async_connect(Socket& socket,
    const typename Socket::endpoint_type& peer_endpoint,
    MA_FWD_REF(Handler) handler)
{
#if defined(MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX)

  if (!socket.is_open())
  {
    // Open the socket before use ConnectEx.
    boost::system::error_code error;
    socket.open(peer_endpoint.protocol(), error);
    if (error)
    {
      socket.get_io_service().post(
          ma::bind_handler(detail::forward<Handler>(handler), error));
      return;
    }
  }

#if BOOST_ASIO_VERSION < 100600
  SOCKET native_socket = socket.native();
#else
  SOCKET native_socket = socket.native_handle();
#endif
  GUID connect_ex_guid = WSAID_CONNECTEX;
  LPFN_CONNECTEX connect_ex_func = 0;
  DWORD result_bytes;

  int ctrl_result = ::WSAIoctl(native_socket,
      SIO_GET_EXTENSION_FUNCTION_POINTER,
      &connect_ex_guid, sizeof(connect_ex_guid),
      &connect_ex_func, sizeof(connect_ex_func),
      &result_bytes, NULL, NULL);

  // If ConnectEx wasn't located then fall to common Asio async_connect.
  if ((SOCKET_ERROR == ctrl_result) || !connect_ex_func)
  {
    socket.async_connect(peer_endpoint, detail::forward<Handler>(handler));
    return;
  }

  if (boost::system::error_code error = detail::bind_to_any(socket,
      peer_endpoint.protocol()))
  {
    socket.get_io_service().post(
        ma::bind_handler(detail::forward<Handler>(handler), error));
    return;
  }

  // Construct an OVERLAPPED-derived object to contain the handler.
  boost::asio::windows::overlapped_ptr overlapped(socket.get_io_service(),
      detail::make_connect_ex_handler(detail::forward<Handler>(handler)));

  // Initiate the ConnectEx operation.
  BOOL ok = connect_ex_func(native_socket, peer_endpoint.data(),
      boost::numeric_cast<int>(peer_endpoint.size()), NULL, 0, NULL,
      overlapped.get());
  int last_error = ::WSAGetLastError();

  // Check if the operation completed immediately.
  if ((FALSE == ok) && (ERROR_IO_PENDING != last_error))
  {
    // The operation completed immediately, so a completion notification needs
    // to be posted. When complete() is called, ownership of the OVERLAPPED-
    // derived object passes to the io_service.
    boost::system::error_code error(last_error,
        boost::asio::error::get_system_category());
    overlapped.complete(error, 0);
  }
  else
  {
    // The operation was successfully initiated, so ownership of the
    // OVERLAPPED-derived object has passed to the io_service.
    overlapped.release();
  }

#else // defined(MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX)

  socket.async_connect(peer_endpoint, detail::forward<Handler>(handler));

#endif // defined(MA_ASYNC_CONNECT_USES_WINDOWS_CONNECT_EX)

}

} // namespace ma

#endif // MA_ASYNC_CONNECT_HPP

max_stars_repo_name: huawenyu/Boost-Cookbook

max_issues_repo_name: huawenyu/Boost-Cookbook

max_forks_repo_name: huawenyu/Boost-Cookbook


text:
//
// Created by Ivan Kishchenko on 15.05.2021.
//

#include "BoostSerialPort.h"
#include "logging/Logging.h"

#include <boost/crc.hpp>
#include <cstdint>

using namespace serial;
namespace asio = boost::asio;
namespace posix_time = boost::posix_time;

BoostSerialPort::BoostSerialPort(asio::io_service &service, const SerialProperties &props, serial::SerialPortCodecCallback& callback)
        : _serial(service), _timer(service), _props(props), _codec(SerialPortCodec::MODE_MASTER, callback) {

    open();
}

int BoostSerialPort::send(const uint8_t *data, size_t size) {
    auto buf = std::make_unique<asio::streambuf>();
    buf->sputn((const char*)data, size);
    asio::async_write(_serial, *buf, [this, b = std::move(buf)](const boost::system::error_code &ec, std::size_t sent) {
        if (ec) {
            onError(ec);
        } else {
            logger::debug("[serial-port] sent: {}", sent);
        }
    });

    return 0;
}

int BoostSerialPort::send(const Message &msg) {
    auto buf = std::make_unique<asio::streambuf>();
    buf->sputc(serial::MSG_MAGIC);
    buf->sputn((const char*)&msg.msgId, sizeof(uint8_t));
    buf->sputn((const char*)&msg.crc16, sizeof(uint16_t));
    buf->sputn((const char*)&msg.len, sizeof(uint16_t));
    buf->sputn((const char*)msg.data, msg.len);
    buf->sputc(serial::MSG_MAGIC);

    asio::async_write(_serial, *buf, [this, b = std::move(buf)](const boost::system::error_code &ec, std::size_t sent) {
        if (ec) {
            onError(ec);
        } else {
            logger::debug("[serial-port] sent: {}", sent);
        }
    });

    return 0;
}

uint16_t BoostSerialPort::crc16(const uint8_t *data, size_t size) {
    boost::crc_16_type crc16;
    crc16.process_block(data, data + size);

    return crc16.checksum();
}

void BoostSerialPort::asyncRead() {
    if (!_serial.is_open()) {
        return;
    }

    _serial.async_read_some(
            boost::asio::buffer(_incBuf, SERIAL_PORT_READ_BUF_SIZE),
            [this](const boost::system::error_code &ec, size_t size) {
                cancelTimer();

                if (!_serial.is_open()) {
                    return;
                }

                if (ec) {
                    onError(ec);
                    open();
                    return;
                }
                logger::debug("[serial-port] recv: {}", size);
                onMessage((const uint8_t*)&_incBuf, size);
                asyncRead();
            }
    );

    setTimer(boost::posix_time::seconds{5}, [this]() { onIdle(); });
}

const char* BoostSerialPort::deviceId() {
    return _props.port.c_str();
}

void BoostSerialPort::onConnect() {
    _codec.onConnect(*this);
}

void BoostSerialPort::onDisconnect() {
    _codec.onDisconnect(*this);
}

void BoostSerialPort::onIdle() {
    _codec.onIdle(*this);

    setTimer(posix_time::seconds{5}, [this]() {
        onIdle();
    });
}

void BoostSerialPort::onError(const boost::system::error_code &ec) {
    _codec.onError(*this, ec.value());
}

void BoostSerialPort::onMessage(const uint8_t *data, size_t size) {
    _codec.onMessage(*this, data, size);
}

void BoostSerialPort::open() {
    if (_serial.is_open()) {
        try {
            _serial.cancel();
            _serial.close();
        } catch (std::exception &ex) {
            logger::warning("[serial-port] can't close port {}, {}", _props.port, ex.what());
        }

        onDisconnect();
    }

    try {
        _serial.open(_props.port);
        _serial.set_option(boost::asio::serial_port_base::baud_rate(_props.baudRate));
        _serial.set_option(boost::asio::serial_port_base::character_size(8));
        _serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
        _serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
        _serial.set_option(
                boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
        onConnect();
        asyncRead();
    } catch (std::exception& ex) {
        logger::warning("[serial-port] can't open port: {}, {}", _props.port, ex.what());
        setTimer(posix_time::seconds{5}, [this]() {
            open();
        });
    }
}

void BoostSerialPort::setTimer(posix_time::time_duration duration, const std::function<void()> &fn) {
    _timer.expires_from_now(duration);
    _timer.async_wait([fn](const boost::system::error_code &ec) {
        if (!ec) {
            fn();
        }
    });
}

void BoostSerialPort::cancelTimer() {
    _timer.cancel();
}