#pragma once
#include <tudocomp/config.h>

#include <vector>

#include <tudocomp/ds/IntVector.hpp>

#include <tudocomp/Algorithm.hpp>
#include <tudocomp/ds/TextDS.hpp>

#include <tudocomp/compressors/lzss/LZSSFactors.hpp>
#include <tudocomp/ds/LCPSada.hpp>
#include <boost/heap/pairing_heap.hpp>

#include <tudocomp_stat/StatPhase.hpp>

namespace tdc {
namespace lcpcomp {

class PLCPStrategy : public Algorithm {
public:
    using Algorithm::Algorithm;

    inline static Meta meta() {
        Meta m("lcpcomp_comp", "plcp", "compressor using PLCP array");
        return m;
    }

    inline static ds::dsflags_t textds_flags() {
        return ds::SA | ds::ISA;
    }

    template<typename text_t>
    inline void factorize(text_t& text,
                   size_t threshold,
                   lzss::FactorBuffer& factors) {

		// Construct SA, ISA and LCP
        auto pplcp = StatPhase::wrap("Construct index ds", [&]{
            text.require(text_t::SA | text_t::ISA);
            const auto& sa = text.require_sa();
            return LCPForwardIterator { (construct_plcp_bitvector(env(), sa, text)) };
        });

        const auto& sa = text.require_sa();
        const auto& isa = text.require_isa();
        const len_t n = sa.size();

	    struct Poi {
		    len_t pos;
		    len_t lcp;
		    len_t no;
		    Poi(len_t _pos, len_t _lcp, len_t _no) : pos(_pos), lcp(_lcp), no(_no) {}
		    bool operator<(const Poi& o) const {
			    DCHECK_NE(o.pos, this->pos);
			    if(o.lcp == this->lcp) return this->pos > o.pos;
			    return this->lcp < o.lcp;
		    }
	    };

		StatPhase::wrap("Search Peaks", [&]{
		    boost::heap::pairing_heap<Poi> heap;
		    std::vector<typename boost::heap::pairing_heap<Poi>::handle_type> handles;

		    IF_STATS(len_t max_heap_size = 0);

		    // std::stack<poi> pois; // text positions of interest, i.e., starting positions of factors we want to replace

		    len_t lastpos = 0;
		    len_t lastpos_lcp = 0;
		    for(len_t i = 0; i+1 < n; ++i) {
			    while(pplcp.index() < i) pplcp.advance();
			    const len_t plcp_i = pplcp(); DCHECK_EQ(pplcp.index(), i);
			    if(heap.empty()) {
				    if(plcp_i >= threshold) {
					    handles.emplace_back(heap.emplace(i, plcp_i, handles.size()));
					    lastpos = i;
					    lastpos_lcp = plcp_i;
				    }
				    continue;
			    }
			    if(i - lastpos >= lastpos_lcp || tdc_unlikely(i+1 == n)) {
				    IF_DEBUG(bool first = true);
				    IF_STATS(max_heap_size = std::max<len_t>(max_heap_size, heap.size()));
				    DCHECK_EQ(heap.size(), handles.size());
				    while(!heap.empty()) {
					    const Poi& top = heap.top();
					    const len_t source_position = sa[isa[top.pos]-1];
					    factors.emplace_back(top.pos, source_position, top.lcp);
					    const len_t next_pos = top.pos; // store top, this is the current position that gets factorized
					    IF_DEBUG(if(first) DCHECK_EQ(top.pos, lastpos); first = false;)

					    {
						    len_t newlcp_peak = 0; // a new peak can emerge at top.pos+top.lcp
						    bool peak_exists = false;
						    if(top.pos+top.lcp < i)
						    for(len_t j = top.no+1; j < handles.size(); ++j) { // erase all right peaks that got substituted
							    if( handles[j].node_ == nullptr) continue;
							    const Poi poi = *(handles[j]);
							    DCHECK_LT(next_pos, poi.pos);
							    if(poi.pos < next_pos+top.lcp) {
								    heap.erase(handles[j]);
								    handles[j].node_ = nullptr;
								    if(poi.lcp + poi.pos > next_pos+top.lcp) {
									    const len_t remaining_lcp = poi.lcp+poi.pos - (next_pos+top.lcp);
									    DCHECK_NE(remaining_lcp,0);
									    if(newlcp_peak != 0) DCHECK_LE(remaining_lcp, newlcp_peak);
									    newlcp_peak = std::max(remaining_lcp, newlcp_peak);
								    }
							    } else if( poi.pos == next_pos+top.lcp) { peak_exists=true; }
							    else { break; }  // only for performance
						    }
    #ifdef DEBUG
						    if(peak_exists) {  //TODO: DEBUG
							    for(len_t j = top.no+1; j < handles.size(); ++j) {
								    if( handles[j].node_ == nullptr) continue;
								    const Poi& poi = *(handles[j]);
								    if(poi.pos == next_pos+top.lcp) {
									    DCHECK_LE(newlcp_peak, poi.lcp);
									    break;
								    }
							    }
						    }
    #endif
						    if(!peak_exists && newlcp_peak >= threshold) {
							    len_t j = top.no+1;
							    DCHECK(handles[j].node_ == nullptr);
							    handles[j] = heap.emplace(next_pos+top.lcp, newlcp_peak, j);
						    }

					    }
					    handles[top.no].node_ = nullptr;
					    heap.pop(); // top now gets erased

					    for(auto it = handles.rbegin(); it != handles.rend(); ++it) {
						    if( (*it).node_ == nullptr) continue;
						    Poi& poi = (*(*it));
						    if(poi.pos > next_pos)  continue;
						    const len_t newlcp = next_pos - poi.pos;
						    if(newlcp < poi.lcp) {
							    if(newlcp < threshold) {
								    heap.erase(*it);
								    it->node_ = nullptr;
							    } else {
								    poi.lcp = newlcp;
								    heap.decrease(*it);

							    }
						    } else {
							    break;
						    }
					    }
				    }
				    handles.clear();
				    --i;
				    continue;
			    }
			    DCHECK_EQ(pplcp.index(), i);
			    DCHECK_EQ(plcp_i, pplcp());
			    if(plcp_i <= lastpos_lcp) continue;
			    DCHECK_LE(threshold, plcp_i);
			    handles.emplace_back(heap.emplace(i,plcp_i, handles.size()));
			    lastpos = i;
    //			DCHECK_EQ(plcp[lastpos], plcp_i);
			    lastpos_lcp = plcp_i;
		    }
            IF_STATS(StatPhase::log("max heap size", max_heap_size));

        });
    }

};

}}//ns


