text:
/*
 * vi:ts=4:tw=78:shiftwidth=4:expandtab
 * vim600:fdm=marker
 *
 * gistrainer.cpp  -  a trainer for conditional ME model with GIS algorithm
 *
 * An implementation of Generalized Iterative Scaling.  The reference paper
 * for this implementation was Adwait Ratnaparkhi's tech report at the
 * University of Pennsylvania's Institute for Research in Cognitive Science,
 * and is available at ftp://ftp.cis.upenn.edu/pub/ircs/tr/97-08.ps.Z
 *
 * This C++ implementation is originally based on java maxent implementation,
 * with the help of developers from java maxent.
 * see http://maxent.sf.net
 *
 * Current implementation implements the "Correction Free" GIS algorithm with
 * Gaussian prior smoothing described in [Curran and Clark, 2003]:
 * "Investigating GIS and Smoothing for Maximum Entropy Taggers".
 *
 * Without the computation of correction parameter, the new GIS algorithm is
 * much faster, making the already simple algorithm simpler.
 *
 * Copyright (C) 2002 by Zhang Le <ejoy@users.sourceforge.net>
 * Begin       : 31-Dec-2002
 * Last Change : 08-Feb-2012.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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 Lesser General Public
 * License along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <cassert>
#include <cfloat>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <tr1/unordered_map>
#include <boost/timer.hpp>
#include <idmlib/maxent/gistrainer.hpp>
#include <idmlib/maxent/display.hpp>
#include <idmlib/maxent/finite.h>

namespace maxent{

void GISTrainer::init_trainer() {
    assert(m_params->size() > 0);
    assert(m_es->size() > 0);

    // (bool) is needed for BCC5.5
    if ((bool)m_heldout_es && m_heldout_es->size() > 0) {
        cerr << "calculating heldout accuracy is not supported in GIS trainer yet." << endl;
    }

    m_modifiers.reset(new vector<vector<double> >
            (m_params->size(), vector<double>(0)) );
    m_observed_expects.reset(new vector<vector<double> >
            (m_params->size(), vector<double>(0)) );

    // init all thetas to 0.0
    for (size_t i = 0; i < m_params->size(); ++i) {
        std::vector<pair<size_t, size_t> >& param = (*m_params)[i];
        (*m_modifiers)[i].resize(param.size());
        (*m_observed_expects)[i].resize(param.size());
        for (size_t j = 0; j < param.size(); ++j) {
            m_theta[param[j].second] = 0.0;
            assert((*m_modifiers)[i][j] == 0.0);
        }
    }

    // determine the correction constant
    // C = max sum_{x,y} f_i(x, y)
    // m_correct_constant = (*m_es)[0].context_size();
    m_correct_constant = -999;
    for (size_t i = 0; i < m_es->size(); ++i) {
        // size_t len = (*m_es)[i].context_size();
        double t = 0.0;
        Event& e = (*m_es)[i];
        // assume no duplicated features
        for (size_t j = 0; j < e.context_size(); ++j) {
            assert (e.m_context[j].second >= 0.0);
            t += e.m_context[j].second;
        }

        if (t > m_correct_constant)
            m_correct_constant = t;
    }

    m_N = 0;
    for (vector<Event>::iterator it = m_es->begin();
            it != m_es->end(); ++it) {
        // XXX: is the calculation of active features correct?
        m_N += it->m_count;
    }

    // calculate observed feature expectations
    // a hash map to hold the value of feature <pred,outcome> pair occured in event list
    // which is the sum of active feature f_i(a,b) in the training set
    typedef std::tr1::unordered_map <pair<size_t, size_t>, float, featid_hasher> FeatSumMap;
    FeatSumMap feat_sum;
    for (vector<Event>::const_iterator it = m_es->begin();
            it != m_es->end(); ++it) {
        size_t len = it->context_size();
        for (size_t i = 0; i < len; ++i) {
        // check for feature values, current implementation only supports
        // binary features
//            if (it->m_context[i].second != 1.0)
//                throw runtime_error("Current GIS implementation only supports binary features, use L-BFGS instead");
            feat_sum[make_pair(it->m_context[i].first,it->m_outcome)] +=
                (it->m_count * it->m_context[i].second);
        }
    }

    // Get the observed expectations of the features. Strictly speaking,
    // we should divide the counts by the number of tokens, but because of
    // the way the model's expectations are approximated in the
    // implementation, this is cancelled out when we compute the next
    // iteration of a parameter, making the extra divisions wasteful.
    // Because we need no division of N in Ep<f_i> and Eq<f_i>
    // when calculating delta of update paramater:
    // lambda(n+1) = lambda(n) + (1/C)*[log(Ep<f_i>) - log(Eq<f_i>)]

    FeatSumMap::iterator it;
    for (size_t pid = 0; pid < m_params->size(); ++pid) {
        vector<pair<size_t, size_t> >& param = (*m_params)[pid];
        vector<double>& observ = (*m_observed_expects)[pid];
        for (size_t j = 0; j < param.size(); ++j) {
            it = feat_sum.find(make_pair(pid,param[j].first));
            assert(it != feat_sum.end());
            if (it == feat_sum.end())
                throw runtime_error("broken training data: some <pid, oid> in params not found in training data");

            observ[j] = it->second; // Ep<f_i> = sum C(f_i)*f_i
        }
    }

    if (!m_sigma2) {
        // We are not using a prior, so we can save log(E_ref) instead
        // to avoid unnecessary log(*) operation during gis parameter updates

        const double LOG_ZERO = log(numeric_limits<double>::min());
        for (size_t pid = 0; pid < m_params->size(); ++pid) {
            vector<pair<size_t, size_t> >& param = (*m_params)[pid];
            vector<double>& observ = (*m_observed_expects)[pid];
            for (size_t j = 0; j < param.size(); ++j) {
                observ[j] = (observ[j] == 0.0) ? LOG_ZERO : log(observ[j]);
            }
        }
    }
}

void GISTrainer::train(size_t iter, double tol) {
    if (!m_params || !m_es)
        throw runtime_error("Can not train on an empty model");

    init_trainer();

    // now enter iterations
    const double LOG_ZERO = log(numeric_limits<double>::min());
    double old_loglikelihood = 99999;
    double new_loglikelihood = 0.0;
    double acc;
    size_t correct;
    size_t best_oid;
    vector<double> q(m_n_outcomes); // q(y|x)
    boost::timer t;
    size_t niter = 0;

    display("");
    display("Starting GIS iterations...");
    display("Number of Predicates: %d", m_params->size());
    display("Number of Outcomes:   %d", m_n_outcomes);
    display("Number of Parameters: %d", m_n_theta);
    display("Tolerance:            %E", tol);
    display("Gaussian Penalty:     %s", (m_sigma2?"on":"off"));
#if defined(NDEBUG)
    display("Optimized version");
#endif
    display("iters   loglikelihood    training accuracy   heldout accuracy");
    display("=============================================================");

    for (; niter < iter;) {
        new_loglikelihood = 0.0;
        correct = 0;

        // computer modifiers for all features from training data
        for (vector<Event>::iterator it = m_es->begin();
                it != m_es->end(); ++it) {
            best_oid = eval(it->m_context, it->context_size(), q);
            if (best_oid == it->m_outcome)
                correct += it->m_count;
            // TODO:optimize the code
            // calculate Eq<f_i> = \sum q(y|x) * Count(f_i) * f_i(x, y)
            // (need not being divided by N)
            for (size_t i = 0; i < it->context_size(); ++i) {
                size_t pid = it->m_context[i].first;
                double fval = it->m_context[i].second;
                vector<pair<size_t, size_t> >& param = (*m_params)[pid];
                for (size_t j = 0; j < param.size(); ++j) {
                    size_t oid = param[j].first;
                    (*m_modifiers)[pid][j] += q[oid] * it->m_count * fval;
                    // binary case: (*m_modifiers)[pid][j] += q[oid] * it->m_count;
                }
            }
            assert(finite(q[it->m_outcome]));
            double t = log(q[it->m_outcome]);
            new_loglikelihood += (finite(t) ? t : LOG_ZERO) * it->m_count;
            assert(finite(new_loglikelihood));
        }
        acc = correct/double(m_N);

        // compute the new parameter values
        if (m_sigma2) { // applying Gaussian penality
            for (size_t pid = 0; pid < m_params->size(); ++pid) {
                vector<pair<size_t, size_t> >& param = (*m_params)[pid];
                for (size_t i = 0; i < param.size(); ++i) {
                    size_t fid = param[i].second;
                    m_theta[fid] += newton((*m_modifiers)[pid][i],
                            (*m_observed_expects)[pid][i], fid);
                    (*m_modifiers)[pid][i] = 0.0; // clear modifiers for next iteration
                }
            }
        } else {
            for (size_t pid = 0; pid < m_params->size(); ++pid) {
                vector<pair<size_t, size_t> >& param = (*m_params)[pid];
                for (size_t i = 0; i < param.size(); ++i) {
                    if ((*m_modifiers)[pid][i] != 0.0) {
                        m_theta[param[i].second] +=
                            ((*m_observed_expects)[pid][i] -
                             log((*m_modifiers)[pid][i])) / m_correct_constant;
                        (*m_modifiers)[pid][i] = 0.0; // clear modifiers for next iteration
                    } else {
                        // E_q == 0 means feature value is 0, which means
                        // update for this parameter will always be zero,
                        // hence can be ignored.
                    }
                }
            }
        }

        ++niter;
        display("%3d\t%E\t  %.3f%%\t     %s",
                niter , (new_loglikelihood/m_N) , (acc*100) ,  "N/A");
        if (fabs((old_loglikelihood - new_loglikelihood)/old_loglikelihood) < tol) {
            display("Training terminats succesfully in %.2f seconds", t.elapsed());
            break;
        }
        old_loglikelihood = new_loglikelihood;
    }
    if (niter >= iter)
        display("Maximum numbers of %d iterations reached in %.2f seconds", iter , t.elapsed());

    // kill a bunch of these big objects now that we don't need them
    m_modifiers.reset();
    m_observed_expects.reset();
}

// Calculate the ith GIS parameter updates with Gaussian prior
// using Newton-Raphson method
// the update rule is the solution of the following equation:
//                                   lambda_i + delta_i
// E_ref = E_q * exp (C * delta_i) + ------------------ * N
//                                       sigma_i^2
// note: E_ref and E_q were not divided by N
double GISTrainer::newton(double f_q, double f_ref, size_t i, double tol) {
    size_t maxiter = 50;
    double x0 = 0.0;
    double x = 0.0;

    for (size_t iter = 1; iter <= maxiter; ++iter) {
        double t = f_q * exp(m_correct_constant * x0);
        double fval = t + m_N * (m_theta[i] + x0) / m_sigma2[i] - f_ref;
        double fpval = t * m_correct_constant + m_N / m_sigma2[i];
        if (fpval == 0) {
            cerr <<
                "Warning: zero-derivative encountered in newton() method."
                << endl;
            return x0;
        }
        x = x0 - fval/fpval;
        if (abs(x-x0) < tol)
            return x;
        x0 = x;
    }
    throw runtime_error("Failed to converge after 50 iterations in newton() method");
}

} // namespace maxent

max_stars_repo_name: izenecloud/idmlib

max_issues_repo_name: izenecloud/idmlib

max_forks_repo_name: izenecloud/idmlib


text:
// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl-Containment.h>

// Using =======================================================================
using namespace boost::python;

// Declarations ================================================================
namespace gmtlWrappers
{

template<typename DATA_TYPE>
void makeVolume(gmtl::Sphere<DATA_TYPE>& container, list pts)
{
   const unsigned int size = extract<unsigned int>(pts.attr("__len__")());
   std::vector< gmtl::Point<DATA_TYPE, 3> > pt_vec(size);

   for ( unsigned int i = 0; i < size; ++i )
   {
#if defined(__GNUC__) && __GNUC__ == 3 && __GNUC_MINOR__ == 2
      gmtl::Point<DATA_TYPE, 3> temp = extract< gmtl::Point<DATA_TYPE, 3> >(pts[i]);
      pt_vec[i] = temp;
#else
      pt_vec[i] = extract< gmtl::Point<DATA_TYPE, 3> >(pts[i]);
#endif
   }

   gmtl::makeVolume(container, pt_vec);
}

}

namespace  {

BOOST_PYTHON_FUNCTION_OVERLOADS(isOnVolume_overloads_2_3, gmtl::isOnVolume, 2, 3)

}// namespace

// Module ======================================================================
void _Export_gmtl_Containment_h()
{
    def("extendVolume", (void (*)(gmtl::Sphere<double> &, const gmtl::Sphere<double> &))&gmtl::extendVolume, return_internal_reference< 1 >());
    def("extendVolume", (void (*)(gmtl::Sphere<float> &, const gmtl::Sphere<float> &))&gmtl::extendVolume, return_internal_reference< 1 >());
    def("extendVolume", (void (*)(gmtl::Sphere<float> &, const gmtl::Point<float,3> &))&gmtl::extendVolume, return_internal_reference< 1 >());
    def("extendVolume", (void (*)(gmtl::AABox<double> &, const gmtl::Point<double,3> &))&gmtl::extendVolume, return_internal_reference< 1 >());
    def("extendVolume", (void (*)(gmtl::AABox<double> &, const gmtl::AABox<double> &))&gmtl::extendVolume, return_internal_reference< 1 >());
    def("extendVolume", (void (*)(gmtl::AABox<float> &, const gmtl::AABox<float> &))&gmtl::extendVolume, return_internal_reference< 1 >());
    def("extendVolume", (void (*)(gmtl::Sphere<double> &, const gmtl::Point<double,3> &))&gmtl::extendVolume, return_internal_reference< 1 >());
    def("extendVolume", (void (*)(gmtl::AABox<float> &, const gmtl::Point<float,3> &))&gmtl::extendVolume, return_internal_reference< 1 >());
    def("isInVolume", (bool (*)(const gmtl::Sphere<float> &, const gmtl::Sphere<float> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Sphere<double> &, const gmtl::Point<double,3> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Sphere<double> &, const gmtl::Sphere<double> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Sphere<float> &, const gmtl::Point<float,3> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::AABox<double> &, const gmtl::Point<double,3> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::AABox<float> &, const gmtl::Point<float,3> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::AABox<double> &, const gmtl::AABox<double> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::AABox<float> &, const gmtl::AABox<float> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Frustum<double> &, const gmtl::Sphere<double> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Frustum<float> &, const gmtl::Sphere<float> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Frustum<double> &, const gmtl::AABox<double> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Frustum<float> &, const gmtl::AABox<float> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Frustum<double> &, const gmtl::Tri<double> &))&gmtl::isInVolume);
    def("isInVolume", (bool (*)(const gmtl::Frustum<float> &, const gmtl::Tri<float> &))&gmtl::isInVolume);
    def("isInVolumeExclusive", (bool (*)(const gmtl::AABox<double> &, const gmtl::Point<double,3> &))&gmtl::isInVolumeExclusive);
    def("isInVolumeExclusive", (bool (*)(const gmtl::AABox<float> &, const gmtl::Point<float,3> &))&gmtl::isInVolumeExclusive);
    def("isOnVolume", (bool (*)(const gmtl::Sphere<double> &, const gmtl::Point<double,3> &, const double &))&gmtl::isOnVolume, isOnVolume_overloads_2_3());
    def("isOnVolume", (bool (*)(const gmtl::Sphere<float> &, const gmtl::Point<float,3> &, const float &))&gmtl::isOnVolume, isOnVolume_overloads_2_3());
    def("makeVolume", (void (*)(gmtl::Sphere<float> &, list))&gmtlWrappers::makeVolume, return_internal_reference< 1 >());
    def("makeVolume", (void (*)(gmtl::AABox<float> &, const gmtl::Sphere<float> &))&gmtl::makeVolume, return_internal_reference< 1 >());
    def("makeVolume", (void (*)(gmtl::Sphere<double> &, list))&gmtlWrappers::makeVolume, return_internal_reference< 1 >());
    def("makeVolume", (void (*)(gmtl::AABox<double> &, const gmtl::Sphere<double> &))&gmtl::makeVolume, return_internal_reference< 1 >());
}

ext: cpp


text:
/*
 *  Copyright 2015 Aldebaran. All rights reserved.
 *
 */