//   Copyright 2015 Patrick Putnam
//
//   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.
#ifndef CLOTHO_CROSSOVER_MT_GENERATOR_HPP_
#define CLOTHO_CROSSOVER_MT_GENERATOR_HPP_

//#include <boost/asio/io_service.hpp>
//#include <boost/bind.hpp>
//#include <boost/thread/thread.hpp>

#include <boost/random/bernoulli_distribution.hpp>

#include "clotho/data_spaces/crossover/crossover_task_list.hpp"
#include "clotho/recombination/sequence_bias_parameter.hpp"

#include "clotho/data_spaces/crossover/position_classifier.hpp"

#include "clotho/data_spaces/generators/position_distribution_helper.hpp"
#include "clotho/data_spaces/generators/crossover_event_distribution_helper.hpp"

#include "clotho/recombination/recombination_rate_parameter.hpp"

#include "clotho/data_spaces/task/thread_pool.hpp"

namespace clotho {
namespace genetics {

template < class RNG, class SequenceSpaceType, class AlleleSpaceType >
class CrossoverMT {
public:
    typedef SequenceSpaceType   sequence_space_type;
    typedef AlleleSpaceType     allele_space_type;

    typedef typename sequence_space_type::sequence_vector      sequence_vector;
    typedef typename sequence_space_type::raw_block_pointer    sequence_iterator;
//    typedef typename sequence_space_type::individual_id_type individual_id_type;
//
//    typedef std::vector< std::pair< individual_id_type, individual_id_type > >  mate_pair_type;
//    typedef typename mate_pair_type::iterator                                   iterator;

    typedef PositionClassifier< typename allele_space_type::position_vector >   classifier_type;
    typedef typename classifier_type::event_type                                event_type;

    typedef typename position_distribution_helper< typename allele_space_type::position_type >::type position_distribution_type;
    typedef typename crossover_event_distribution_helper< double >::type event_distribution_type;

    CrossoverMT( RNG * rng, boost::property_tree::ptree & config ) :
        m_rng( rng )
    {
        sequence_bias_parameter< double > bias(config);
        m_bias_dist.param( typename boost::random::bernoulli_distribution< double >::param_type( bias.m_bias ) );

        recombination_rate_parameter< double > rho(config);
        m_event_dist.param( typename event_distribution_type::param_type( rho.m_rho ) );
    }

    template < class SelectionType, class PoolType >
    void operator()( const SelectionType & parents, sequence_space_type * parental, sequence_space_type * offspring, allele_space_type * alleles, PoolType & pool ) {
        offspring->clear();
        if( pool.pool_size() <= 1 ) {
            generate( parents, parental, offspring, alleles );
        } else {
            generate( parents, parental, offspring, alleles, pool );
        }
    }

    virtual ~CrossoverMT() {}

protected:

    template < class SelectionType >
    void generate( const SelectionType & parents, sequence_space_type * parental, sequence_space_type * offspring, allele_space_type * alleles ) {
        typedef typename SelectionType::const_iterator iterator;
        iterator mate_it = parents.begin(), mate_end = parents.end();

        size_t i = 0;
        while( mate_it != mate_end ) {

            unsigned int idx = 2 * mate_it->first;
            sequence_vector s0 = parental->getSequence( idx++ );
            sequence_vector s1 = parental->getSequence( idx );

            sequence_vector c = offspring->getSequence( i++ );

            event_type evts = make_events( );
            classifier_type cfier0( &alleles->getPositions(), evts );
            bool _swap = m_bias_dist( *m_rng );
            make_crossover_tasks( cfier0, s0.first, s0.second, s1.first, s1.second, c.first, _swap );

            idx = 2 * mate_it->second;
            s0 = parental->getSequence( idx++ );
            s1 = parental->getSequence( idx );

            c = offspring->getSequence( i++ );

            event_type evts1 = make_events( );
            classifier_type cfier1( &alleles->getPositions(), evts1 );
            _swap = m_bias_dist( *m_rng );
            make_crossover_tasks( cfier1, s0.first, s0.second, s1.first, s1.second, c.first, _swap);

            ++mate_it;
        }
    }

    template < class SelectionType, class PoolType >
    void generate( const SelectionType & parents, sequence_space_type * parental, sequence_space_type * offspring, allele_space_type * alleles, PoolType & pool ) {
        typedef typename SelectionType::const_iterator iterator;

        iterator mate_it = parents.begin(), mate_end = parents.end();

        size_t i = 0;
        while( mate_it != mate_end ) {

            unsigned int idx = 2 * mate_it->first;
            sequence_vector s0 = parental->getSequence( idx++ );
            sequence_vector s1 = parental->getSequence( idx );

            sequence_vector c = offspring->getSequence( i++ );

            event_type evts = make_events( );
            classifier_type cfier0( &alleles->getPositions(), evts );
            bool _swap = m_bias_dist( *m_rng );
            make_crossover_tasks( cfier0, s0.first, s0.second, s1.first, s1.second, c.first, pool, _swap );

            idx = 2 * mate_it->second;
            s0 = parental->getSequence( idx++ );
            s1 = parental->getSequence( idx );

            c = offspring->getSequence( i++ );

            event_type evts1 = make_events( );
            classifier_type cfier1( &alleles->getPositions(), evts1 );
            _swap = m_bias_dist( *m_rng );
            make_crossover_tasks( cfier1, s0.first, s0.second, s1.first, s1.second, c.first, pool, _swap );

            ++mate_it;
        }

        pool.sync();
    }

    event_type make_events( ) {
        event_type res;

        unsigned int N = m_event_dist( *m_rng );

        while( N-- ) {
            res.push_back( m_pos_dist( *m_rng ) );
        }

        return res;
    }

    RNG * m_rng;

    event_distribution_type     m_event_dist;
    position_distribution_type  m_pos_dist;
    boost::random::bernoulli_distribution< double > m_bias_dist;
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_CROSSOVER_MT_GENERATOR_HPP_


//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_multibody_parsers_urdf_model_hxx__
#define __pinocchio_multibody_parsers_urdf_model_hxx__

#include "pinocchio/math/matrix.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/multibody/model.hpp"

#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>

#include <sstream>
#include <boost/foreach.hpp>
#include <limits>

namespace pinocchio
{
  namespace urdf
  {
    namespace details
    {
      const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT);

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      FrameIndex getParentJointFrame(const ::urdf::LinkConstSharedPtr link,
                                     const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
      {
        typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
        typedef typename Model::Frame Frame;
        typedef typename Model::FrameIndex FrameIndex;

        PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());

        FrameIndex id;
        if (!link->getParent()->parent_joint)
        {
          if (model.existFrame("root_joint",  JOINT_OR_FIXED_JOINT))
            id = model.getFrameId("root_joint", JOINT_OR_FIXED_JOINT);
          else
            id = 0;
        }
        else
        {
          if (model.existFrame(link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT))
            id = model.getFrameId (link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT);
          else
            throw std::invalid_argument("Model does not have any joints named "
                                        + link->getParent()->parent_joint->name);
        }

        const Frame & f = model.frames[id];
        if (f.type == JOINT || f.type == FIXED_JOINT)
          return id;
        throw std::invalid_argument("Parent frame is not a JOINT neither a FIXED_JOINT");
      }

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      void appendBodyToJoint(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                             const FrameIndex fid,
                             const ::urdf::InertialConstSharedPtr Y_ptr,
                             const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & placement,
                             const std::string & body_name)
      {
        typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
        typedef typename Model::Frame Frame;
        typedef typename Model::SE3 SE3;

        const Frame & frame = model.frames[fid];
        const SE3 & p = frame.placement * placement;
        if(frame.parent > 0
           && Y_ptr
           && Y_ptr->mass > Eigen::NumTraits<double>::epsilon())
        {
          model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y_ptr), p);
        }

        model.addBodyFrame(body_name, frame.parent, p, (int)fid);
        // Reference to model.frames[fid] can has changed because the vector
        // may have been reallocated.
        if (model.frames[fid].parent > 0)
        {
          assert (   !hasNaN(model.inertias[model.frames[fid].parent].lever())
                  && !hasNaN(model.inertias[model.frames[fid].parent].inertia().data()));
        }
      }

      ///
      /// \brief Shortcut for adding a joint and directly append a body to it.
      ///
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename JointModel>
      void addJointAndBody(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                           const JointModelBase<JointModel> & jmodel,
                           const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex & parentFrameId,
                           const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & joint_placement,
                           const std::string & joint_name,
                           const ::urdf::InertialConstSharedPtr Y,
                           const std::string & body_name,
                           const typename JointModel::TangentVector_t & max_effort,
                           const typename JointModel::TangentVector_t & max_velocity,
                           const typename JointModel::ConfigVector_t  & min_config,
                           const typename JointModel::ConfigVector_t  & max_config)
      {
        typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
        typedef typename Model::Frame Frame;
        typedef typename Model::SE3 SE3;
        typedef typename Model::JointIndex JointIndex;
        typedef typename Model::FrameIndex FrameIndex;

        JointIndex idx;
        const Frame & frame = model.frames[parentFrameId];

        idx = model.addJoint(frame.parent,jmodel,
                             frame.placement * joint_placement,
                             joint_name,
                             max_effort,max_velocity,min_config,max_config
                             );
        int res (model.addJointFrame(idx, (int)parentFrameId));
        if (res == -1) {
          std::ostringstream oss;
          oss << joint_name << " already inserted as a frame. Current frames "
          "are [";
          for (typename container::aligned_vector<Frame>::const_iterator it =
               model.frames.begin (); it != model.frames.end (); ++it) {
            oss << "\"" << it->name << "\",";
          }
          oss << "]";
          throw std::invalid_argument(oss.str().c_str());
        }

        FrameIndex jointFrameId = (FrameIndex) res; // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
        appendBodyToJoint(model, jointFrameId, Y, SE3::Identity(), body_name);
      }

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename JointModel>
      void addJointAndBody(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                           const JointModelBase<JointModel> & jmodel,
                           const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex & parentFrameId,
                           const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & joint_placement,
                           const std::string & joint_name,
                           const ::urdf::InertialConstSharedPtr Y,
                           const std::string & body_name)
      {
        const Scalar infty = std::numeric_limits<Scalar>::infinity();

        const typename JointModel::TangentVector_t max_effort   = JointModel::TangentVector_t::Constant(jmodel.nv(), infty);
        const typename JointModel::TangentVector_t max_velocity = JointModel::TangentVector_t::Constant(jmodel.nv(), infty);
        const typename JointModel::ConfigVector_t  min_config   = JointModel::ConfigVector_t ::Constant(jmodel.nq(),-infty);
        const typename JointModel::ConfigVector_t  max_config   = JointModel::ConfigVector_t ::Constant(jmodel.nq(), infty);

        addJointAndBody(model,jmodel.derived(),parentFrameId,joint_placement,joint_name,Y,body_name,
                        max_effort,max_velocity,min_config,max_config);
      }