#include <almath/scenegraph/urdf.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>
#include <boost/lexical_cast.hpp>
#include <Eigen/Geometry>

#include <boost/multi_index_container.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/identity.hpp>
#include <boost/multi_index/mem_fun.hpp>
#include <map>
#include <iostream>
#include <almath/scenegraph/urdfeigen.h>

namespace {

using namespace boost::property_tree;

std::string name_it(ptree::const_iterator pt) {
  return AL::urdf::name(pt->second);
}

std::string child_link_it(ptree::const_iterator pt) {
  return AL::urdf::child_link(pt->second);
}

std::string parent_link_it(ptree::const_iterator pt) {
  return AL::urdf::parent_link(pt->second); }

boost::optional<std::string> mimic_it(ptree::const_iterator pt) {
  boost::optional<std::string> ret;
  if (auto child = pt->second.get_child_optional("mimic")) {
    ret = child->get<std::string>("<xmlattr>.joint");
  }
  return ret;
}

// boost::multiindex does not provide an "at()" member function.
template <typename T, typename K>
typename T::const_iterator checked_find(const T &t, const K &k) {
  typename T::const_iterator it = t.find(k);
  if (it == t.end()) throw std::out_of_range("checked_find");
  return it;
}

template <typename T, typename K>
typename T::iterator checked_find(T &t, const K &k) {
  typename T::iterator it = t.find(k);
  if (it == t.end()) throw std::out_of_range("checked_find");
  return it;
}
}