      ///
      /// \brief Shortcut for adding a fixed joint and directly append a body to it.
      ///
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      void addFixedJointAndBody(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex & parentFrameId,
                                const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & joint_placement,
                                const std::string & joint_name,
                                const ::urdf::InertialConstSharedPtr Y,
                                const std::string & body_name)
      {
        typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
        typedef typename Model::Frame Frame;

        const Frame & frame = model.frames[parentFrameId];

        int fid = model.addFrame(Frame(joint_name, frame.parent, parentFrameId,
                                       frame.placement * joint_placement, FIXED_JOINT)
                                 );
        if (fid < 0)
          throw std::invalid_argument("Fixed joint " + joint_name + " could not be added.");

        appendBodyToJoint(model, (FrameIndex)fid, Y, SE3::Identity(), body_name);
      }

      ///
      /// \brief Recursive procedure for reading the URDF tree.
      ///        The function returns an exception as soon as a necessary Inertia or Joint information are missing.
      ///
      /// \param[in] link The current URDF link.
      /// \param[in] model The model where the link must be added.
      ///
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      void parseTree(::urdf::LinkConstSharedPtr link,
                     ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                     bool verbose)
      {
        typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
        typedef typename Model::JointCollection JointCollection;
        typedef typename Model::SE3 SE3;
        typedef typename Model::FrameIndex FrameIndex;

        // Parent joint of the current body
        const ::urdf::JointConstSharedPtr joint =
        ::urdf::const_pointer_cast< ::urdf::Joint>(link->parent_joint);

        if(joint) // if the link is not the root of the tree
        {
          PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent());

          const std::string & joint_name = joint->name;
          const std::string & link_name = link->name;
          const std::string & parent_link_name = link->getParent()->name;
          std::ostringstream joint_info;

          FrameIndex parentFrameId = getParentJointFrame(link, model);

          // Transformation from the parent link to the joint origin
          const SE3 jointPlacement
          = convertFromUrdf(joint->parent_to_joint_origin_transform).template cast<Scalar>();

          const ::urdf::InertialSharedPtr Y =
          ::urdf::const_pointer_cast< ::urdf::Inertial>(link->inertial);

          switch(joint->type)
          {
            case ::urdf::Joint::FLOATING:
              joint_info << "joint FreeFlyer";
              addJointAndBody(model,typename JointCollection::JointModelFreeFlyer(),
                              parentFrameId,jointPlacement,joint->name,
                              Y,link->name);

              break;

            case ::urdf::Joint::REVOLUTE:
              {
              joint_info << "joint REVOLUTE with axis";

              typedef JointModelRX::ConfigVector_t ConfigVector_t;
              typedef JointModelRX::TangentVector_t TangentVector_t;

              TangentVector_t max_effort;
              TangentVector_t max_velocity;
              ConfigVector_t lower_position;
              ConfigVector_t upper_position;

              if (joint->limits)
              {
                max_effort << joint->limits->effort;
                max_velocity << joint->limits->velocity;
                lower_position << joint->limits->lower;
                upper_position << joint->limits->upper;
              }

              CartesianAxis axis = extractCartesianAxis(joint->axis);

              switch(axis)
              {
                case AXIS_X:
                  joint_info << " along X";
                  addJointAndBody(model, typename JointCollection::JointModelRX(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_Y:
                  joint_info << " along Y";
                  addJointAndBody(model, typename JointCollection::JointModelRY(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_Z:
                  joint_info << " along Z";
                  addJointAndBody(model, typename JointCollection::JointModelRZ(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_UNALIGNED:
                {
                  typename SE3::Vector3 joint_axis((Scalar)joint->axis.x,(Scalar)joint->axis.y,(Scalar)joint->axis.z);
                  joint_info << " unaligned along (" << joint_axis.transpose() << ")";

                  addJointAndBody(model, typename JointCollection::JointModelRevoluteUnaligned(joint_axis.normalized()),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;
                }
                default:
                  PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the revolute joint is of wrong type.");
                  break;
              }
              break;
              }

            case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
              {
              joint_info << "joint CONTINUOUS with axis";

              typedef JointModelRUBX::ConfigVector_t ConfigVector_t;
              typedef JointModelRUBX::TangentVector_t TangentVector_t;

              TangentVector_t max_effort;
              TangentVector_t max_velocity;
              const ConfigVector_t::Scalar u = 1.01;
              ConfigVector_t lower_position(-u, -u);
              ConfigVector_t upper_position( u,  u);

              if(joint->limits)
              {
                max_effort << joint->limits->effort;
                max_velocity << joint->limits->velocity;
              }

              CartesianAxis axis = extractCartesianAxis(joint->axis);

              switch(axis)
              {
                case AXIS_X:
                  joint_info << " along X";
                  addJointAndBody(model, typename JointCollection::JointModelRUBX(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_Y:
                  joint_info << " along Y";
                  addJointAndBody(model, typename JointCollection::JointModelRUBY(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_Z:
                  joint_info << " along Z";
                  addJointAndBody(model, typename JointCollection::JointModelRUBZ(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_UNALIGNED:
                {
                  typename SE3::Vector3 joint_axis((Scalar)joint->axis.x,(Scalar)joint->axis.y,(Scalar)joint->axis.z);
                  joint_info << " unaligned along (" << joint_axis.transpose() << ")";

                  typedef typename JointCollection::JointModelRevoluteUnboundedUnaligned::ConfigVector_t ConfigVector_t;

                  const Scalar infty = std::numeric_limits<Scalar>::infinity();
                  ConfigVector_t lower_position(ConfigVector_t::Constant(-infty));
                  ConfigVector_t upper_position(ConfigVector_t::Constant(infty));

                  addJointAndBody(model, typename JointCollection::JointModelRevoluteUnboundedUnaligned(joint_axis.normalized()),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position,upper_position);
                  break;
                }

                default:
                  PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the revolute joint is of wrong type.");
                  break;
              }
              break;
              }

            case ::urdf::Joint::PRISMATIC:
              {
              joint_info << "joint PRISMATIC with axis";

              typedef JointModelRX::ConfigVector_t ConfigVector_t;
              typedef JointModelRX::TangentVector_t TangentVector_t;

              TangentVector_t max_effort;
              TangentVector_t max_velocity;
              ConfigVector_t lower_position;
              ConfigVector_t upper_position;

              if (joint->limits)
              {
                max_effort << joint->limits->effort;
                max_velocity << joint->limits->velocity;
                lower_position << joint->limits->lower;
                upper_position << joint->limits->upper;
              }

              CartesianAxis axis = extractCartesianAxis(joint->axis);
              switch(axis)
              {
                case AXIS_X:
                  joint_info << " along X";
                  addJointAndBody(model, typename JointCollection::JointModelPX(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_Y:

                  joint_info << " along Y";
                  addJointAndBody(model, typename JointCollection::JointModelPY(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_Z:
                  joint_info << " along Z";
                  addJointAndBody(model, typename JointCollection::JointModelPZ(),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;

                case AXIS_UNALIGNED:
                {
                  typename SE3::Vector3 joint_axis((Scalar)joint->axis.x,(Scalar)joint->axis.y,(Scalar)joint->axis.z);
                  joint_info << " unaligned along (" << joint_axis.transpose() << ")";

                  addJointAndBody(model, typename JointCollection::JointModelPrismaticUnaligned(joint_axis.normalized()),
                                  parentFrameId,jointPlacement,joint->name,
                                  Y,link->name,
                                  max_effort,max_velocity,
                                  lower_position, upper_position);
                  break;
                }

                default:
                  PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the prismatic joint is of wrong type.");
                  break;
              }
              break;
              }

            case ::urdf::Joint::PLANAR:
              {
              joint_info << "joint PLANAR with normal axis along Z";

              typedef JointModelPlanar::ConfigVector_t ConfigVector_t;
              typedef JointModelPlanar::TangentVector_t TangentVector_t;

              TangentVector_t max_effort;
              TangentVector_t max_velocity;
              ConfigVector_t lower_position;
              ConfigVector_t upper_position;

              if (joint->limits)
              {
                max_effort << joint->limits->effort;
                max_velocity << joint->limits->velocity;
                lower_position << joint->limits->lower;
                upper_position << joint->limits->upper;
              }

              addJointAndBody(model, typename JointCollection::JointModelPlanar(),
                              parentFrameId,jointPlacement,joint->name,
                              Y,link->name,
                              max_effort,max_velocity,
                              lower_position, upper_position);

              }
              break;

            case ::urdf::Joint::FIXED:
              // In case of fixed joint, if link has inertial tag:
              //    -add the inertia of the link to his parent in the model
              // Otherwise do nothing.
              // In all cases:
              //    -let all the children become children of parent
              //    -inform the parser of the offset to apply
              //    -add fixed body in model to display it in gepetto-viewer

              joint_info << "fixed joint";
              addFixedJointAndBody(model, parentFrameId, jointPlacement,
                                   joint_name, Y, link_name);

              break;

            default:
              {
                const std::string exception_message("The type of joint " + joint_name + " is not supported.");
                throw std::invalid_argument(exception_message);
                break;
              }
          }

          if(verbose)
          {
            const Inertia YY = (!Y) ? Inertia::Zero() : convertFromUrdf(*Y);
            std::cout << "Adding Body" << std::endl;
            std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
            std::cout << "joint type: " << joint_info.str() << std::endl;
            std::cout << "joint placement:\n" << jointPlacement;
            std::cout << "body info: " << std::endl;
            std::cout << "  " << "mass: " << YY.mass() << std::endl;
            std::cout << "  " << "lever: " << YY.lever().transpose() << std::endl;
            std::cout << "  " << "inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << YY.inertia().data().transpose() << std::endl << std::endl;
          }
        }
        else if (link->getParent())
        {
          const std::string exception_message (link->name + " - joint information missing.");
          throw std::invalid_argument(exception_message);
        }

        BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)
        {
          parseTree(child, model, verbose);
        }
      }

      ///
      /// \brief Parse a tree with a specific root joint linking the model to the environment.
      ///        The function returns an exception as soon as a necessary Inertia or Joint information are missing.
      ///
      /// \param[in] link The current URDF link.
      /// \param[in] model The model where the link must be added.
      /// \param[in] verbose Print parsing info.
      ///
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      void parseRootTree(::urdf::LinkConstSharedPtr root_link,
                         ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                         const bool verbose)
      {
        addFixedJointAndBody(model, 0, SE3::Identity(), "root_joint",
                             root_link->inertial, root_link->name);

        BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
        {
          parseTree(child, model, verbose);
        }
      }

      ///
      /// \brief Parse a tree with a specific root joint linking the model to the environment.
      ///        The function returns an exception as soon as a necessary Inertia or Joint information are missing.
      ///
      /// \param[in] link The current URDF link.
      /// \param[in] model The model where the link must be added.
      /// \param[in] root_joint The specific root joint.
      /// \param[in] verbose Print parsing info.
      ///
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename JointModel>
      void parseRootTree(::urdf::LinkConstSharedPtr root_link,
                         ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                         const JointModelBase<JointModel> & root_joint,
                         const bool verbose)
      {
        addJointAndBody(model,root_joint,
                        0,SE3::Identity(),"root_joint",
                        root_link->inertial,root_link->name);

        BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
        {
          parseTree(child, model, verbose);
        }
      }
    } // namespace details

    ///
    /// \brief Call parse root tree templated function
    ///
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    struct ParseRootTreeVisitor : public boost::static_visitor<>
    {
      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
      ::urdf::LinkConstSharedPtr m_root_link;
      Model & m_model;
      const bool m_verbose;

      ParseRootTreeVisitor(::urdf::LinkConstSharedPtr root_link,
                           Model & model,
                           const bool verbose)
      : m_root_link(root_link)
      , m_model(model)
      , m_verbose(verbose)
      {}

      template<typename JointModel>
      void operator()(const JointModelBase<JointModel> & root_joint) const
      {
        details::parseRootTree(m_root_link,m_model,root_joint,m_verbose);
      }

      static void run(::urdf::LinkConstSharedPtr root_link,
                      Model & model,
                      const typename Model::JointModel & root_joint,
                      const bool verbose)
      {
        boost::apply_visitor(ParseRootTreeVisitor(root_link,model,verbose),root_joint);
      }
    }; // struct ParseRootTreeVisitor

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const std::string & filename,
               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & root_joint,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)

    {
      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);

      if (urdfTree)
      {
          return buildModel (urdfTree, root_joint, model, verbose);
      }
      else
      {
        const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
        throw std::invalid_argument(exception_message);
      }
      return model;
    }
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const std::string & filename,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)

    {
      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
      if (urdfTree)
      {
          return buildModel(urdfTree, model, verbose);
      }
      else
      {
        const std::string exception_message("The file " + filename + " does not contain a valid URDF model.");
        throw std::invalid_argument(exception_message);
      }

      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModelFromXML(const std::string & xmlStream,
                      const JointModelVariant & rootJoint,
                      ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const bool verbose)
    {
      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlStream);

      if (urdfTree)
        return buildModel(urdfTree, rootJoint, model, verbose);
      else
      {
        const std::string exception_message ("The XML stream does not contain a valid URDF model.");
        throw std::invalid_argument(exception_message);
      }

      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModelFromXML(const std::string & xmlStream,
                      ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const bool verbose)
    {
      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlStream);

      if (urdfTree)
        return buildModel(urdfTree, model, verbose);
      else
      {
        const std::string exception_message ("The XML stream does not contain a valid URDF model.");
        throw std::invalid_argument(exception_message);
      }

      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & root_joint,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)
    {
      PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree);
      model.name = urdfTree->getName();
      ParseRootTreeVisitor<Scalar,Options,JointCollectionTpl>::run(urdfTree->getRoot(),model,root_joint,verbose);
      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)
    {
      PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree);
      model.name = urdfTree->getName();
      details::parseRootTree(urdfTree->getRoot(),model,verbose);
      return model;
    }

  } // namespace urdf
} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_parsers_urdf_model_hxx__


// -*- c++ -*-
// Parallel text alignment via IBM1 / raw counts of word alignments
// aiming at high precision (to seed Yawat alignments)
// This program is tailored for use with Yawat.
// Written by Ulrich Germann.

#include <string>

#include <unicode/stringpiece.h>
#include <unicode/utypes.h>
#include <unicode/unistr.h>
#include <unicode/uchar.h>
#include <unicode/utf8.h>

#include "moses/TranslationModel/UG/generic/file_io/ug_stream.h"
#include "moses/TranslationModel/UG/mm/tpt_tokenindex.h"
#include <boost/unordered_map.hpp>
#include "moses/TranslationModel/UG/mm/tpt_pickler.h"
#include "moses/TranslationModel/UG/mm/ug_mm_2d_table.h"

using namespace std;
using namespace ugdiss;

typedef mm2dTable<id_type,id_type,uint32_t,uint32_t> table_t;

class IBM1
{
public:
  table_t COOC;
  TokenIndex V1,V2;

  void
  align(string const& s1, string const& s2, vector<int>& aln) const;

  void
  align(vector<id_type> const& x1,
	vector<id_type> const& x2,
	vector<int>& aln) const;

  void
  fill_amatrix(vector<id_type> const& x1,
	       vector<id_type> const& x2,
	       vector<vector<int> >& aln) const;

  void
  open(string const base, string const L1, string const L2);
};

void
IBM1::
open(string const base, string const L1, string const L2)
{
  V1.open(base+L1+".tdx");
  V2.open(base+L2+".tdx");
  COOC.open(base+L1+"-"+L2+".lex");
}

void
IBM1::
align(string const& s1, string const& s2, vector<int>& aln) const
{
  vector<id_type> x1,x2;
  V1.fillIdSeq(s1,x1);
  V2.fillIdSeq(s2,x2);
  align(x1,x2,aln);
}

 static UnicodeString apos   = UnicodeString::fromUTF8(StringPiece("'"));

string
u(StringPiece str, size_t start, size_t stop)
{
  string ret;
  UnicodeString::fromUTF8(str).tempSubString(start,stop).toUTF8String(ret);
  return ret;
}

void
IBM1::
fill_amatrix(vector<id_type> const& x1,
	     vector<id_type> const& x2,
	     vector<vector<int> >& aln) const
{
  aln.assign(x1.size(),vector<int>(x2.size()));
  for (size_t i = 0; i < x1.size(); ++i)
    for (size_t k = 0; k < x2.size(); ++k)
      aln[i][k] = COOC[x1[i]][x2[k]];
#if 0
  cout << setw(10) << " ";
  for (size_t k = 0; k < x2.size(); ++k)
    cout << setw(7) << right << u(V2[x2[k]],0,6);
  cout << endl;
  for (size_t i = 0; i < x1.size(); ++i)
    {
      cout << setw(10) << u(V1[x1[i]],0,10);
      for (size_t k = 0; k < x2.size(); ++k)
	{
	  if (aln[i][k] > 999999)
	    cout << setw(7) << aln[i][k]/1000 << " K";
	  else
	    cout << setw(7) << aln[i][k];
	}
      cout << endl;
    }
#endif
}

void
IBM1::
align(vector<id_type> const& x1,
      vector<id_type> const& x2,
      vector<int>& aln) const
{
  vector<vector<int> > M;
  // fill_amatrix(x1,x2,M);
  vector<int> i1(x1.size(),0), max1(x1.size(),0);
  vector<int> i2(x2.size(),0), max2(x2.size(),0);
  aln.clear();
  for (size_t i = 0; i < i1.size(); ++i)
    {
      for (size_t k = 0; k < i2.size(); ++k)
	{
	  int c = COOC[x1[i]][x2[k]];
	  if (c >  max1[i]) { i1[i] = k; max1[i] = c; }
	  if (c >= max2[k]) { i2[k] = i; max2[k] = c; }
	}
    }
  for (size_t i = 0; i < i1.size(); ++i)
    {
      if (max1[i] && i2[i1[i]] == i)
	{
	  aln.push_back(i);
	  aln.push_back(i1[i]);
	}
    }
}

int main(int argc, char* argv[])
{
  IBM1 ibm1;
  ibm1.open(argv[1],argv[2],argv[3]);
  string line1,line2,sid;
  while (getline(cin,sid))
    {
      if (!getline(cin,line1)) assert(false);
      if (!getline(cin,line2)) assert(false);
      vector<int> a;
      vector<id_type> s1,s2;
      ibm1.V1.fillIdSeq(line1,s1);
      ibm1.V2.fillIdSeq(line2,s2);
      ibm1.align(s1,s2,a);
      cout << sid;
      for (size_t i = 0; i < a.size(); i += 2)
	cout << " " << a[i] << ":" << a[i+1] << ":unspec";
      cout << endl;
      // cout << line1 << endl;
      // cout << line2 << endl;
      // for (size_t i = 0; i < a.size(); i += 2)
      // 	cout << ibm1.V1[s1[a[i]]] << " - "
      // 	     << ibm1.V2[s2[a[i+1]]] << endl;
    }
  // cout << endl;
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_DWAVE_SAMPLER_HPP
#define NETKET_DWAVE_SAMPLER_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "abstract_sampler.hpp"

#include "EmbeddingAlgorithm.hpp"
#include "IRGenerator.hpp"
#include "XACC.hpp"
#include "xacc_service.hpp"

namespace netket {

// Exact sampling using heat bath, mostly for testing purposes on small systems
class DWaveSampler : public AbstractSampler {
  // number of visible units
  const int nv_;

  // states of visible units
  Eigen::VectorXd v_;

  Eigen::VectorXd accept_;
  Eigen::VectorXd moves_;

  int mynode_;
  int totalnodes_;

  const HilbertIndex& hilbert_index_;

  const int dim_;

  std::discrete_distribution<int> dist_;

  std::vector<Complex> logpsivals_;
  std::vector<double> psivals_;

 protected:
  std::shared_ptr<xacc::Function> _rbm;
  std::shared_ptr<xacc::Accelerator> dwave;
  xacc::quantum::Embedding embedding;
  int nh = 0;

 public:
  explicit DWaveSampler(AbstractMachine& psi)
      : AbstractSampler(psi),
        nv_(GetHilbert().Size()),
        hilbert_index_(GetHilbert().GetIndex()),
        dim_(hilbert_index_.NStates()) {
    Init();
  }

  void Init() {
    v_.resize(nv_);
    try {
      nh = dynamic_cast<RbmSpinPhase&>(GetMachine()).Nhidden();
    } catch (const std::bad_cast& e) {
      // Cast failed, try real
      nh = dynamic_cast<RbmSpin&>(GetMachine()).Nhidden();
    }

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    if (!GetHilbert().IsDiscrete()) {
      throw InvalidInputError(
          "DWave sampler works only for discrete "
          "Hilbert spaces");
    }

    accept_.resize(1);
    moves_.resize(1);

    // Get the D-Wave QPU
    dwave = xacc::getAccelerator("dwave");

    // Create the RBM IR Generator
    auto rbmGenerator = xacc::getService<xacc::IRGenerator>("rbm");

    // Create RBMs for Amplitude and Phase
    _rbm =
        rbmGenerator->generate({{"visible-units", nv_}, {"hidden-units", nh}});

    /// ---
    // Create the Hardware graph for minor graph embedding
    auto hardwareconnections = dwave->getAcceleratorConnectivity();
    std::set<int> nUniqueBits;
    for (auto& edge : hardwareconnections) {
      nUniqueBits.insert(edge.first);
      nUniqueBits.insert(edge.second);
    }

    int nBits = *std::max_element(nUniqueBits.begin(), nUniqueBits.end()) + 1;

    auto hardware = xacc::getService<xacc::Graph>("boost-ugraph");
    for (int i = 0; i < nBits; i++) {
      std::map<std::string, xacc::InstructionParameter> m{{"bias", 1.0}};
      hardware->addVertex(m);
    }

    for (auto& edge : hardwareconnections) {
      hardware->addEdge(edge.first, edge.second);
    }

    int maxBitIdx = 0;
    for (auto inst : _rbm->getInstructions()) {
      if (inst->name() == "dw-qmi") {
        auto qbit1 = inst->bits()[0];
        auto qbit2 = inst->bits()[1];
        if (qbit1 > maxBitIdx) {
          maxBitIdx = qbit1;
        }
        if (qbit2 > maxBitIdx) {
          maxBitIdx = qbit2;
        }
      }
    }

    auto problemGraph = xacc::getService<xacc::Graph>("boost-ugraph");
    for (int i = 0; i < maxBitIdx + 1; i++) {
      std::map<std::string, xacc::InstructionParameter> m{{"bias", 1.0}};
      problemGraph->addVertex(m);
    }

    for (auto inst : _rbm->getInstructions()) {
      if (inst->name() == "dw-qmi") {
        auto qbit1 = inst->bits()[0];
        auto qbit2 = inst->bits()[1];
        if (qbit1 != qbit2) {
          problemGraph->addEdge(qbit1, qbit2, 1.0);
        }
      }
    }

    // Compute the minor graph embedding
    auto a = xacc::getService<xacc::quantum::EmbeddingAlgorithm>("cmr");
    embedding = a->embed(problemGraph, hardware);

    // Reset(true);

    InfoMessage() << "DWave sampler is ready " << std::endl;
  }

  void Reset(bool initrandom) override {
    if (initrandom) {
      GetHilbert().RandomVals(v_, this->GetRandomEngine());
    }

    psivals_.resize(dim_);
    //   auto v = hilbert_index_.NumberToState(4);
    //   std::cout << nh << ", HELLO: \n" << v << "\n";

    // auto vv = (Eigen::VectorXd::Ones(4) + v ) / 2;
    // std::cout << "HOWDY:\n" << vv << "\n";
    // exit(0);
    auto qbits1 = xacc::qalloc(2000);

    qbits1->addExtraInfo("embedding", embedding);

    auto params = GetMachine().GetParameters();
    auto half = params.size() / 2;
    Eigen::VectorXcd rbm1(half);

    // this is [visible | hidden | weights || visible | hidden | weights]
    rbm1.segment(0, nv_) = params.segment(0, nv_);
    rbm1.segment(nv_, nh) = params.segment(nv_, nh);

    const int initw = nv_ + nh;

    rbm1.segment(initw, nv_ * nh) = params.segment(initw, nv_ * nh);

    auto rbm1_tmp = rbm1.real();
    std::vector<double> rbm1vec(rbm1_tmp.data(),
                                rbm1_tmp.data() + rbm1_tmp.size());

    auto amp_tmp = _rbm->operator()(rbm1vec);

    dwave->execute(qbits1, amp_tmp);

    // Task for Sindhu
    // * Unembed the counts...
    // * Map counts to what they need for sampling
    // qbits1->print();
    auto results = qbits1->getMeasurementCounts();
    std::map<std::string, double> visibleCounts;
    double totalCounts = 0;
    for (auto& kv : results) {
        totalCounts += kv.second;
        auto vis = kv.first.substr(0, nv_);
        if (visibleCounts.count(vis)) {
            visibleCounts[vis] += kv.second;
        } else {
            visibleCounts.insert({vis, kv.second});
        }
    }
    // exit(0);

    // for (auto & kv : visibleCounts) {
    //     std::cout << "VIS: " << kv.first << ", " << kv.second << "\n";
    // }
    // FIXME psivals_ from counts distribution...
    for (int i = 0; i < dim_; ++i) {
      auto v = hilbert_index_.NumberToState(i);
      auto vv = (Eigen::VectorXi::Ones(nv_) + v.cast<int>() ) / 2;

      std::string bitStr = "";
      for (int j = 0; j < nv_; j++) bitStr += std::to_string(vv(j));

      if (visibleCounts.count(bitStr)) {
        psivals_[i] = visibleCounts[bitStr] / totalCounts;
      } else {
        psivals_[i] = 0.0;
      }
    }

    dist_ = std::discrete_distribution<int>(psivals_.begin(), psivals_.end());

    accept_ = Eigen::VectorXd::Zero(1);
    moves_ = Eigen::VectorXd::Zero(1);
  }

  void Sweep() override {
    int newstate = dist_(this->GetRandomEngine());
    v_ = hilbert_index_.NumberToState(newstate);

    accept_(0) += 1;
    moves_(0) += 1;
  }

  const Eigen::VectorXd& Visible() const noexcept override { return v_; }

  void SetVisible(const Eigen::VectorXd& v) override { v_ = v; }

  Eigen::VectorXd Acceptance() const override {
    Eigen::VectorXd acc = accept_;
    for (int i = 0; i < 1; i++) {
      acc(i) /= moves_(i);
    }
    return acc;
  }

  void SetMachineFunc(MachineFunction machine_func) override {
    AbstractSampler::SetMachineFunc(machine_func);
    Reset(true);
  }
};

}  // namespace netket

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_LINALG_DETAILS_BLAS_SV_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_DETAILS_BLAS_SV_HPP_INCLUDED

#include <complex>
#include <boost/preprocessor/cat.hpp>
#include <nt2/toolbox/linalg/details/blas/blas2.hpp>
#include <nt2/toolbox/linalg/details/utility/f77_wrapper.hpp>

namespace nt2 { namespace details
{
#define NT2_SV(T, PREFIX)                                                 \
inline void trsv( const char *uplo, const char *trans, const char *diag   \
                , const long int *n                                       \
                , const T *a, const long int *lda                         \
                , T *x      , const long int *incx                        \
                )                                                         \
{                                                                         \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,trsv))(uplo,trans,diag,n,a,lda,x,incx); \
}                                                                         \
/**/

  // INTERNAL ONLY
  // _TRSV ( UPLO, TRANS, DIAG, N, A, LDA, X, INCX ) S, D, C, Z
  NT2_SV(double, d)
  NT2_SV(float,  s)
  NT2_SV(std::complex<double> , z )
  NT2_SV(std::complex<float>  , c )
#undef NT2_SV

#define NT2_SV(T, PREFIX)                                                   \
inline void tbsv( const char *uplo, const char *trans, const char *diag     \
                , const long int *n, const long int *k                      \
                , const T *a, const long int *lda                           \
                , T *x      , const long int *incx                          \
                )                                                           \
{                                                                           \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,tbsv))(uplo,trans,diag,n,k,a,lda,x,incx); \
}                                                                           \
/**/

  // INTERNAL ONLY
  // _TBSV ( UPLO, TRANS, DIAG, N, K, A, LDA, X, INCX ) S, D, C, Z
  NT2_SV(double, d)
  NT2_SV(float,  s)
  NT2_SV(std::complex<double>, z)
  NT2_SV(std::complex<float>, c)
#undef NT2_SV

#define NT2_SV(T, PREFIX)                                                 \
inline void tpsv( const char *uplo, const char *trans, const char *diag   \
                , const long int *n, const T *ap                          \
                , T *x, const long int *incx                              \
                )                                                         \
{                                                                         \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,tpsv))(uplo,trans,diag,n,ap,x,incx);    \
}                                                                         \

  // _TPSV ( UPLO, TRANS, DIAG, N, AP, X, INCX ) S, D, C, Z
  NT2_SV(double, d)
  NT2_SV(float,  s)
  NT2_SV(std::complex<double> , z )
  NT2_SV(std::complex<float>  , c )
#undef NT2_SV
} }

#endif