namespace AL {

namespace urdf {

boost::optional<Array3dTranslator::external_type> Array3dTranslator::get_value(
    const Array3dTranslator::internal_type &str) {
  // str is expected to hold 3 floating point numbers.
  // let split it into 3 strings, then use the usual boost::property_tree
  // translator to convert each substring into a double.
  // In case of failure, return an uninitialized boost::optional, like
  // boost::property_tree does itself.
  boost::tokenizer<> tok(
      str, boost::char_delimiters_separator<char>(false, "", " \t\n\v\f\r"));
  boost::optional<double> d;
  boost::property_tree::translator_between<internal_type, double>::type tr;
  external_type x;  // Array3d
  boost::tokenizer<>::iterator beg = tok.begin();
  size_t i = 0;
  for (; beg != tok.end() && i < 3; ++beg, ++i) {
    d = tr.get_value(*beg);
    if (!d) return boost::optional<external_type>();
    x[i] = *d;
  }
  if (i != 3 || beg != tok.end())
    return boost::optional<external_type>();  // todo: maybe throw
  return boost::optional<external_type>(x);
}

boost::optional<Array3dTranslator::internal_type> Array3dTranslator::put_value(
    const Array3dTranslator::external_type &v) {
  boost::property_tree::translator_between<internal_type, double>::type tr;
  std::ostringstream ss;
  ss << *tr.put_value(v[0]) << " " << *tr.put_value(v[1]) << " "
     << *tr.put_value(v[2]);
  return ss.str();
}

std::string name(const ptree &pt) {
  return pt.get<std::string>("<xmlattr>.name");
}

std::string parent_link(const ptree &pt) {
  return pt.get<std::string>("parent.<xmlattr>.link");
}

std::string child_link(const ptree &pt) {
  return pt.get<std::string>("child.<xmlattr>.link");
}

Pose Pose::inverse() const {
  Pose res;
  if (is_zero(_rpy)) {
    res._xyz = {{-_xyz[0], -_xyz[1], -_xyz[2]}};
  } else {
    Eigen::Matrix3d new_mat =
        Math::eigenQuaternionFromUrdfRpy(_rpy).inverse().matrix();
    res._rpy = Math::urdfRpyFromEigenMatrix3(new_mat);
    if (!is_zero(_xyz)) {
      Eigen::Map<Eigen::Vector3d>(res._xyz.data()) =
          -(new_mat * Eigen::Map<const Eigen::Vector3d>(_xyz.data()));
    }
  }
  return res;
}

Pose operator*(const Pose &lhs, const Pose &rhs) {
  Pose res{lhs.xyz(), {{0, 0, 0}}};
  Eigen::Quaterniond lhs_quat = Math::eigenQuaternionFromUrdfRpy(lhs.rpy());
  if (is_zero(lhs.rpy())) {
    res._rpy = rhs.rpy();
  } else if (is_zero(rhs.rpy())) {
    res._rpy = lhs.rpy();
  } else {
    res._rpy = Math::urdfRpyFromEigenMatrix3(
        (lhs_quat * Math::eigenQuaternionFromUrdfRpy(rhs.rpy())).matrix());
  }
  Eigen::Map<Eigen::Vector3d>(res._xyz.data()) +=
      lhs_quat * Eigen::Map<const Eigen::Vector3d>(rhs._xyz.data());
  return res;
}

// private helper
static Array3d get_pose_child_array3d(const ptree &pt, const char *name) {
  if (boost::optional<const ptree &> child = pt.get_child_optional(name))
    return child->get_value<Array3d>(Array3dTranslator());
  return Array3d{0, 0, 0};
}

Pose Pose::from_ptree(const ptree &pt) {
  return Pose(get_pose_child_array3d(pt, "<xmlattr>.xyz"),
              get_pose_child_array3d(pt, "<xmlattr>.rpy"));
}

Pose Pose::from_ptree(const boost::optional<const ptree &> &pt) {
  return pt ? from_ptree(*pt) : Pose();
}

boost::optional<ptree> Pose::to_ptree() const {
  if (is_zero(xyz()) && is_zero(rpy())) return boost::optional<ptree>();
  ptree res;
  if (!is_zero(xyz()))
    res.put<Array3d>("<xmlattr>.xyz", xyz(), Array3dTranslator());
  if (!is_zero(rpy()))
    res.put<Array3d>("<xmlattr>.rpy", rpy(), Array3dTranslator());
  return res;
}

Mimic::Mimic(const ptree &pt) : pt(pt) {}

std::string Mimic::joint() const {
  return pt.get<std::string>("<xmlattr>.joint");
}

double Mimic::multiplier() const {
  if (boost::optional<const ptree &> child =
          pt.get_child_optional("<xmlattr>.multiplier"))
    return child->get_value<double>();
  return 1.;
}

double Mimic::offset() const {
  if (boost::optional<const ptree &> child =
          pt.get_child_optional("<xmlattr>.offset"))
    return child->get_value<double>();
  return 0.;
}

Joint::Joint(const ptree &pt) : pt(pt) {}
std::string Joint::name() const {
  return pt.get<std::string>("<xmlattr>.name");
}

Joint::Type Joint::type() const {
  const std::string str = pt.get<std::string>("<xmlattr>.type");
  if (str == "revolute") return revolute;
  if (str == "continuous") return continuous;
  if (str == "prismatic") return prismatic;
  if (str == "fixed") return fixed;
  if (str == "floating") return floating;
  if (str == "planar") return planar;
  throw std::runtime_error("unknown joint type");
}

std::string Joint::parent_link() const {
  return pt.get<std::string>("parent.<xmlattr>.link");
}

std::string Joint::child_link() const {
  return pt.get<std::string>("child.<xmlattr>.link");
}

Pose Joint::origin() const {
  return Pose::from_ptree(pt.get_child_optional("origin"));
}

Array3d Joint::axis() const {
  return pt.get<Array3d>("axis.<xmlattr>.xyz",
                         Array3d{1, 0, 0},
                         Array3dTranslator());
}

boost::optional<std::pair<double, double>> Joint::limit_lower_upper() const {
  using Pair = std::pair<double, double>;
  using OptPair = boost::optional<Pair>;
  const auto lower = pt.get_optional<double>("limit.<xmlattr>.lower");
  const auto upper = pt.get_optional<double>("limit.<xmlattr>.upper");
  if (lower || upper)
    return Pair(lower.get_value_or(0.), upper.get_value_or(0.));
  return OptPair();
}

boost::optional<double> Joint::limit_effort() const {
  return pt.get_optional<double>("limit.<xmlattr>.effort");
}

boost::optional<double> Joint::limit_velocity() const {
  return pt.get_optional<double>("limit.<xmlattr>.velocity");
}

boost::optional<Mimic> Joint::mimic() const {
  if (auto child = pt.get_child_optional("mimic"))
    return Mimic(*child);
  return boost::optional<Mimic>();
}

Inertial::Inertial(const ptree &pt) : pt(pt) {}

Pose Inertial::origin() const {
  return Pose::from_ptree(pt.get_child_optional("origin"));
}

double Inertial::mass() const { return pt.get<double>("mass.<xmlattr>.value"); }

#define DEF_INERTIA_GETTER(member)                       \
  double Inertial::member() const {                      \
    return pt.get<double>("inertia.<xmlattr>." #member); \
  }

DEF_INERTIA_GETTER(ixx);
DEF_INERTIA_GETTER(ixy);
DEF_INERTIA_GETTER(ixz);
DEF_INERTIA_GETTER(iyy);
DEF_INERTIA_GETTER(iyz);
DEF_INERTIA_GETTER(izz);

#undef DEF_INERTIA_GETTER

Box::Box(const ptree &pt) : pt(pt) {}

Array3d Box::size() {
  return pt.get<Array3d>("<xmlattr>.size", Array3dTranslator());
}

Cylinder::Cylinder(const ptree &pt) : pt(pt) {}

double Cylinder::radius() {
  return pt.get<double>("<xmlattr>.radius");
}

double Cylinder::length() {
  return pt.get<double>("<xmlattr>.length");
}

Sphere::Sphere(const ptree &pt) : pt(pt) {}

double Sphere::radius() {
  return pt.get<double>("<xmlattr>.radius");
}

Mesh::Mesh(const ptree &pt) : pt(pt) {}

std::string Mesh::filename() const {
  return pt.get<std::string>("<xmlattr>.filename");
}

Array3d Mesh::scale() const {
  return pt.get<Array3d>("<xmlattr>.scale",
                         Array3d{1., 1., 1.},
                         Array3dTranslator());
}

Visual::Visual(const ptree &pt) : pt(pt) {}

Pose Visual::origin() const {
  return Pose::from_ptree(pt.get_child_optional("origin"));
}

Geometry Visual::geometry() const {
  const auto &g = pt.get_child("geometry");

  boost::optional<const ptree &> gc;
  if ((gc = g.get_child_optional("box")))
    return Box(*gc);
  if ((gc = g.get_child_optional("cylinder")))
    return Cylinder(*gc);
  if ((gc = g.get_child_optional("sphere")))
    return Sphere(*gc);
  if ((gc = g.get_child_optional("mesh")))
    return Mesh(*gc);
  throw std::runtime_error("this geometry has no shape");
}

bool Visual::is_visual(const ptree::value_type &val) {
  return val.first == "visual";
}

Link::Link(const ptree &pt) : pt(pt) {}

std::string Link::name() const { return pt.get<std::string>("<xmlattr>.name"); }

boost::optional<Inertial> Link::inertial() const {
  if (boost::optional<const ptree &> child = pt.get_child_optional("inertial"))
    return boost::optional<Inertial>(Inertial(*child));
  return boost::optional<Inertial>();
}

namespace detail {

using namespace boost::multi_index;

class RobotTreeP {
 public:
  // tags
  struct Name {};
  struct ParentLink {};
  struct ChildLink {};
  struct Mimic {};

  typedef boost::multi_index_container<
      ptree::iterator,
      indexed_by<
          sequenced<>,               // used to keep the original joint ordering
          ordered_unique<tag<Name>,  // joint name
                         global_fun<ptree::const_iterator, std::string, &::name_it> >,
          ordered_unique<tag<ChildLink>, global_fun<ptree::const_iterator, std::string,
                                                    &::child_link_it> >,
          ordered_non_unique<
              tag<ParentLink>,
              global_fun<ptree::const_iterator, std::string, &::parent_link_it> >,
          ordered_non_unique<
              tag<Mimic>,
              global_fun<ptree::const_iterator, boost::optional<std::string>,
                         &::mimic_it> > > > Joints;

  typedef std::map<std::string, ptree::iterator> Links;

  typedef std::function<bool(ptree::const_iterator)> donotprune_fct;

  ptree &robot;
  Links links;
  Joints joints;
  std::string root_link;
  RobotTreeP(ptree &robot); // keeps refs and points to robot elements
  void rm_root_joint();
  void rm_leaf_joint(const std::string &name);
  void transport_root_link_frame(
      const Pose &pose,
      const donotprune_fct &noprune = [](ptree::const_iterator) { return false; });
  void flip_root_link_joint(const std::string &joint_name,
                            bool add_prune_exception);
  void define_as_root_link(const std::string &link_name);
  void walk_joint(RobotTree::JointConstVisitor &vis,
                  const std::string &link) const;
  void walk_joint(RobotTree::JointVisitor &vis, const std::string &link);
  bool is_mimic_tree_flat() const;
  // return true if lhs mimics rhs or a joint which itself mimics rhs.
  // This function is defined static so that it can be used from the ctor.
  static bool transitively_mimics(const Joints &joints,
                                  ptree::const_iterator lhs_joint_it,
                                  const std::string &rhs_joint_name);
};

bool RobotTreeP::transitively_mimics(const Joints &joints,
                                     ptree::const_iterator lhs_joint_it,
                                     const std::string &rhs_joint_name) {
  BOOST_FOREACH(ptree::const_iterator joint,
                joints.get<Mimic>().equal_range(rhs_joint_name)) {
    if ((joint == lhs_joint_it) ||
        transitively_mimics(joints, joint, rhs_joint_name))
      return true;
  }
  return false;
}

RobotTreeP::RobotTreeP(ptree &robot) : robot(robot) {
  std::set<std::string> root_links;

  // list all the links
  for(auto it = robot.begin(); it != robot.end(); ++it) {
    if (it->first != "link")
      continue;
    const std::string name = ::name_it(it);
    if (!links.emplace(name, it).second) {
      throw std::runtime_error("cannot add link \"" + name +
                               "\": duplicate link name");
    }
    root_links.insert(name);
  }
  if (links.empty()) throw std::runtime_error("this urdf file has no link");

  // list all the joints and check the kinematic tree consistency
  for(auto it = robot.begin(); it != robot.end(); ++ it) {
    if (it->first != "joint")
      continue;
    const auto name = ::name_it(it);

    if (joints.get<Name>().find(name) != joints.get<Name>().end())
      throw std::runtime_error("cannot add joint \"" + name +
                               "\": duplicate joint name");

    const auto parent_link = ::parent_link_it(it);
    if (links.find(parent_link) == links.end())
      throw std::runtime_error("cannot add joint \"" + name +
                               "\": non-existing parent link \"" +
                               parent_link + "\"");

    const auto child_link = ::child_link_it(it);
    if (links.find(child_link) == links.end())
      throw std::runtime_error("cannot add joint \"" + name +
                               "\": non-existing child link \"" +
                               child_link + "\"");

    if (joints.get<ChildLink>().find(child_link) !=
        joints.get<ChildLink>().end())
      throw std::runtime_error("cannot add joint \"" + name +
                               "\": there is a loop in the kinematic tree");
    if (boost::optional<std::string> source_name = ::mimic_it(it)) {
      // joint_kv mimics source_name, let check that adding it won't
      // create a "mimicking loop":
      if (name == *source_name) {
        throw std::runtime_error("cannot add joint \"" + name +
                                 "\": there is a loop in the mimic tree");
      }
      // check that, if it is already in the tree, source joint does not
      // itself mimics joint_kv.
      auto source_joint = joints.get<Name>().find(*source_name);
      if ((source_joint != joints.get<Name>().end()) &&
          transitively_mimics(joints, *source_joint, name))
        throw std::runtime_error("cannot add joint \"" + name +
                                 "\": there is a loop in the mimic tree");
    }
    joints.push_back(it);
    auto link_it = root_links.find(child_link);
    assert(link_it != root_links.end());
    root_links.erase(link_it);
    if (root_links.empty())
      throw std::runtime_error("cannot add joint \"" + name +
                               "\": there is a loop in the kinematic tree");
  }
  assert(!root_links.empty());
  if (root_links.size() != 1)
    throw std::runtime_error("kinematic tree has several roots");
  // Now that all the joints have been added, let check that there is not any
  // mimicking a non-existent joint.
  for(auto joint_it: joints.get<Name>()) {
    if (boost::optional<std::string> source_name = ::mimic_it(joint_it)) {
      if (joints.get<Name>().find(*source_name) == joints.get<Name>().end()) {
        throw std::runtime_error("cannot add joint \"" +
                                 ::name_it(joint_it) +
                                 "\": it mimics a non-existent joint named \"" +
                                 *source_name + "\"");
      }
    }
  }
  root_link = *root_links.begin();
}

void RobotTreeP::rm_root_joint() {
  // joints whose  parent link is root_link
  if (joints.get<ParentLink>().count(root_link) != 1) {
    throw std::runtime_error("rm_root_joint: there is not a single root joint");
  }
  auto root_joint_it2 = joints.get<ParentLink>().find(root_link);
  auto root_link_it2 = links.find(root_link);

  root_link = ::child_link_it(*root_joint_it2);

  robot.erase(root_link_it2->second);
  robot.erase(*root_joint_it2);

  joints.get<ParentLink>().erase(root_joint_it2);
  links.erase(root_link_it2);
}

void RobotTreeP::rm_leaf_joint(const std::string &name) {
  auto joint_it2 = checked_find(joints.get<Name>(), name);
  const auto child_link = ::child_link_it(*joint_it2);
  // joints whose parent link is child_link_name
  if (joints.get<ParentLink>().count(child_link) > 0) {
    throw std::runtime_error("rm_leaf_joint: the joint \"" + name +
                             "\" is not a leaf in the kinematic tree");
  }
  auto link_it2 = links.find(child_link);

  robot.erase(*joint_it2);
  robot.erase(link_it2->second);

  joints.get<Name>().erase(joint_it2);
  links.erase(link_it2);
}

// premultiply the rhs with lhs. So that
// new_rhs = lhs * old_rhs
//
// ilhs shall be the inverse of lhs. Thus, if lhs -== old_rhs, then
// we know that new_rhs == identity, without suffering from numerical noise
// coming from inversion and product.
void premul(const Pose &lhs, const Pose &ilhs, ptree *rhs_parent,
            bool noprune) {
  assert(lhs == ilhs.inverse());
  auto const_rhs_parent = const_cast<const ptree *>(rhs_parent);

  Pose rhs = Pose::from_ptree(const_rhs_parent->get_child_optional("origin"));
  if (rhs == ilhs) {
    if (noprune) {
      // instead of removing the element, empty it.
      // Wrt to URDF, this has the same meaning (identity transform)
      // Keeping the element around is more verbose (bad), but enables to
      // keeps its position in the tree it we want to modify it again
      // (otherwise we would create a new element which would be the last
      // among simblings)
      rhs_parent->put_child("origin", ptree());
    } else {
      rhs_parent->erase("origin");
    }
  } else {
    boost::optional<ptree> new_origin = (lhs * rhs).to_ptree();
    assert(new_origin);
    rhs_parent->put_child("origin", *new_origin);
  }
}

void premul(const Pose &lhs, const Pose &ilhs, ptree::assoc_iterator rhs_parent,
            bool noprune) {
  premul(lhs, ilhs, &(rhs_parent->second), noprune);
}

void premul(const Pose &lhs, const Pose &ilhs, ptree::iterator rhs_parent,
            bool noprune) {
  premul(lhs, ilhs, &(rhs_parent->second), noprune);
}

void RobotTreeP::transport_root_link_frame(
    const Pose &pose, const std::function<bool(ptree::const_iterator)> &noprune) {
  Pose ipose = pose.inverse();
  // relocate the joints origins
  for(ptree::iterator child_joint_it :
      boost::make_iterator_range(
        joints.get<ParentLink>().equal_range(root_link))) {
    premul(ipose, pose, child_joint_it, noprune(child_joint_it));
  }
  auto root_link_it = links.at(root_link);
  for (const std::string &key : {"inertial", "visual", "collision"}) {
    auto range = root_link_it->second.equal_range(key);
    for (auto it = range.first; it != range.second; ++it) {
      premul(ipose, pose, it, false);
    }
  }
}

struct flip_joint_links_fctor {
  void operator()(ptree::iterator joint_it) {
    std::string parent_link = ::parent_link_it(joint_it);
    std::string child_link = ::child_link_it(joint_it);
    joint_it->second.put<std::string>("parent.<xmlattr>.link", child_link);
    joint_it->second.put<std::string>("child.<xmlattr>.link", parent_link);
  }
};

// If the joint type has an axis, flip it
// (ie. make it go in the opposite direction), so that the joint
// direction is inversed.
void flip_joint_axis(ptree &joint_pt) {
  Joint joint(joint_pt);
  switch (joint.type()) {
    case Joint::Type::revolute:
    case Joint::Type::continuous:
    case Joint::Type::prismatic:
    case Joint::Type::planar: {
      Array3d old_axis = joint.axis();
      Array3d new_axis{{-old_axis[0], -old_axis[1], -old_axis[2]}};
      if (new_axis == Array3d{{1, 0, 0}})
        joint_pt.erase("axis");
      else
        joint_pt.put("axis.<xmlattr>.xyz", new_axis, Array3dTranslator());
      break;
    }
    case Joint::Type::fixed:
    case Joint::Type::floating:
      break;
  }
}

struct joint_has_name_fctor {
  const std::string &name;

  joint_has_name_fctor(const std::string &name) : name(name) {}

  bool operator()(ptree::const_iterator joint_it) {
    return name == ::name_it(joint_it);
  }
};

// flip joint named "joint_name"
// The joint must be a child of the root link. As a consequence, flipping it
// will change the kinematic tree root.
//
// We will
// * move the parent link frame
// * inverse the joint's parent and child links
// * inverse the joint axis
//
// This won't change the physical meaning of the kinematic system being
// described.
void RobotTreeP::flip_root_link_joint(const std::string &joint_name,
                                      bool add_prune_exception) {
  auto joint_it2 = checked_find(joints.get<Name>(), joint_name);
  ptree::iterator joint_it = *joint_it2;
  Joint joint(joint_it->second);
  assert(joint.parent_link() == root_link);
  // The root link frame can be defined anywhere. But this is not the
  // case for other links, whose frame is defined by their parent joint,
  // and must have its origin on the parent joint axis.
  //
  // Since the current root link won't be a root link anymore, we need to
  // move its frame on its future parent joint axis.
  // So, let move the the root link frame so that is is identical to the
  // future parent joint when this joint is at the zero pose.
  // Note: maybe we should instead do a pure translation?
  boost::optional<const ptree &> opt_origin =
      const_cast<const ptree &>(joint_it->second).get_child_optional("origin");
  if (opt_origin) {
    Pose origin = Pose::from_ptree(opt_origin);
    if (add_prune_exception) {
      RobotTreeP::transport_root_link_frame(origin,
                                           joint_has_name_fctor(joint_name));
    } else {
      RobotTreeP::transport_root_link_frame(origin);
    }
  }
  // update kinematic tree
  bool ok = joints.get<Name>().modify(joint_it2, flip_joint_links_fctor());
  assert(ok);
  root_link = joint.parent_link();
  flip_joint_axis(joint_it->second);
}

void RobotTreeP::define_as_root_link(const std::string &link_name) {
  std::vector<std::string> joints_path;
  std::string cur_link_name = link_name;
  while (cur_link_name != root_link) {
    Joint joint((*checked_find(joints.get<ChildLink>(), cur_link_name))->second);
    if ((joint.type() == Joint::floating) || (joint.type() == Joint::planar))
      throw std::runtime_error(
          "define_as_root_link: there is a multi-dof"
          " joint between the old and the new root");
    joints_path.push_back(joint.name());

    cur_link_name = joint.parent_link();
  }
  for (auto it = joints_path.rbegin(); it != joints_path.rend(); ++it) {
    bool add_prune_exception = it + 1 != joints_path.rend();
    flip_root_link_joint(*it, add_prune_exception);
  }
}

void RobotTreeP::walk_joint(RobotTree::JointConstVisitor &vis,
                            const std::string &link) const {
  BOOST_FOREACH (ptree::const_iterator joint,
                 joints.get<ParentLink>().equal_range(link)) {
    if (vis.discover(joint->second))
      walk_joint(vis, ::child_link_it(joint));
    vis.finish(joint->second);
  }
}

void RobotTreeP::walk_joint(RobotTree::JointVisitor &vis,
                            const std::string &link) {
  BOOST_FOREACH (ptree::iterator joint,
                 joints.get<ParentLink>().equal_range(link)) {
    if (vis.discover(joint->second))
      walk_joint(vis, ::child_link_it(joint));
    vis.finish(joint->second);
  }
}

bool RobotTreeP::is_mimic_tree_flat() const {
  BOOST_FOREACH (auto joint_it, joints.get<Name>()) {
    if (auto opt_mimic = ::mimic_it(joint_it)) {
      auto it = joints.get<Name>().find(*opt_mimic);
      assert(it != joints.get<Name>().end());
      if (::mimic_it(*it)) {
        // the mimic source is itself a mimic target, we should flatten the
        // tree.
        return false;
      }
    }
  }
  return true;
}
}  // end of namespace AL::urdf::detail

RobotTree::RobotTree(ptree &robot) : _p(new detail::RobotTreeP(robot)) {}
RobotTree::~RobotTree() {}

const ptree &RobotTree::link(const std::string &name) const {
  return _p->links.at(name)->second;
}
ptree &RobotTree::link(const std::string &name) {
  return _p->links.at(name)->second;
}

const ptree &RobotTree::joint(const std::string &name) const {
  auto it2 = checked_find(_p->joints.get<detail::RobotTreeP::Name>(), name);
  return (**it2).second;
}
ptree &RobotTree::joint(const std::string &name) {
  auto it2 = checked_find(_p->joints.get<detail::RobotTreeP::Name>(), name);
  return (**it2).second;
}

const std::string &RobotTree::root_link() const { return _p->root_link; }

void RobotTree::rm_root_joint() { _p->rm_root_joint(); }

void RobotTree::rm_leaf_joint(const std::string &name) { _p->rm_leaf_joint(name); }

void RobotTree::transport_root_link_frame(const Pose &pose) {
  _p->transport_root_link_frame(pose);
}

void RobotTree::define_as_root_link(const std::string &name) {
  _p->define_as_root_link(name);
}

void RobotTree::traverse_joints(JointConstVisitor &vis) const {
  _p->walk_joint(vis, _p->root_link);
}

void RobotTree::traverse_joints(JointVisitor &vis) {
  _p->walk_joint(vis, _p->root_link);
}

bool RobotTree::is_mimic_tree_flat() const { return _p->is_mimic_tree_flat(); }
UrdfDotPrinterVisitor::UrdfDotPrinterVisitor(std::ostream &os)
    : tab('\t'), depth(1), do_indent(false), os(os) {}

bool UrdfDotPrinterVisitor::discover(const ptree &joint_pt) {
  Joint joint(joint_pt);
  os << (do_indent ? std::string(depth, tab) : "") << joint.parent_link()
     << " -> " << joint.child_link() << " [label=" << joint.name() << "];\n";
  ++depth;
  return true;
}

void UrdfDotPrinterVisitor::finish(const ptree &joint) { --depth; }

// internal helper
void makeJointFixed(ptree &joint_pt) {
  joint_pt.put("<xmlattr>.type", "fixed");
  joint_pt.erase("axis");
}

void makeJointFixed(RobotTree &parser, const std::string &name) {
  makeJointFixed(parser.joint(name));
}

void makeJointFloating(RobotTree &parser, const std::string &name) {
  ptree &joint = parser.joint(name);
  joint.put("<xmlattr>.type", "floating");
  joint.erase("axis");
}

// internal helper class
class MakeContinuousJointsFixedVisitor : public JointVisitor {
 public:
  MakeContinuousJointsFixedVisitor() {}
  std::vector<std::string> names;
  bool discover(ptree &joint_pt) {
    if (Joint(joint_pt).type() == Joint::continuous) {
      makeJointFixed(joint_pt);
      names.push_back(Joint(joint_pt).name());
    }
    return true;
  }
};

std::vector<std::string> makeContinuousJointsFixed(RobotTree &parser) {
  MakeContinuousJointsFixedVisitor vis;
  parser.traverse_joints(vis);
  return vis.names;
}

// internal helper overload
void squashJointMass(RobotTree &parser, ptree &joint_pt) {
  typedef Eigen::Matrix3d Matrix3;
  typedef Eigen::Vector3d Vector3;
  typedef Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>
      EPose;

  Joint joint(joint_pt);
  ptree &child_pt = parser.link(joint.child_link());
  Link child(child_pt);
  boost::optional<Inertial> child_inertial = child.inertial();
  if (!child_inertial) return;  // nothing to squash

  // pose ot the child inertial in the parent frame.
  const EPose child_inertial_pose =
      Math::toEigenTransform(joint.origin()) *
      Math::toEigenTransform(child_inertial->origin());

  EPose new_inertial_pose;
  ptree new_inertial;

  ptree &parent_pt = parser.link(joint.parent_link());
  Link parent(parent_pt);
  boost::optional<Inertial> parent_inertial = parent.inertial();
  if (!parent_inertial) {
    // we can copy the child inertial as-is, except for the pose which
    // needs to get updated.
    new_inertial = child_inertial->pt;
    new_inertial_pose = child_inertial_pose;
  } else {
    const EPose parent_inertial_pose =
        Math::toEigenTransform(parent_inertial->origin());
    double new_mass;
    Matrix3 new_inertia;

    Math::squashInertial(parent_inertial->mass(), parent_inertial_pose,
                         Math::toEigenMatrix3(*parent_inertial),
                         child_inertial->mass(), child_inertial_pose,
                         Math::toEigenMatrix3(*child_inertial), new_mass,
                         new_inertial_pose, new_inertia);
    new_inertial.put("mass.<xmlattr>.value", new_mass);
    new_inertial.put("inertia.<xmlattr>.ixx", new_inertia(0, 0));
    new_inertial.put("inertia.<xmlattr>.ixy", new_inertia(0, 1));
    new_inertial.put("inertia.<xmlattr>.ixz", new_inertia(0, 2));
    new_inertial.put("inertia.<xmlattr>.iyy", new_inertia(1, 1));
    new_inertial.put("inertia.<xmlattr>.iyz", new_inertia(1, 2));
    new_inertial.put("inertia.<xmlattr>.izz", new_inertia(2, 2));
  }
  if (new_inertial_pose.translation() != Vector3::Zero()) {
    Array3d xyz = {{new_inertial_pose.translation()[0],
                    new_inertial_pose.translation()[1],
                    new_inertial_pose.translation()[2]}};
    new_inertial.put("origin.<xmlattr>.xyz", xyz, urdf::Array3dTranslator());
  }
  if (new_inertial_pose.linear() != Matrix3::Identity()) {
    new_inertial.put("origin.<xmlattr>.rpy",
                     Math::urdfRpyFromEigenMatrix3(new_inertial_pose.linear()),
                     urdf::Array3dTranslator());
  }
  parent_pt.put_child("inertial", new_inertial);
  child_pt.erase("inertial");
}

void squashJointMass(RobotTree &parser, const std::string &name) {
  squashJointMass(parser, parser.joint(name));
}

// internal helper class
class SquashFixedJointsMassVisitor : public JointVisitor {
 public:
  SquashFixedJointsMassVisitor(RobotTree &parser) : parser(parser) {}
  void finish(ptree &joint_pt) {
    if (Joint(joint_pt).type() != Joint::fixed) return;
    squashJointMass(parser, joint_pt);
  }

 private:
  RobotTree &parser;
};

void squashFixedJointsMass(RobotTree &parser) {
  urdf::SquashFixedJointsMassVisitor vis(parser);
  parser.traverse_joints(vis);
}

// internal helper class
class MakeMasslessJointsFixedVisitor : public JointVisitor {
 public:
  MakeMasslessJointsFixedVisitor(RobotTree &parser) : parser(parser) {}
  std::vector<std::string> names;
  void finish(ptree &joint_pt) {
    Joint joint(joint_pt);
    if (joint.type() == Joint::fixed) {
      squashJointMass(parser, joint_pt);
    } else if (!Link(parser.link(joint.child_link())).inertial()) {
      makeJointFixed(joint_pt);
      names.push_back(joint.name());
      // note: no need to call squashJointMass, we know the child link has no
      // mass
    }
  }

 private:
  RobotTree &parser;
};

std::vector<std::string> makeMasslessJointsFixed(RobotTree &parser) {
  urdf::MakeMasslessJointsFixedVisitor vis(parser);
  parser.traverse_joints(vis);
  return vis.names;
}

// internal helper class
class ListJointsToRemoveVisitor : public JointConstVisitor {
 private:
  using Pred = std::function<bool(const ptree &joint)>;
  Pred pred;
  int level = 0; // prune if level >0
 public:
  ListJointsToRemoveVisitor(Pred pred)
    : pred(pred) {}
  std::vector<std::string> names; // names of joints to remove

  bool discover(const ptree &joint_pt) {
    if (pred(joint_pt)) {
      level += 1;
    }
    auto n = name(joint_pt);
    if (level > 0)
      names.push_back(n);
    return true;
  }

  void finish(const ptree &joint_pt) {
    if (pred(joint_pt))
      level -= 1;
    assert(level >= 0);
  }
};

std::vector<std::string> removeSubTreeIfJoint(
    RobotTree &parser,
    std::function<bool(const ptree &joint)> pred) {
  ListJointsToRemoveVisitor vis(std::move(pred));
  parser.traverse_joints(vis);
  // we iterate over the joint in reverse order to remove the leaves first.
  for (auto it = vis.names.rbegin(); it != vis.names.rend(); ++it) {
    parser.rm_leaf_joint(*it);
  }
  return std::move(vis.names);
}

void put_name(ptree &pt, const std::string &name) {
  pt.put("<xmlattr>.name", name);
}

namespace robot {
void transform_filenames(
    ptree &robot, std::function<std::string(std::string)> op) {
  // parent can be a texture or a mesh element
  auto apply_op =
      [&op](ptree &parent) -> void {
    // Note: according to the urdf spec, the filename is a mandatory attribute
    // of the mesh and texture elements.
    // However, the official urdf parser from ROS produces texture elements
    // without the filename attribute.
    // As a consequence, we need to be tolerant.
    auto opt = parent.get_child_optional("<xmlattr>.filename");
    if (opt) {
      ptree &f = *opt;
      f.put_value(op(f.get_value<std::string>()));
    }
  };
  {
    auto materials = robot.equal_range("material");
    for (ptree &material: boost::adaptors::values(materials)) {
      boost::optional<ptree &> texture = material.get_child_optional("texture");
      if (texture)
        apply_op(*texture);
    }
  }
  auto links = robot.equal_range("link");
  for (ptree &link: boost::adaptors::values(links)) {
    auto visuals = link.equal_range("visual");
    for (ptree &visual: boost::adaptors::values(visuals)) {
      ptree &geometry = visual.get_child("geometry");
      auto meshes = geometry.equal_range("mesh");
      for (ptree &mesh: boost::adaptors::values(meshes)) {
        apply_op(mesh);
      }
      auto materials = visual.equal_range("material");
      for (ptree &material: boost::adaptors::values(materials)) {
        boost::optional<ptree &> texture = material.get_child_optional("texture");
        if (texture)
          apply_op(*texture);
      }
    }
  }
}
}
}
}

max_stars_repo_name: UCCS-Social-Robotics/libalmath

max_issues_repo_name: UCCS-Social-Robotics/libalmath

max_forks_repo_name: UCCS-Social-Robotics/libalmath


text:
///////////////////////////////////////////////////////////////////////////////
/// \file regex_primitives.hpp
/// Contains the syntax elements for writing static regular expressions.
//
//  Copyright 2004 Eric Niebler. 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_XPRESSIVE_REGEX_PRIMITIVES_HPP_EAN_10_04_2005
#define BOOST_XPRESSIVE_REGEX_PRIMITIVES_HPP_EAN_10_04_2005

#include <climits>
#include <boost/mpl/assert.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/xpressive/proto/proto.hpp>
#include <boost/xpressive/detail/detail_fwd.hpp>
#include <boost/xpressive/detail/core/icase.hpp>
#include <boost/xpressive/detail/core/action.hpp>
#include <boost/xpressive/detail/core/matchers.hpp>
#include <boost/xpressive/detail/static/as_xpr.hpp>
#include <boost/xpressive/detail/static/compile.hpp>
#include <boost/xpressive/detail/static/modifier.hpp>
#include <boost/xpressive/detail/static/regex_operators.hpp>
#include <boost/xpressive/detail/static/productions/productions.hpp>

namespace boost { namespace xpressive { namespace detail
{

typedef assert_word_placeholder<word_boundary<true> > assert_word_boundary;
typedef assert_word_placeholder<word_begin> assert_word_begin;
typedef assert_word_placeholder<word_end> assert_word_end;

/*
///////////////////////////////////////////////////////////////////////////////
/// INTERNAL ONLY
// BOOST_XPRESSIVE_GLOBAL
//  for defining globals that neither violate the One Definition Rule nor
//  lead to undefined behavior due to global object initialization order.
//#define BOOST_XPRESSIVE_GLOBAL(type, name, init)                                        \
//    namespace detail                                                                    \
//    {                                                                                \
//        template<int Dummy>                                                             \
//        struct BOOST_PP_CAT(global_pod_, name)                                          \
//        {                                                                               \
//            static type const value;                                                    \
//        private:                                                                        \
//            union type_must_be_pod                                                      \
//            {                                                                           \
//                type t;                                                                 \
//                char ch;                                                                \
//            } u;                                                                        \
//        };                                                                              \
//        template<int Dummy>                                                             \
//        type const BOOST_PP_CAT(global_pod_, name)<Dummy>::value = init;                \
//    }                                                                                \
//    type const &name = detail::BOOST_PP_CAT(global_pod_, name)<0>::value
*/

} // namespace detail

/// INTERNAL ONLY (for backwards compatibility)
unsigned int const repeat_max = UINT_MAX-1;

///////////////////////////////////////////////////////////////////////////////
/// \brief For infinite repetition of a sub-expression.
///
/// Magic value used with the repeat\<\>() function template
/// to specify an unbounded repeat. Use as: repeat<17, inf>('a').
/// The equivalent in perl is /a{17,}/.
unsigned int const inf = UINT_MAX-1;

/// INTERNAL ONLY (for backwards compatibility)
proto::op_proxy<
    proto::unary_op<detail::epsilon_matcher, proto::noop_tag>
> const epsilon = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Successfully matches nothing.
///
/// Successfully matches a zero-width sequence. nil always succeeds and
/// never consumes any characters.
proto::op_proxy<
    proto::unary_op<detail::epsilon_matcher, proto::noop_tag>
> const nil = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches an alpha-numeric character.
///
/// The regex traits are used to determine which characters are alpha-numeric.
/// To match any character that is not alpha-numeric, use ~alnum.
///
/// \attention alnum is equivalent to /[[:alnum:]]/ in perl. ~alnum is equivalent
/// to /[[:^alnum:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const alnum = {"alnum"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches an alphabetic character.
///
/// The regex traits are used to determine which characters are alphabetic.
/// To match any character that is not alphabetic, use ~alpha.
///
/// \attention alpha is equivalent to /[[:alpha:]]/ in perl. ~alpha is equivalent
/// to /[[:^alpha:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const alpha = {"alpha"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a blank (horizonal white-space) character.
///
/// The regex traits are used to determine which characters are blank characters.
/// To match any character that is not blank, use ~blank.
///
/// \attention blank is equivalent to /[[:blank:]]/ in perl. ~blank is equivalent
/// to /[[:^blank:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const blank = {"blank"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a control character.
///
/// The regex traits are used to determine which characters are control characters.
/// To match any character that is not a control character, use ~cntrl.
///
/// \attention cntrl is equivalent to /[[:cntrl:]]/ in perl. ~cntrl is equivalent
/// to /[[:^cntrl:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const cntrl = {"cntrl"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a digit character.
///
/// The regex traits are used to determine which characters are digits.
/// To match any character that is not a digit, use ~digit.
///
/// \attention digit is equivalent to /[[:digit:]]/ in perl. ~digit is equivalent
/// to /[[:^digit:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const digit = {"digit"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a graph character.
///
/// The regex traits are used to determine which characters are graphable.
/// To match any character that is not graphable, use ~graph.
///
/// \attention graph is equivalent to /[[:graph:]]/ in perl. ~graph is equivalent
/// to /[[:^graph:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const graph = {"graph"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a lower-case character.
///
/// The regex traits are used to determine which characters are lower-case.
/// To match any character that is not a lower-case character, use ~lower.
///
/// \attention lower is equivalent to /[[:lower:]]/ in perl. ~lower is equivalent
/// to /[[:^lower:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const lower = {"lower"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a printable character.
///
/// The regex traits are used to determine which characters are printable.
/// To match any character that is not printable, use ~print.
///
/// \attention print is equivalent to /[[:print:]]/ in perl. ~print is equivalent
/// to /[[:^print:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const print = {"print"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a punctuation character.
///
/// The regex traits are used to determine which characters are punctuation.
/// To match any character that is not punctuation, use ~punct.
///
/// \attention punct is equivalent to /[[:punct:]]/ in perl. ~punct is equivalent
/// to /[[:^punct:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const punct = {"punct"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a space character.
///
/// The regex traits are used to determine which characters are space characters.
/// To match any character that is not white-space, use ~space.
///
/// \attention space is equivalent to /[[:space:]]/ in perl. ~space is equivalent
/// to /[[:^space:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const space = {"space"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches an upper-case character.
///
/// The regex traits are used to determine which characters are upper-case.
/// To match any character that is not upper-case, use ~upper.
///
/// \attention upper is equivalent to /[[:upper:]]/ in perl. ~upper is equivalent
/// to /[[:^upper:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const upper = {"upper"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a hexadecimal digit character.
///
/// The regex traits are used to determine which characters are hex digits.
/// To match any character that is not a hex digit, use ~xdigit.
///
/// \attention xdigit is equivalent to /[[:xdigit:]]/ in perl. ~xdigit is equivalent
/// to /[[:^xdigit:]]/ in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const xdigit = {"xdigit"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Beginning of sequence assertion.
///
/// For the character sequence [begin, end), 'bos' matches the
/// zero-width sub-sequence [begin, begin).
proto::op_proxy<
    proto::unary_op<detail::assert_bos_matcher, proto::noop_tag>
> const bos = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief End of sequence assertion.
///
/// For the character sequence [begin, end),
/// 'eos' matches the zero-width sub-sequence [end, end).
///
/// \attention Unlike the perl end of sequence assertion \$, 'eos' will
/// not match at the position [end-1, end-1) if *(end-1) is '\\n'. To
/// get that behavior, use (!_n >> eos).
proto::op_proxy<
    proto::unary_op<detail::assert_eos_matcher, proto::noop_tag>
> const eos = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Beginning of line assertion.
///
/// 'bol' matches the zero-width sub-sequence
/// immediately following a logical newline sequence. The regex traits
/// is used to determine what constitutes a logical newline sequence.
proto::op_proxy<
    proto::unary_op<detail::assert_bol_placeholder, proto::noop_tag>
> const bol = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief End of line assertion.
///
/// 'eol' matches the zero-width sub-sequence
/// immediately preceeding a logical newline sequence. The regex traits
/// is used to determine what constitutes a logical newline sequence.
proto::op_proxy<
    proto::unary_op<detail::assert_eol_placeholder, proto::noop_tag>
> const eol = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Beginning of word assertion.
///
/// 'bow' matches the zero-width sub-sequence
/// immediately following a non-word character and preceeding a word character.
/// The regex traits are used to determine what constitutes a word character.
proto::op_proxy<
    proto::unary_op<detail::assert_word_begin, proto::noop_tag>
> const bow = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief End of word assertion.
///
/// 'eow' matches the zero-width sub-sequence
/// immediately following a word character and preceeding a non-word character.
/// The regex traits are used to determine what constitutes a word character.
proto::op_proxy<
    proto::unary_op<detail::assert_word_end, proto::noop_tag>
> const eow = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Word boundary assertion.
///
/// '_b' matches the zero-width sub-sequence at the beginning or the end of a word.
/// It is equivalent to (bow | eow). The regex traits are used to determine what
/// constitutes a word character. To match a non-word boundary, use ~_b.
///
/// \attention _b is like \\b in perl. ~_b is like \\B in perl.
proto::op_proxy<
    proto::unary_op<detail::assert_word_boundary, proto::noop_tag>
> const _b = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a word character.
///
/// '_w' matches a single word character. The regex traits are used to determine which
/// characters are word characters. Use ~_w to match a character that is not a word
/// character.
///
/// \attention _w is like \\w in perl. ~_w is like \\W in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const _w = {"w"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a digit character.
///
/// '_d' matches a single digit character. The regex traits are used to determine which
/// characters are digits. Use ~_d to match a character that is not a digit
/// character.
///
/// \attention _d is like \\d in perl. ~_d is like \\D in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const _d = {"d"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a space character.
///
/// '_s' matches a single space character. The regex traits are used to determine which
/// characters are space characters. Use ~_s to match a character that is not a space
/// character.
///
/// \attention _s is like \\s in perl. ~_s is like \\S in perl.
proto::op_proxy<
    proto::unary_op<detail::posix_charset_placeholder, proto::noop_tag>
  , char const *
> const _s = {"s"};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a literal newline character, '\\n'.
///
/// '_n' matches a single newline character, '\\n'. Use ~_n to match a character
/// that is not a newline.
///
/// \attention ~_n is like '.' in perl without the /s modifier.
proto::op_proxy<
    proto::unary_op<detail::literal_placeholder<char>, proto::noop_tag>
  , char
> const _n = {'\n'};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches a logical newline sequence.
///
/// '_ln' matches a logical newline sequence. This can be any character in the
/// line separator class, as determined by the regex traits, or the '\\r\\n' sequence.
/// For the purpose of back-tracking, '\\r\\n' is treated as a unit.
/// To match any one character that is not a logical newline, use ~_ln.
proto::op_proxy<
    detail::logical_newline_xpression
> const _ln = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Matches any one character.
///
/// Match any character, similar to '.' in perl syntax with the /s modifier.
/// '_' matches any one character, including the newline.
///
/// \attention To match any character except the newline, use ~_n
proto::op_proxy<
    proto::unary_op<detail::any_matcher, proto::noop_tag>
> const _ = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Reference to the current regex object
///
/// Useful when constructing recursive regular expression objects. The 'self'
/// identifier is a short-hand for the current regex object. For instance,
/// sregex rx = '(' >> (self | nil) >> ')'; will create a regex object that
/// matches balanced parens such as "((()))".
proto::op_proxy<
    proto::unary_op<detail::self_placeholder, proto::noop_tag>
> const self = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Used to create character sets.
///
/// There are two ways to create character sets with the 'set' identifier. The
/// easiest is to create a comma-separated list of the characters in the set,
/// as in (set= 'a','b','c'). This set will match 'a', 'b', or 'c'. The other
/// way is to define the set as an argument to the set subscript operator.
/// For instance, set[ 'a' | range('b','c') | digit ] will match an 'a', 'b',
/// 'c' or a digit character.
///
/// To complement a set, apply the '~' operator. For instance, ~(set= 'a','b','c')
/// will match any character that is not an 'a', 'b', or 'c'.
///
/// Sets can be composed of other, possibly complemented, sets. For instance,
/// set[ ~digit | ~(set= 'a','b','c') ].
proto::op_proxy<
    detail::set_initializer_type
> const set = {};

///////////////////////////////////////////////////////////////////////////////
/// \brief Sub-match placeholder, like $& in Perl
proto::op_proxy<detail::mark_tag, int> const s0 = {0};

///////////////////////////////////////////////////////////////////////////////
/// \brief Sub-match placeholder, like $1 in perl.
///
/// To create a sub-match, assign a sub-expression to the sub-match placeholder.
/// For instance, (s1= _) will match any one character and remember which
/// character was matched in the 1st sub-match. Later in the pattern, you can
/// refer back to the sub-match. For instance,  (s1= _) >> s1  will match any
/// character, and then match the same character again.
///
/// After a successful regex_match() or regex_search(), the sub-match placeholders
/// can be used to index into the match_results\<\> object to retrieve the Nth
/// sub-match.
proto::op_proxy<detail::mark_tag, int> const s1 = {1};
proto::op_proxy<detail::mark_tag, int> const s2 = {2};
proto::op_proxy<detail::mark_tag, int> const s3 = {3};
proto::op_proxy<detail::mark_tag, int> const s4 = {4};
proto::op_proxy<detail::mark_tag, int> const s5 = {5};
proto::op_proxy<detail::mark_tag, int> const s6 = {6};
proto::op_proxy<detail::mark_tag, int> const s7 = {7};
proto::op_proxy<detail::mark_tag, int> const s8 = {8};
proto::op_proxy<detail::mark_tag, int> const s9 = {9};

// NOTE: For the purpose of xpressive's documentation, make icase() look like an
// ordinary function. In reality, it is a function object defined in detail/icase.hpp
// so that it can serve double-duty as regex_constants::icase, the syntax_option_type.
// Do the same for as_xpr(), which is actually defined in detail/static/as_xpr.hpp
#ifdef BOOST_XPRESSIVE_DOXYGEN_INVOKED
///////////////////////////////////////////////////////////////////////////////
/// \brief Makes a literal into a regular expression.
///
/// Use as_xpr() to turn a literal into a regular expression. For instance,
/// "foo" >> "bar" will not compile because both operands to the right-shift
/// operator are const char*, and no such operator exists. Use as_xpr("foo") >> "bar"
/// instead.
///
/// You can use as_xpr() with character literals in addition to string literals.
/// For instance, as_xpr('a') will match an 'a'. You can also complement a
/// character literal, as with ~as_xpr('a'). This will match any one character
/// that is not an 'a'.
template<typename Literal>
inline typename detail::as_xpr_type<Literal>::const_reference
as_xpr(Literal const &literal)
{
    return detail::as_xpr_type<Literal>::call(xpr);
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Makes a sub-expression case-insensitive.
///
/// Use icase() to make a sub-expression case-insensitive. For instance,
/// "foo" >> icase(set['b'] >> "ar") will match "foo" exactly followed by
/// "bar" irrespective of case.
template<typename Xpr>
inline proto::binary_op<detail::icase_modifier, typename detail::as_xpr_type<Xpr>::type, modifier_tag> const
icase(Xpr const &xpr)
{
    detail::icase_modifier mod;
    return proto::make_op<modifier_tag>(mod, as_xpr(xpr));
}
#endif

///////////////////////////////////////////////////////////////////////////////
/// \brief Embed a regex object by reference.
///
/// \param rex The basic_regex object to embed by reference.
template<typename BidiIter>
inline proto::unary_op<detail::regex_placeholder<BidiIter, true>, proto::noop_tag> const
by_ref(basic_regex<BidiIter> const &rex)
{
    typedef detail::core_access<BidiIter> access;
    shared_ptr<detail::regex_impl<BidiIter> > impl = access::get_regex_impl(rex);
    return proto::noop(detail::regex_placeholder<BidiIter, true>(impl));
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Match a range of characters.
///
/// Match any character in the range [ch_min, ch_max].
///
/// \param ch_min The lower end of the range to match.
/// \param ch_max The upper end of the range to match.
template<typename Char>
inline proto::unary_op<detail::range_placeholder<Char>, proto::noop_tag> const
range(Char ch_min, Char ch_max)
{
    return proto::noop(detail::range_placeholder<Char>(ch_min, ch_max));
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Make a sub-expression optional. Equivalent to !as_xpr(xpr).
///
/// \param xpr The sub-expression to make optional.
template<typename Xpr>
inline proto::unary_op
<
    typename detail::as_xpr_type<Xpr>::type
  , proto::logical_not_tag
> const
optional(Xpr const &xpr)
{
    return !as_xpr(xpr);
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Repeat a sub-expression multiple times.
///
/// There are two forms of the repeat\<\>() function template. To match a
/// sub-expression N times, use repeat\<N\>(xpr). To match a sub-expression
/// from M to N times, use repeat\<M,N\>(xpr).
///
/// The repeat\<\>() function creates a greedy quantifier. To make the quantifier
/// non-greedy, apply the unary minus operator, as in -repeat\<M,N\>(xpr).
///
/// \param xpr The sub-expression to repeat.
template<unsigned int Min, unsigned int Max, typename Xpr>
inline proto::unary_op
<
    typename detail::as_xpr_type<Xpr>::type
  , detail::generic_quant_tag<Min, Max>
> const
repeat(Xpr const &xpr)
{
    return proto::make_op<detail::generic_quant_tag<Min, Max> >(as_xpr(xpr));
}

/// \overload
template<unsigned int Count, typename Xpr2>
inline proto::unary_op
<
    typename detail::as_xpr_type<Xpr2>::type
  , detail::generic_quant_tag<Count, Count>
> const
repeat(Xpr2 const &xpr)
{
    return proto::make_op<detail::generic_quant_tag<Count, Count> >(as_xpr(xpr));
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Create an independent sub-expression.
///
/// Turn off back-tracking for a sub-expression. Any branches or repeats within
/// the sub-expression will match only one way, and no other alternatives are
/// tried.
///
/// \attention keep(xpr) is equivalent to the perl (?>...) extension.
///
/// \param xpr The sub-expression to modify.
template<typename Xpr>
inline proto::unary_op
<
    typename detail::as_xpr_type<Xpr>::type
  , detail::keeper_tag
> const
keep(Xpr const &xpr)
{
    return proto::make_op<detail::keeper_tag>(as_xpr(xpr));
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Look-ahead assertion.
///
/// before(xpr) succeeds if the xpr sub-expression would match at the current
/// position in the sequence, but xpr is not included in the match. For instance,
/// before("foo") succeeds if we are before a "foo". Look-ahead assertions can be
/// negated with the bit-compliment operator.
///
/// \attention before(xpr) is equivalent to the perl (?=...) extension.
/// ~before(xpr) is a negative look-ahead assertion, equivalent to the
/// perl (?!...) extension.
///
/// \param xpr The sub-expression to put in the look-ahead assertion.
template<typename Xpr>
inline proto::unary_op
<
    typename detail::as_xpr_type<Xpr>::type
  , detail::lookahead_tag<true>
> const
before(Xpr const &xpr)
{
    return proto::make_op<detail::lookahead_tag<true> >(as_xpr(xpr));
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Look-behind assertion.
///
/// after(xpr) succeeds if the xpr sub-expression would match at the current
/// position minus N in the sequence, where N is the width of xpr. xpr is not included in
/// the match. For instance,  after("foo") succeeds if we are after a "foo". Look-behind
/// assertions can be negated with the bit-complement operator.
///
/// \attention after(xpr) is equivalent to the perl (?<=...) extension.
/// ~after(xpr) is a negative look-behind assertion, equivalent to the
/// perl (?<!...) extension.
///
/// \param xpr The sub-expression to put in the look-ahead assertion.
///
/// \pre xpr cannot match a variable number of characters.
template<typename Xpr>
inline proto::unary_op
<
    typename detail::as_xpr_type<Xpr>::type
  , detail::lookbehind_tag<true>
> const
after(Xpr const &xpr)
{
    return proto::make_op<detail::lookbehind_tag<true> >(as_xpr(xpr));
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Specify a regex traits or a std::locale.
///
/// imbue() instructs the regex engine to use the specified traits or locale
/// when matching the regex. The entire expression must use the same traits/locale.
/// For instance, the following specifies a locale for use with a regex:
///   std::locale loc;
///   sregex rx = imbue(loc)(+digit);
///
/// \param loc The std::locale or regex traits object.
template<typename Locale>
inline detail::modifier_op<detail::locale_modifier<Locale> > const
imbue(Locale const &loc)
{
    detail::modifier_op<detail::locale_modifier<Locale> > mod =
    {
        detail::locale_modifier<Locale>(loc)
      , regex_constants::ECMAScript
    };
    return mod;
}

}} // namespace boost::xpressive

#endif

ext: hpp

max_stars_repo_name: dstrigl/mcotf

max_issues_repo_name: dstrigl/mcotf

max_forks_repo_name: dstrigl/mcotf