//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//               2014 Piotr Godlewski
//
// 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)
//=======================================================================
/**
 * @file generalised_assignment.hpp
 * @brief
 * @author Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-05-06
 */
#ifndef PAAL_GENERALISED_ASSIGNMENT_HPP
#define PAAL_GENERALISED_ASSIGNMENT_HPP

#include "paal/iterative_rounding/ir_components.hpp"
#include "paal/iterative_rounding/iterative_rounding.hpp"

#include <boost/range/adaptor/indexed.hpp>

namespace paal {
namespace ir {

/**
 * Relax Condition of the IR Generalised Assignment algorithm.
 */
struct ga_relax_condition {
    /**
     * Checks if a given row of the LP corresponds to a machine and can be
     * relaxed.
     */
    template <typename Problem, typename LP>
    bool operator()(Problem &problem, const LP &lp, lp::row_id row) {
        auto &&machine_rows = problem.get_machine_rows();
        if (machine_rows.find(row) == machine_rows.end()) {
            return false;
        }
        auto row_deg = lp.get_row_degree(row);
        return row_deg <= 1 || (row_deg == 2 && problem.get_compare().ge(
                                                    lp.get_row_sum(row), 1));
    }
};

/**
 * Set Solution component of the IR Generalised Assignment algorithm.
 */
struct ga_set_solution {
    /**
     * Creates the result assignment form the LP (all edges with value 1).
     */
    template <typename Problem, typename GetSolution>
    void operator()(Problem &problem, const GetSolution &solution) {
        auto jbegin = std::begin(problem.get_jobs());
        auto mbegin = std::begin(problem.get_machines());
        auto &col_idx = problem.get_col_idx();
        auto job_to_machine = problem.get_job_to_machines();

        for (auto idx : irange(col_idx.size())) {
            if (problem.get_compare().e(solution(col_idx[idx]), 1)) {
                *job_to_machine =
                    std::make_pair(*(jbegin + problem.get_j_idx(idx)),
                                   *(mbegin + problem.get_m_idx(idx)));
                ++job_to_machine;
            }
        }
    }
};

/**
 * Initialization of the IR Generalised Assignment algorithm.
 */
class ga_init {
  public:
    /**
     * Initializes the LP: variables for edges, constraints for jobs and
     * machines.
     */
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        lp.set_lp_name("generalized assignment problem");
        lp.set_optimization_type(lp::MINIMIZE);

        add_variables(problem, lp);
        add_constraints_for_jobs(problem, lp);
        add_constraints_for_machines(problem, lp);
    }

  private:
    /**
     * Adds a variable to the LP for each (machine, job) edge, unless the
     * job proceeding time is greater than machine available time. Binds the
     * LP columns to the (machine, job) pairs.
     */
    template <typename Problem, typename LP>
    void add_variables(Problem &problem, LP &lp) {
        auto &col_idx = problem.get_col_idx();
        col_idx.reserve(problem.get_machines_cnt() * problem.get_jobs_cnt());
        for (auto &&j : problem.get_jobs()) {
            for (auto &&m : problem.get_machines()) {
                if (problem.get_proceeding_time()(j, m) <=
                    problem.get_machine_available_time()(m)) {
                    col_idx.push_back(lp.add_column(problem.get_cost()(j, m)));
                } else {
                    col_idx.push_back(
                        lp.add_column(problem.get_cost()(j, m), 0, 0));
                }
            }
        }
    }

    // constraints for job
    template <typename Problem, typename LP>
    void add_constraints_for_jobs(Problem &problem, LP &lp) {
        auto &col_idx = problem.get_col_idx();
        for (auto j_idx : irange(problem.get_jobs_cnt())) {
            lp::linear_expression expr;
            for (auto m_idx : irange(problem.get_machines_cnt())) {
                expr += col_idx[problem.idx(j_idx, m_idx)];
            }
            lp.add_row(std::move(expr) == 1.0);
        }
    }

    // constraints for machines
    template <typename Problem, typename LP>
    void add_constraints_for_machines(Problem &problem, LP &lp) {
        auto &col_idx = problem.get_col_idx();
        for (auto m : problem.get_machines() | boost::adaptors::indexed()) {
            auto T = problem.get_machine_available_time()(m.value());
            lp::linear_expression expr;

            for (auto j : problem.get_jobs() | boost::adaptors::indexed()) {
                auto t = problem.get_proceeding_time()(j.value(), m.value());
                auto x = col_idx[problem.idx(j.index(), m.index())];
                expr += x * t;
            }
            auto row = lp.add_row(std::move(expr) <= T);
            problem.get_machine_rows().insert(row);
        }
    }
};

template <typename Init = ga_init,
          typename RoundCondition = default_round_condition,
          typename RelaxContition = ga_relax_condition,
          typename SetSolution = ga_set_solution>
using ga_ir_components =
    IRcomponents<Init, RoundCondition, RelaxContition, SetSolution>;

/**
 * @class generalised_assignment
 * @brief The class for solving the Generalised Assignment problem using
* Iterative Rounding.
 *
 * @tparam MachineIter
 * @tparam JobIter
 * @tparam Cost
 * @tparam ProceedingTime
 * @tparam MachineAvailableTime
 * @tparam JobsToMachinesOutputIterator
 */
template <typename MachineIter, typename JobIter, typename Cost,
          typename ProceedingTime, typename MachineAvailableTime,
          typename JobsToMachinesOutputIterator>
class generalised_assignment {
  public:
    using Job = typename std::iterator_traits<JobIter>::value_type;
    using Machine = typename std::iterator_traits<MachineIter>::value_type;

    using Compare = utils::compare<double>;
    using MachineRows = std::unordered_set<lp::row_id>;
    using ColIdx = std::vector<lp::col_id>;

    using ErrorMessage = boost::optional<std::string>;

    /**
     * Constructor.
     */
    generalised_assignment(MachineIter mbegin, MachineIter mend, JobIter jbegin,
                           JobIter jend, const Cost &c, const ProceedingTime &t,
                           const MachineAvailableTime &T,
                           JobsToMachinesOutputIterator job_to_machines)
        : m_m_cnt(std::distance(mbegin, mend)),
          m_j_cnt(std::distance(jbegin, jend)), m_jbegin(jbegin), m_jend(jend),
          m_mbegin(mbegin), m_mend(mend), m_c(c), m_t(t), m_T(T),
          m_job_to_machine(job_to_machines) {}

    /**
     * Checks if input is valid.
     */
    ErrorMessage check_input_validity() {
        return ErrorMessage{};
    }

    /**
     * Returns the index of the edge between a given job and a given machine.
     */
    std::size_t idx(std::size_t j_idx, std::size_t m_idx) { return j_idx * m_m_cnt + m_idx; }

    /**
     * Returns the index of a job given the index of the edge between the job
     * and a machine.
     */
    std::size_t get_j_idx(std::size_t idx) { return idx / m_m_cnt; }

    /**
     * Returns the index of a machine given the index of the edge between a job
     * and the machine.
     */
    std::size_t get_m_idx(std::size_t idx) { return idx % m_m_cnt; }

    /**
     * Returns the LP rows corresponding to the machines.
     */
    MachineRows &get_machine_rows() { return m_machine_rows; }

    /**
     * Returns the double comparison object.
     */
    Compare get_compare() { return m_compare; }

    /**
     * Returns the number of machines in the problem.
     */
    std::size_t get_machines_cnt() const { return m_m_cnt; }

    /**
     * Returns the number of jobs in the problem.
     */
    std::size_t get_jobs_cnt() const { return m_j_cnt; }

    /**
     * Returns the machines iterator range.
     */
    boost::iterator_range<MachineIter> get_machines() {
        return boost::make_iterator_range(m_mbegin, m_mend);
    }

    /**
     * Returns the jobs iterator range.
     */
    boost::iterator_range<JobIter> get_jobs() {
        return boost::make_iterator_range(m_jbegin, m_jend);
    }

    /**
     * Returns the vector of LP column IDs.
     */
    ColIdx &get_col_idx() { return m_col_idx; }

    /**
     * Returns the result output iterator.
     */
    JobsToMachinesOutputIterator get_job_to_machines() {
        return m_job_to_machine;
    }

    /**
     * Returns the proceeding time function (function from (job, machine)
     * pairs into the proceeding time of the job on the machine).
     */
    const ProceedingTime &get_proceeding_time() { return m_t; }

    /**
     * Returns the machine available time function (function returning
     * the time available on a given machine).
     */
    const MachineAvailableTime &get_machine_available_time() { return m_T; }

    /**
     * Returns the cost function (function from (job, machine)
     * pairs into the cost of executing the job on the machine).
     */
    const Cost &get_cost() const { return m_c; }

  private:

    const std::size_t m_m_cnt;
    const std::size_t m_j_cnt;
    JobIter m_jbegin;
    JobIter m_jend;
    MachineIter m_mbegin;
    MachineIter m_mend;
    const Cost &m_c;
    const ProceedingTime &m_t;
    const MachineAvailableTime &m_T;
    JobsToMachinesOutputIterator m_job_to_machine;
    const Compare m_compare;
    ColIdx m_col_idx;
    MachineRows m_machine_rows;
};

/**
 * @brief Creates a generalised_assignment object.
 *
 * @tparam MachineIter
 * @tparam JobIter
 * @tparam Cost
 * @tparam ProceedingTime
 * @tparam MachineAvailableTime
 * @tparam JobsToMachinesOutputIterator
 * @param mbegin begin machines iterator
 * @param mend end machines iterator
 * @param jbegin begin jobs iterator
 * @param jend end jobs iterator
 * @param c costs of assignments
 * @param t jobs proceeding times
 * @param T times available for the machines
 * @param jobs_to_machines found assignment
 *
 * @return generalised_assignment object
 */
template <typename MachineIter, typename JobIter, typename Cost,
          typename ProceedingTime, typename MachineAvailableTime,
          typename JobsToMachinesOutputIterator>
generalised_assignment<MachineIter, JobIter, Cost, ProceedingTime,
                       MachineAvailableTime, JobsToMachinesOutputIterator>
make_generalised_assignment(MachineIter mbegin, MachineIter mend,
                            JobIter jbegin, JobIter jend, const Cost &c,
                            const ProceedingTime &t,
                            const MachineAvailableTime &T,
                            JobsToMachinesOutputIterator jobs_to_machines) {
    return generalised_assignment<
        MachineIter, JobIter, Cost, ProceedingTime, MachineAvailableTime,
        JobsToMachinesOutputIterator>(mbegin, mend, jbegin, jend, c, t, T,
                                      jobs_to_machines);
}

/**
 * @brief Solves the Generalised Assignment problem using Iterative Rounding.
 *
 * @tparam MachineIter
 * @tparam JobIter
 * @tparam Cost
 * @tparam ProceedingTime
 * @tparam MachineAvailableTime
 * @tparam JobsToMachinesOutputIterator
 * @tparam Components
 * @tparam Visitor
 * @param mbegin begin machines iterator
 * @param mend end machines iterator
 * @param jbegin begin jobs iterator
 * @param jend end jobs iterator
 * @param c costs of assignments
 * @param t jobs proceeding times
 * @param T times available for the machines
 * @param jobs_to_machines found assignment
 * @param components IR components
 * @param visitor
 *
 * @return solution status
 */
template <typename MachineIter, typename JobIter, typename Cost,
          typename ProceedingTime, typename MachineAvailableTime,
          typename JobsToMachinesOutputIterator,
          typename Components = ga_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult generalised_assignment_iterative_rounding(
    MachineIter mbegin, MachineIter mend, JobIter jbegin, JobIter jend,
    const Cost &c, const ProceedingTime &t, const MachineAvailableTime &T,
    JobsToMachinesOutputIterator jobs_to_machines,
    Components components = Components(), Visitor visitor = Visitor()) {
    auto ga_solution = make_generalised_assignment(mbegin, mend, jbegin, jend,
                                                   c, t, T, jobs_to_machines);
    return solve_iterative_rounding(ga_solution, std::move(components),
                                    std::move(visitor));
}

} // ir
} // paal
#endif // PAAL_GENERALISED_ASSIGNMENT_HPP


#include <boost/geometry/geometry.hpp>
#include "boost_geom_wrapper.hpp"

namespace gpkg {
    namespace detail {
        template<typename Visitor>
        struct geometry_ptr_visitor : boost::static_visitor<typename Visitor::result_type> {
          typename Visitor::result_type null_result;

          geometry_ptr_visitor(typename Visitor::result_type null_result) : null_result(null_result) {
          }

          typename Visitor::result_type operator()(const boost::blank &value) const {
            return null_result;
          }

          template<typename Geometry>
          typename Visitor::result_type operator()(const Geometry *value) const {
            return value != nullptr ? Visitor()(*value) : null_result;
          }
        };

        struct is_valid : boost::static_visitor<int> {
          template<typename Geometry>
          int operator()(const Geometry &geom) const {
            return boost::geometry::is_valid(geom);
          }

          int operator()(const gpkg::GeometryCollection &geom) const {
            for (auto it = geom.cbegin(); it < geom.cend(); ++it) {
              if (!boost::apply_visitor(*this, *it)) {
                return false;
              }
            }
            return true;
          }
        };

        struct is_simple : boost::static_visitor<int> {
          template<typename Geometry>
          int operator()(const Geometry &geom) const {
            return boost::geometry::is_simple(geom);
          }

          int operator()(const gpkg::GeometryCollection &geom) const {
            for (auto it = geom.cbegin(); it < geom.cend(); ++it) {
              if (!boost::apply_visitor(*this, *it)) {
                return false;
              }
            }
            return true;
          }
        };

        struct area : boost::static_visitor<double> {
          template<typename Geometry>
          double operator()(const Geometry &geom) const {
            return boost::geometry::area(geom);
          }

          double operator()(const gpkg::GeometryCollection &geom) const {
            double total = 0.0;

            for (auto it = geom.cbegin(); it < geom.cend(); ++it) {
              total += boost::apply_visitor(*this, *it);
            }

            return total;
          }
        };
    }

    int is_valid(GeometryPtr& geometry) {
      return boost::apply_visitor(detail::geometry_ptr_visitor<detail::is_valid>(true), geometry);
    }

    int is_simple(GeometryPtr& geometry) {
      return boost::apply_visitor(detail::geometry_ptr_visitor<detail::is_simple>(true), geometry);
    }

    double area(GeometryPtr& geometry) {
      return boost::apply_visitor(detail::geometry_ptr_visitor<detail::area>(0.0), geometry);
    }
}


#include <bits/stdc++.h>
#include <boost/dynamic_bitset.hpp>
#include "aho_corasick.hpp"

namespace ac = aho_corasick;
using utrie = ac::utrie;
using namespace std;

int main(void)
{
    basic_string<uint64_t> all = {0,1,2,3,0,1,2,0,1,0};
    vector<basic_string<uint64_t>> tests = {{0,1,2,3},{0,1,2},{0,1},{0},{3},{4},{0}};
    vector<uint64_t> test_counts;
    utrie t;
    for(auto & pattern: tests)
    {
        t.insert(pattern);
        test_counts.push_back(0);
    }
    auto matches = t.parse_text(all);
    cout<<matches.size()<<endl;
    for(auto match: matches)
    {
        /*cout<<endl;
        cout<<match.get_index()<<endl;
        cout<<match.get_start()<<","<<match.get_end()<<endl;
        for(auto it: match.get_keyword())cout<<it<<",";
        cout<<endl;*/
        test_counts[match.get_index()]++;
    }
    for(uint64_t i =0; i<tests.size(); i++)
    {
        for(auto it: tests[i])cout<<it<<",";
        cout<<test_counts[i]<<endl;
    }
    boost::dynamic_bitset<> reference(4);
    vector<boost::dynamic_bitset<>> test_bits;
    test_bits.push_back(boost::dynamic_bitset<>(4));
    test_bits.push_back(boost::dynamic_bitset<>(4));
    test_bits.push_back(boost::dynamic_bitset<>(4));
    test_bits.push_back(boost::dynamic_bitset<>(4));
    test_bits.push_back(boost::dynamic_bitset<>(4));
    test_bits[0][2]=test_bits[0][1]=1;
    test_bits[1][1]=test_bits[1][0]=1;
    test_bits[2][2]=test_bits[2][0]=1;
    test_bits[3][3]=test_bits[3][1]=test_bits[3][0]=1;
    test_bits[4][3]=test_bits[4][2]=test_bits[4][0]=1;
    for(auto &it : test_bits)
    {
        cout<<it<<",";
        it = (~reference)&(it);
        cout<<it.count()<<",";
        reference|=it;
        cout<<reference<<endl;
        if(reference.count()==4)break;
    }
}


#ifndef GRL_ATRACSYS_FUSION_TRACK_TO_EIGEN_HPP
#define GRL_ATRACSYS_FUSION_TRACK_TO_EIGEN_HPP

// FusionTrack Libraries
#include <ftkInterface.h>
#include <geometryHelper.hpp> // I would like to get rid of this and use only ftk functions + remove atracsys_DIR/bin from include directories

#include <spdlog/spdlog.h>

#include <boost/container/static_vector.hpp>
#include <tuple>
#include <string>
#include <vector>
#include <system_error>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace grl { namespace sensor {

Eigen::Affine3f ftkMarkerToAffine3f(const ftkMarker& marker){

         Eigen::Affine3f cameraToMarkerTransform;
         cameraToMarkerTransform.matrix().block<3,3>(0,0) = Eigen::Map<const Eigen::Matrix<float,3,3,Eigen::RowMajor> >(&marker.rotation[0][0]);
         cameraToMarkerTransform.translation().x() = marker.translationMM[0]/1000;
         cameraToMarkerTransform.translation().y() = marker.translationMM[1]/1000;
         cameraToMarkerTransform.translation().z() = marker.translationMM[2]/1000;
         return cameraToMarkerTransform;
}

ftkMarker Affine3fToftkMarker(const Eigen::Affine3f& cameraToMarkerTransform){
         ftkMarker marker;
         Eigen::Map<Eigen::Matrix<float,3,3,Eigen::RowMajor> > markerRotation(&marker.rotation[0][0]);
         markerRotation = cameraToMarkerTransform.matrix().block<3,3>(0,0);
         marker.translationMM[0] = cameraToMarkerTransform.translation().x()*1000;
         marker.translationMM[1] = cameraToMarkerTransform.translation().y()*1000;
         marker.translationMM[2] = cameraToMarkerTransform.translation().z()*1000;
         return marker;
}

Eigen::Vector3f ftk3DPointToVector3f(const ftk3DPoint& point)
{
    Eigen::Vector3f v;
    v.x() = point.x/1000;
    v.y() = point.y/1000;
    v.z() = point.z/1000;
    return v;
}

ftk3DPoint Vector3fToftk3DPoint(const Eigen::Vector3f& point)
{
    ftk3DPoint v;
    v.x = point.x()*1000;
    v.y = point.y()*1000;
    v.z = point.z()*1000;
    return v;
}

}}
#endif // _TO_EIGEN_HPP


#include <stan/variational/advi.hpp>
#include <stan/variational/families/normal_meanfield.hpp>
#include <stan/callbacks/stream_logger.hpp>
#include <gtest/gtest.h>
#include <test/unit/util.hpp>
#include <string>
#include <test/test-models/good/model/valid.hpp>
#include <boost/random/additive_combine.hpp>  // L'Ecuyer RNG
#include <stan/mcmc/chains.hpp>
#include <stan/io/stan_csv_reader.hpp>
#include <gtest/gtest.h>
#include <fstream>
#include <sstream>

typedef boost::ecuyer1988 rng_t;

class RVI_SUBMODULE_TEST : public testing::Test {
public:
    void SetUp() {
      blocker1_stream.open("src/test/unit/mcmc/test_csv_files/blocker.1.csv");
      blocker2_stream.open("src/test/unit/mcmc/test_csv_files/blocker.2.csv");
    }

    void TearDown() {
      blocker1_stream.close();
      blocker2_stream.close();
    }
    std::ifstream blocker1_stream, blocker2_stream;
};

TEST_F(RVI_SUBMODULE_TEST, ess_mcse) {
/*std::stringstream out;
stan::io::stan_csv blocker1
        = stan::io::stan_csv_reader::parse(blocker1_stream, &out);
stan::io::stan_csv blocker2
        = stan::io::stan_csv_reader::parse(blocker2_stream, &out);
EXPECT_EQ("", out.str());

stan::mcmc::chains<> chains(blocker1);
chains.add(blocker2);
Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, 1> samples(
        chains.num_chains());
std::vector<const double*> draws(chains.num_chains());
std::vector<size_t> sizes(chains.num_chains());

for (int index = 4; index < chains.num_params(); index++) {
  for (int chain = 0; chain < chains.num_chains(); ++chain) {
    samples(chain) = chains.samples(chain, index);
    draws[chain] = &samples(chain)(0);
    sizes[chain] = samples(chain).size();
  }
}

Eigen::VectorXd x(6);
x << 1, 2, 3, 4, 5, 6;
std::fstream data_stream(std::string("").c_str(), std::fstream::in);
stan::io::dump data_var_context(data_stream);
data_stream.close();

std::stringstream output;
stan_model model(data_var_context, 0, static_cast<std::stringstream*>(0));

Eigen::VectorXd cont_params = Eigen::VectorXd::Zero(2);
cont_params(0) = 0.75;
cont_params(1) = 0.75;
rng_t base_rng;
base_rng.seed(3021828106u);

double ess, mcse;
int window = 5;
stan::variational::advi<stan_model, stan::variational::normal_meanfield, rng_t>
  test_advi(model, cont_params, base_rng, 10, 10, 100, 1);
  int num_chains = draws.size();
  std::vector<size_t> windows(num_chains, window);
  test_advi.ESS_MCSE(ess, mcse, draws, windows); // J chains of W parameters stored
*/

  stan::variational::normal_meanfield mf(4);
  Eigen::VectorXd params(8);
  params << 1, 2, 3, 4, 5, 6, 7, 8;
  mf.set_approx_params(params);
  std::cout << mf.return_approx_params() << "\n";
  Eigen::VectorXd rhs(8);
  rhs.setOnes(8);
  mf += 1;
  std::cout << mf.mu() << "\n";
}


// Copyright (c) 2017-2018 Steven Watanabe
//
// 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_UNITS2_UNIT_HPP_INCLUDED
#define BOOST_UNITS2_UNIT_HPP_INCLUDED

#include <boost/units2/detail/merge.hpp>
#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/utility.hpp>
#include <boost/integer/common_factor_ct.hpp>
#include <type_traits>
#include <limits>
#include <cstdint>
#include <cmath>

// Design goals:
// - Can represent any unit.
// - Can convert between any units with the same dimensions.
// - New units can be defined in terms of any existing units.
//   Which unit is the base unit and which unit is the derived
//   unit shall not change the behavior of any user code outside
//   the definition itself.
// - The set of dimensions shall be extensible.
// - Every unit shall be represented by one and only one type.
// - To the maximum extent possible, conversions shall not
//   lose precision.
// - The raw type of a unit should be (somewhat) intelligible.
//
// Implementation Notes:
// - All units are ultimately seen as a scaled combination of base units.
// - There is exactly one base unit for each dimension.
// - Conversions reduce both sides to the base form.
// - To avoid loss-of-precision, all conversion factors
//   are tracked and identical components are combined/canceled
//   before any evaluation is done.  In addition, all calculations
//   are carried out using exact (rational) arithmetic wherever possible.
// - The struct that a user defines is a unit, with no wrapping required.
// - All units are reduced to normalized form after every operation.
// - The different types of units can be processed using a visitor via the
//   alias _boost_units2_apply.
//
// Possible improvements:
// - Allow floating point scaling to have greater precision than double.
//   This also means passing the value_type through all the conversion logic.

namespace boost {
namespace units2 {

/*
 * Base class for scales
 */
struct scale_base {
    /// INTERNAL ONLY
    using _boost_units2_is_scale = void;
};

/**
 * Base class for all units.
 */
template<class Derived>
struct unit_base {
    /// INTERNAL ONLY
    /// Tag for use with SFINAE
    using _boost_units2_is_unit = void;
    /// INTERNAL ONLY
    /// visitor to help code that needs to switch on the kind of unit
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_base<T>;
    /// INTERNAL ONLY
    using _boost_units2_type = Derived;
    /// INTERNAL ONLY
    auto operator<=>(const unit_base&) const = default;
};

/**
 * Represents a unit that is a scaled version of another unit.
 * \pre Base is a Unit
 * \pre Scale is either a std::ratio or a type with a nested static constexpr double value();
 */
template<class Base, class Scale>
struct scaled_unit : unit_base<scaled_unit<Base, Scale> > {
    /// INTERNAL ONLY
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_scaled<Base, Scale>;
    /// INTERNAL ONLY
    auto operator<=>(const scaled_unit&) const = default;
};

/**
 * Represents a unit that is a combination of other units.
 * \pre All the elements of P must be specializations of dim
 */
template<class... P>
struct compound_unit : unit_base<compound_unit<P...>> {
    /// INTERNAL ONLY
    auto operator<=>(const compound_unit&) const = default;
};

/// INTERNAL ONLY
/// Specializations to make compound_unit useable with merge.
template<>
struct compound_unit<> : unit_base<compound_unit<>> {
    static constexpr const bool empty = true;
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_compound<>;
    /// INTERNAL ONLY
    auto operator<=>(const compound_unit&) const = default;
};
/// INTERNAL ONLY
template<class P0, class... P>
struct compound_unit<P0, P...> : unit_base<compound_unit<P0,P...>> {
    static constexpr const bool empty = false;
    using front = P0;
    using pop_front = compound_unit<P...>;
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_compound<P0, P...>;
    /// INTERNAL ONLY
    auto operator<=>(const compound_unit&) const = default;
};

namespace detail {

template<class F, class T>
using visit = typename T::template _boost_units_apply2<F, T>;

constexpr int const_strcmp(const char * lhs, const char * rhs)
{
    return (*lhs && *rhs) ?
        (*lhs == *rhs ? const_strcmp(lhs+1, rhs+1) : (*lhs < *rhs ? -1 : 1)) :
        ((!*lhs && !*rhs) ? 0 : (!*lhs ? -1 : 1));
}

template<class T, class U>
struct scale_compare {
    static constexpr const int value = T::value() < U::value?-1:(T::value()>U::value()?1:0);
};
template<class T, long long N, long long D>
struct scale_compare<T,std::ratio<N,D>>
{
    static const constexpr int value = 1;
};
template<class T, long long N, long long D>
struct scale_compare<std::ratio<N,D>,T>
{
    static const constexpr int value = -1;
};
template<long long N1, long long D1, long long N2, long long D2>
struct scale_compare<std::ratio<N1,D1>,std::ratio<N2,D2>>
{
    static const constexpr int value = std::ratio_less<std::ratio<N1,D1>,std::ratio<N2,D2>>::value?-1:
        (std::ratio_less<std::ratio<N2,D2>,std::ratio<N1,D1>>::value?1:0);
};

// For two user-defined units, compare by name
template<class T, class U>
struct unit_compare_impl {
    static constexpr const int value = const_strcmp(T::name, U::name);
    static_assert(std::is_same<T, U>::value || value != 0, "Different units cannot have the same name.");
};

// Compare compond units lexicographically
template<class T0, class... T, class U0, class... U>
struct unit_compare_impl<compound_unit<T0, T...>, compound_unit<U0, U...> >
{
    static constexpr const int value = unit_compare_impl<typename T0::base, typename U0::base>::value?
        unit_compare_impl<typename T0::base, typename U0::base>::value:
        (std::ratio_less<typename T0::exponent, typename U0::exponent>::value?-1:1);
};
template<class T0, class... T, class... U>
struct unit_compare_impl<compound_unit<T0, T...>, compound_unit<T0, U...> >
{
    static constexpr const int value = unit_compare_impl<compound_unit<T...>, compound_unit<U...>>::value;
};
template<class... T>
struct unit_compare_impl<compound_unit<T...>, compound_unit<> >
{
    static constexpr const int value = 1;
};
template<class... T>
struct unit_compare_impl<compound_unit<>, compound_unit<T...> >
{
    static constexpr const int value = -1;
};
template<>
struct unit_compare_impl<compound_unit<>, compound_unit<> >
{
    static constexpr const int value = 0;
};

// For scaled_unit compare the base first, then the exponent
template<class B1, class E1, class B2, class E2>
struct unit_compare_impl<scaled_unit<B1, E1>, scaled_unit<B2, E2> >
{
    static const constexpr int value = (unit_compare_impl<B1, B2>::value != 0)?
        (unit_compare_impl<B1, B2>::value != 0):
        scale_compare<E1, E2>::value;
};

// cross type comparisons
// user-defined unit < scaled_unit < compound_unit
template<class... T, class U>
struct unit_compare_impl<compound_unit<T...>, U>
{
    static const constexpr int value = 1;
};
template<class T, class... U>
struct unit_compare_impl<T, compound_unit<U...>>
{
    static const constexpr int value = -1;
};
template<class... T, class B, class E>
struct unit_compare_impl<compound_unit<T...>, scaled_unit<B,E> >
{
    static const constexpr int value = 1;
};
template<class B, class E, class... U>
struct unit_compare_impl<scaled_unit<B, E>, compound_unit<U...>>
{
    static const constexpr int value = -1;
};
template<class T, class B, class E>
struct unit_compare_impl<T, scaled_unit<B,E> >
{
    static const constexpr int value = -1;
};
template<class B, class E, class U>
struct unit_compare_impl<scaled_unit<B, E>, U>
{
    static const constexpr int value = 1;
};

// pre: T and U are both compound_units
template<class T, class U>
using compound_unit_multiply = detail::merge<unit_compare_impl, compound_unit, T, U>;

// Wrap any class in a compound_unit iff it is not already a compound_unit.
template<class T>
struct as_compound_unit_impl { typedef compound_unit<dim<T, std::ratio<1, 1> > > type; };
template<class... T>
struct as_compound_unit_impl<compound_unit<T...> > { typedef compound_unit<T...> type; };
template<class T>
using as_compound_unit = typename as_compound_unit_impl<T>::type;

// Unwrap any compound units of the form U^1.
// Folds scaled_units that use std::ratio.
// May perform other normalization as needed in the future.
// Note: It is assumed that this normalization is applied
// consistently, so we never need to fix more than the outer layer.
template<class T>
struct simplify_unit_impl { using type = T; };
template<class T>
struct simplify_unit_impl<compound_unit<dim<T, std::ratio<1, 1> > > > { using type = T; };
template<class T, std::intmax_t N1, std::intmax_t D1, std::intmax_t N2, std::intmax_t D2>
struct simplify_unit_impl<scaled_unit<scaled_unit<T, std::ratio<N1,D1>>,std::ratio<N2,D2>>> {
    using new_scale = std::ratio_multiply<std::ratio<N1,D1>,std::ratio<N2,D2>>;
    using type = boost::mp11::mp_if_c<(new_scale::num==1&&new_scale::den==1), T, scaled_unit<T, new_scale> >;
};
template<class T>
struct simplify_unit_impl<scaled_unit<T,std::ratio<1,1>>> { using type = T; };
// resolve ambiguity
template<class T, std::intmax_t N, std::intmax_t D>
struct simplify_unit_impl<scaled_unit<scaled_unit<T,std::ratio<N,D>>,std::ratio<1,1>>> {
    using type = scaled_unit<T,std::ratio<N,D>>;
};
template<class T>
using simplify_unit = typename simplify_unit_impl<T>::type;

// The result of multiplying two units
template<class T, class U>
using unit_multiply = simplify_unit<compound_unit_multiply<as_compound_unit<T>, as_compound_unit<U> > >;

template<class T, class E>
struct unit_pow_impl;
template<class... T, class... E, class R>
struct unit_pow_impl<compound_unit<dim<T, E>...>, R> {
    using type = compound_unit<dim<T, std::ratio_multiply<E, R> >...>;
};
template<class... T, class... E>
struct unit_pow_impl<compound_unit<dim<T, E>...>, std::ratio<0>> {
    using type = compound_unit<>;
};
template<class T, class E>
using unit_pow = simplify_unit<typename unit_pow_impl<as_compound_unit<T>, E>::type>;

template<class T, class U>
using unit_divide = unit_multiply<T, unit_pow<U, std::ratio<-1>>>;

// determine whether a type is a unit
template<class T>
using requires_unit = typename T::_boost_units2_is_unit;

template<class T, class E = void>
struct is_unit_impl {
    using type = std::false_type;
};
template<class T>
struct is_unit_impl<T, requires_unit<T>> {
    using type = std::true_type;
};

// A scale must either be a std::ratio or inherit from scale_base
// This only handles the latter.
template<class T>
using requires_scale = typename T::_boost_units2_is_scale;

} // namespace detail

template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_unit<U> >
constexpr auto operator*(T, U) -> detail::unit_multiply<T, U>
{ return {}; }

template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_unit<U> >
constexpr auto operator/(T, U) -> detail::unit_divide<T, U>
{ return {}; }

// multiplying a unit by a std::ratio creates a scaled_unit
template<class T, long long N, long long D, class = detail::requires_unit<T>>
constexpr auto operator*(T, std::ratio<N,D>) -> detail::simplify_unit<scaled_unit<T, typename std::ratio<N,D>::type>>
{ return {}; }
template<class T, long long N, long long D, class = detail::requires_unit<T>>
constexpr auto operator*(std::ratio<N,D>, T) -> detail::simplify_unit<scaled_unit<T, typename std::ratio<N,D>::type>>
{ return {}; }

// multiplying a unit by any scale gives a scaled unit
template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_scale<U> >
constexpr auto operator*(T, U) -> detail::simplify_unit<scaled_unit<T, U>>
{ return {}; }
template<class T, class U, class = detail::requires_scale<T>, class = detail::requires_unit<U> >
constexpr auto operator*(T, U) -> detail::simplify_unit<scaled_unit<U,T>>
{ return {}; }

template<std::intmax_t N, class T, class = detail::requires_unit<T>>
constexpr auto pow(T) -> detail::unit_pow<T, std::ratio<N>>
{ return {}; }

    template<class T, std::intmax_t N, std::intmax_t D, class = detail::requires_unit<T>>
constexpr auto pow(T, std::ratio<N,D>) -> detail::unit_pow<T, std::ratio<N,D>>
{ return {}; }

// Conversion support

namespace detail {

template<class... D>
struct scale_list;

template<>
struct scale_list<> {
    static const constexpr bool empty = true;
};

template<class D0, class... D>
struct scale_list<D0, D...> {
    static const constexpr bool empty = false;
    using front = D0;
    using pop_front = scale_list<D...>;
};

template<class T, class E>
struct scale_list_pow_impl;
template<class... T, class... E, class R>
struct scale_list_pow_impl<scale_list<dim<T, E>...>, R> {
    using type = scale_list<dim<T, std::ratio_multiply<E, R> >...>;
};
template<class T, class E>
using scale_list_pow = typename scale_list_pow_impl<T, E>::type;

template<class T, class U>
using scale_list_multiply = detail::merge<scale_compare, scale_list, T, U>;

struct flatten_scale_impl;
template<class T>
using flatten_scale = visit<flatten_scale_impl, T>;
struct flatten_scale_impl
{
    template<class T>
    using apply_base = scale_list<>;

    template<class Base, class Scale>
    using apply_scaled = scale_list_multiply<flatten_scale<Base>, scale_list<dim<Scale, std::ratio<1>>>>;

    template<class... T>
    using apply_compound = boost::mp11::mp_fold<boost::mp11::mp_list<scale_list_pow<flatten_scale<typename T::base>, typename T::exponent>...>, scale_list<>, scale_list_multiply>;
};

struct dimension_check_impl;
template<class T>
using dimension_check = visit<dimension_check_impl, T>;
struct dimension_check_impl
{
    template<class T>
    using apply_base = typename T::_boost_units2_type;

    template<class Base, class Scale>
    using apply_scaled = dimension_check<Base>;

    template<class... T>
    using apply_compound = ::boost::mp11::mp_fold< boost::mp11::mp_list<unit_pow<dimension_check<typename T::base>, typename T::exponent>...>, compound_unit<>, unit_multiply>;
};

template<class T>
constexpr double get_value(T) { return T::value(); }
template<long long N, long long D>
constexpr double get_value(std::ratio<N, D>) { return static_cast<double>(N)/D; }

template<class T, class U>
struct multiplier {
    static constexpr double value() { return ::boost::units2::detail::get_value(T()) * ::boost::units2::detail::get_value(U()); }
};

template<class B, class E>
struct power {
    static /*constexpr*/ double value() { return ::std::pow(::boost::units2::detail::get_value(B()), ::boost::units2::detail::get_value(E())); }
};

// Returns 0 if overflow would happen
// precondition: all ratios are positive
template<class T, class U>
struct safe_ratio_multiply
{
    static const constexpr long long gcd1 = ::boost::integer::static_gcd<T::num, U::den>::value;
    static const constexpr long long gcd2 = ::boost::integer::static_gcd<U::num, T::den>::value;
    static const constexpr bool overflow =
        (std::numeric_limits<long long>::max()/(T::num/gcd1) > (U::num/gcd2)) ||
        (std::numeric_limits<long long>::max()/(T::den/gcd1) > (U::den/gcd1));
    using type = std::ratio<
        overflow?0:(T::num/gcd1)*(U::num/gcd2),
        overflow?1:(T::den/gcd1)*(U::den/gcd1)>;
};

constexpr long long safe_multiply(long long lhs, long long rhs)
{
    return (std::numeric_limits<long long>::max)()/lhs >= rhs? lhs*rhs : 0;
}
constexpr long long safe_square(long long arg)
{
    return safe_multiply(arg, arg);
}
constexpr long long safe_power(long long base, long long exponent) {
    return exponent == 1? base : safe_multiply(safe_square(safe_power(base, exponent/2)), (exponent%2?base:1));
}

template<class B, long long E>
struct safe_ratio_pow {
    static const constexpr long long abs_exponent = E < 0? -E : E;
    static const constexpr long long num = safe_power(E<0?B::den:B::num, abs_exponent);
    static const constexpr long long den = safe_power(E<0?B::num:B::den, abs_exponent);
    static const constexpr bool overflow = num==0||den==0;
    using type = std::ratio<overflow?0:num,overflow?1:den>;
};

template<class T, class U>
struct fold_conversion_impl { using type = multiplier<T, U>; };

template<long long N1, long long D1, long long N2, long long D2>
struct fold_conversion_impl<std::ratio<N1,D1>,std::ratio<N2,D2>>
{
    using result1 = typename safe_ratio_multiply<std::ratio<N1, D1>, std::ratio<N2, D2> >::type;
    using type = ::boost::mp11::mp_if_c<result1::num!=0, result1, multiplier<std::ratio<N1,D1>,std::ratio<N2,D2> > >;
};

template<class T>
struct evaluate_power;
template<class Base, class Exponent>
struct evaluate_power<dim<Base, Exponent> >
{
    using type = power<Base, Exponent>;
};
template<long long N, long long D, long long E>
struct evaluate_power<dim<std::ratio<N,D>, std::ratio<E> > >
{
    using result1 = typename safe_ratio_pow<std::ratio<N,D>, E>::type;
    using type = ::boost::mp11::mp_if_c<result1::num!=0, result1, power<std::ratio<N,D>, std::ratio<E> > >;
};

template<class T, class U>
using conversion_fold_op = typename fold_conversion_impl<T,U>::type;

template<class... T>
struct fold_conversion;
template<class... T>
struct fold_conversion<scale_list<T...>> {
    using type = boost::mp11::mp_fold<boost::mp11::mp_list<typename evaluate_power<T>::type...>,std::ratio<1>,conversion_fold_op>;
};

template<class T, class U>
void check_conversion() {
    static_assert(std::is_same<T, U>::value,
        "Cannot convert units with different dimensions.");
}

} // namespace detail

template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_unit<U>>
constexpr bool has_same_dimension(T, U)
{
    return std::is_same<detail::dimension_check<T>, detail::dimension_check<U>>::value;
}

template<class T, class U, class = detail::requires_unit<T>, class = detail::requires_unit<U>>
constexpr double conversion_factor(T, U)
{
    // Indirection to make sure that the reduced dimensions appear
    // in the template backtrace.
    detail::check_conversion<detail::dimension_check<T>, detail::dimension_check<U>>();
    return ::boost::units2::detail::get_value(typename detail::fold_conversion<detail::flatten_scale<detail::unit_divide<T, U>>>::type());
}

}
}

#endif


#ifndef ASLAM_BACKEND_GENERIC_MATRIX_EXPRESSION_HPP
#define ASLAM_BACKEND_GENERIC_MATRIX_EXPRESSION_HPP

#include <Eigen/Core>
#include <aslam/backend/JacobianContainer.hpp>
#include <boost/shared_ptr.hpp>
#include <set>
#include <type_traits>
#include "Differential.hpp"
#include "GenericMatrixExpressionNode.hpp"
#include "ScalarExpression.hpp"

namespace aslam {
namespace backend {

namespace internal {
template <typename TNode>
struct GenericMatrixNodeTraits {
    typedef TNode node_t;
    typedef typename boost::shared_ptr<node_t> node_ptr_t;
    typedef typename node_t::matrix_t matrix_t;
    typedef typename node_t::value_t value_t;
    typedef typename node_t::tangent_vector_t tangent_vector_t;
    typedef typename node_t::differential_t differential_t;
    typedef typename node_t::constant_t constant_t;
};
}  // namespace internal

template <int IRows, int ICols, typename TScalar = double,
          typename TNode = GenericMatrixExpressionNode<IRows, ICols, TScalar> >
class GenericMatrixExpression {
  public:
    typedef GenericMatrixExpression self_t;
    typedef GenericMatrixExpressionNode<IRows, ICols, TScalar> default_node_t;
    typedef TNode node_t;
    typedef GenericMatrixExpression<IRows, ICols, TScalar, default_node_t> default_self_t;

    typedef internal::GenericMatrixNodeTraits<node_t> node_traits_t;
    typedef typename node_traits_t::node_ptr_t node_ptr_t;
    typedef typename node_traits_t::matrix_t matrix_t;
    typedef typename node_traits_t::value_t value_t;
    typedef typename node_traits_t::tangent_vector_t tangent_vector_t;
    typedef typename node_traits_t::differential_t differential_t;
    typedef typename node_traits_t::constant_t constant_t;

    /// \brief initialize from an existing node.
    GenericMatrixExpression(node_ptr_t root);

    /// \brief Initialize from an existing node. The node will not be deleted.
    GenericMatrixExpression(node_t* root);

    /// \brief Initialize a constant expression from an matrix. The matrix will be copied.
    template <typename DERIVED>
    GenericMatrixExpression(const Eigen::MatrixBase<DERIVED>& mat);

    virtual ~GenericMatrixExpression() {}

    /// \brief Evaluate the full Eigen matrix.
    matrix_t toFullMatrix() const;

    /// \brief Evaluate the full Eigen matrix.
    matrix_t evaluate() const { return toFullMatrix(); }

    /// \brief return the expression representing the Moore-Penrose-Inverse of this expression.
    GenericMatrixExpression<ICols, IRows, TScalar> inverse() const;

    /// \brief return the transposed matrix expression.
    GenericMatrixExpression<ICols, IRows, TScalar> transpose() const;

    template <int IColsOther, typename TOtherNode>
    GenericMatrixExpression<IRows, IColsOther, TScalar> operator*(
        const GenericMatrixExpression<ICols, IColsOther, TScalar, TOtherNode>& other) const;

    inline operator GenericMatrixExpression<IRows, ICols, TScalar>() const;

    default_self_t operator*(const ScalarExpression& scalarExpression) const;
    default_self_t operator*(TScalar scalar) const;
    default_self_t operator-() const;
    template <typename TOtherNode>
    default_self_t operator+(const GenericMatrixExpression<IRows, ICols, TScalar, TOtherNode>& other) const;
    template <typename TOtherNode>
    default_self_t operator-(const GenericMatrixExpression<IRows, ICols, TScalar, TOtherNode>& other) const;

    template <int IColsOther, typename TOtherNode,
              typename std::enable_if<IRows == 3 && (ICols == 1 || IColsOther == 1), int>::type = 0>
    GenericMatrixExpression<3, (ICols == 1 ? IColsOther : ICols), TScalar> cross(
        const GenericMatrixExpression<3, IColsOther, TScalar, TOtherNode>& other) const;

    /// \brief Evaluate the Jacobians
    void evaluateJacobians(JacobianContainer& outJacobians) const;
    void evaluateJacobians(JacobianContainer& outJacobians, const Eigen::MatrixXd& applyChainRule) const;
    void evaluateJacobians(JacobianContainer& outJacobians, const differential_t& diff) const;

    void getDesignVariables(DesignVariable::set_t& designVariables) const;

    inline node_ptr_t root() const { return _root; }

    inline static self_t constant(int rows = IRows, int cols = ICols) {
        return GenericMatrixExpression(new constant_t(rows, cols));
    }

  protected:
    GenericMatrixExpression() {}

  private:
    node_ptr_t _root;

  public:
    template <typename TDerived, typename TOperand>
    class UnaryOperationResult : public node_t {
      public:
        typedef TOperand operand_t;
        typedef typename TOperand::node_t operand_node_t;
        typedef typename node_t::differential_t::domain_t apply_diff_return_t;

        virtual ~UnaryOperationResult() {}

        inline apply_diff_return_t applyDiff(const typename operand_t::tangent_vector_t& /* tangent_vector */) const {
            throw std::runtime_error("This method must be shadowed or not used!");
        }

        static inline self_t create(const TOperand& operand) {
            TDerived* p = new TDerived();
            p->_operand = operand.root();
            return self_t(node_ptr_t(p));
        }
        template <typename TData>
        static inline self_t create(const TOperand& operand, TData data) {
            TDerived* p = new TDerived(data);
            p->_operand = operand.root();
            return self_t(node_ptr_t(p));
        }

      protected:
        inline UnaryOperationResult() {}
        const operand_node_t& getOperandNode() const { return *_operand; }
        virtual void getDesignVariablesImplementation(DesignVariable::set_t& designVariables) const {
            _operand->getDesignVariables(designVariables);
        }
        virtual void evaluateJacobiansImplementation(
            JacobianContainer& outJacobians, const typename node_t::differential_t& chainRuleDifferentail) const {
            this->getOperandNode().evaluateJacobians(outJacobians,
                                                     Diff(*static_cast<const TDerived*>(this), chainRuleDifferentail));
        }

      private:
        typename TOperand::node_ptr_t _operand;

        class Diff
            : public ComposedDifferential<typename operand_t::tangent_vector_t, apply_diff_return_t, TScalar, Diff> {
            const TDerived& _derivedNode;

          public:
            typedef ComposedDifferential<typename operand_t::tangent_vector_t, apply_diff_return_t, TScalar, Diff>
                base_t;
            typedef typename base_t::domain_t domain_t;

            inline Diff(const TDerived& derivedNode, const typename TDerived::differential_t& diff)
                : base_t(diff), _derivedNode(derivedNode) {}
            ~Diff() {}

            inline apply_diff_return_t apply(const typename operand_t::tangent_vector_t& tangent_vector) const {
                return _derivedNode.applyDiff(tangent_vector);
            }
        };
    };

    template <typename TDerived, typename TLhs, typename TRhs>
    class BinaryOperationResult : public node_t {
      public:
        typedef typename node_t::differential_t::domain_t apply_diff_return_t;

        virtual ~BinaryOperationResult() {}

        typedef TLhs lhs_t;
        typedef TRhs rhs_t;
        typedef typename TLhs::node_t lhs_node_t;
        typedef typename TRhs::node_t rhs_node_t;

        inline apply_diff_return_t applyLhsDiff(const typename lhs_t::tangent_vector_t& tangent_vector) const {
            throw std::runtime_error("This method must be shadowed or not used!");
        }
        inline apply_diff_return_t applyRhsDiff(const typename rhs_t::tangent_vector_t& tangent_vector) const {
            throw std::runtime_error("This method must be shadowed or not used!");
        }

        static inline self_t create(const TLhs& l, const TRhs& r) {
            TDerived* p = new TDerived();
            p->_lhs = l.root();
            p->_rhs = r.root();
            return self_t(node_ptr_t(p));
        }

      protected:
        inline BinaryOperationResult() {}
        const lhs_node_t& getLhsNode() const { return *_lhs; }
        const rhs_node_t& getRhsNode() const { return *_rhs; }
        virtual void getDesignVariablesImplementation(DesignVariable::set_t& designVariables) const {
            _lhs->getDesignVariables(designVariables);
            _rhs->getDesignVariables(designVariables);
        }
        virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians,
                                                     const typename node_t::differential_t& diff) const {
            this->getLhsNode().evaluateJacobians(
                outJacobians, Diff<lhs_t, &TDerived::applyLhsDiff>(*static_cast<const TDerived*>(this), diff));
            this->getRhsNode().evaluateJacobians(
                outJacobians, Diff<rhs_t, &TDerived::applyRhsDiff>(*static_cast<const TDerived*>(this), diff));
        }

      private:
        typename TLhs::node_ptr_t _lhs;
        typename TRhs::node_ptr_t _rhs;

        template <typename TSide,
                  apply_diff_return_t (TDerived::*FApply)(const typename TSide::tangent_vector_t&) const>
        class Diff : public ComposedDifferential<typename TSide::tangent_vector_t, apply_diff_return_t, TScalar,
                                                 Diff<TSide, FApply> > {
            const TDerived& _derivedNode;

          public:
            typedef ComposedDifferential<typename TSide::tangent_vector_t, apply_diff_return_t, TScalar, Diff> base_t;
            typedef typename base_t::domain_t domain_t;

            inline Diff(const TDerived& derivedNode, const typename TDerived::differential_t& diff)
                : base_t(diff), _derivedNode(derivedNode) {}
            ~Diff() {}

            inline apply_diff_return_t apply(const typename TSide::tangent_vector_t& tangent_vector) const {
                return (_derivedNode.*FApply)(tangent_vector);
            }
        };
    };
};

}  // namespace backend
}  // namespace aslam

#include "implementation/GenericMatrixExpression.hpp"

#endif /* ASLAM_BACKEND_MATRIX_EXPRESSION_HPP */


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_FMA4_FMA_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_FMA4_FMA_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_FMA4_SUPPORT

#include <boost/simd/arithmetic/functions/fma.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::fma_, boost::simd::tag::fma4_,
                      (A0),
                      ((simd_<single_<A0>,boost::simd::tag::sse_>))
                      ((simd_<single_<A0>,boost::simd::tag::sse_>))
                      ((simd_<single_<A0>,boost::simd::tag::sse_>))
                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return _mm_macc_ps(a0, a1, a2);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::fma_, boost::simd::tag::fma4_,
                      (A0),
                      ((simd_<double_<A0>,boost::simd::tag::sse_>))
                      ((simd_<double_<A0>,boost::simd::tag::sse_>))
                      ((simd_<double_<A0>,boost::simd::tag::sse_>))
                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return _mm_macc_pd(a0, a1, a2);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::fma_, boost::simd::tag::fma4_,
                      (A0),
                      ((simd_<single_<A0>,boost::simd::tag::avx_>))
                      ((simd_<single_<A0>,boost::simd::tag::avx_>))
                      ((simd_<single_<A0>,boost::simd::tag::avx_>))
                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return _mm256_macc_ps(a0, a1, a2);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::fma_, boost::simd::tag::fma4_,
                      (A0),
                      ((simd_<double_<A0>,boost::simd::tag::avx_>))
                      ((simd_<double_<A0>,boost::simd::tag::avx_>))
                      ((simd_<double_<A0>,boost::simd::tag::avx_>))
                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return _mm256_macc_pd(a0, a1, a2);
    }
  };
} } }

#endif
#endif


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include "Triangle.h"

#include <Eigen/Dense>

#include "AnalyticalGeometry.h"
#include "MathLib/GeometricBasics.h"
#include "Point.h"

namespace GeoLib
{
Triangle::Triangle(std::vector<Point*> const& pnt_vec, std::size_t pnt_a,
                   std::size_t pnt_b, std::size_t pnt_c)
    : _pnts(pnt_vec), _pnt_ids({{pnt_a, pnt_b, pnt_c}})
{
    assert(!_pnts.empty());
    assert(pnt_a < _pnts.size() && pnt_b < _pnts.size() &&
           pnt_c < _pnts.size());
}

bool Triangle::containsPoint(MathLib::Point3d const& q, double eps) const
{
    GeoLib::Point const& a(*(_pnts[_pnt_ids[0]]));
    GeoLib::Point const& b(*(_pnts[_pnt_ids[1]]));
    GeoLib::Point const& c(*(_pnts[_pnt_ids[2]]));
    return MathLib::isPointInTriangle(q, a, b, c, eps);
}
}  // namespace GeoLib


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "CompareDVH.h"

#include <boost/make_shared.hpp>

namespace rttb
{

	namespace testing
	{

		bool checkEqualDVH(DVHPointer aDVH1, DVHPointer aDVH2)
		{

			bool result;
			const double errorConstantDVH = 1e-4;
			result = lit::AreClose(aDVH1->getDeltaD(), aDVH2->getDeltaD(), errorConstantDVH);
			result = result && lit::AreClose(aDVH1->getDeltaV(), aDVH2->getDeltaV(), errorConstantDVH);
			result = result && (aDVH1->getDoseID() == aDVH2->getDoseID());
			result = result && (aDVH1->getStructureID() == aDVH2->getStructureID());
			result = result && lit::AreClose(aDVH1->getMaximum(), aDVH2->getMaximum(), errorConstantDVH);
			result = result && lit::AreClose(aDVH1->getMinimum(), aDVH2->getMinimum(), errorConstantDVH);
			result = result && lit::AreClose(aDVH1->getMean(), aDVH2->getMean(), errorConstantDVH);
			result = result && (aDVH1->getDataDifferential().size() == aDVH2->getDataDifferential().size());

			for (size_t i = 0; i < aDVH1->getDataDifferential().size(); i++)
			{
				result = result
				         && lit::AreClose(aDVH1->getDataDifferential().at(i), aDVH2->getDataDifferential().at(i),
							 errorConstantDVH);
			}

			return result;

		}

        rttb::testing::DVHPointer computeDiffDVH(DVHPointer aDVH1, DVHPointer aDVH2)
        {
            if (aDVH1->getDeltaD() == aDVH2->getDeltaD() && aDVH1->getDeltaV() == aDVH2->getDeltaV()){

                rttb::core::DVH::DataDifferentialType dvhData1 = aDVH1->getDataDifferential();
                rttb::core::DVH::DataDifferentialType dvhData2 = aDVH2->getDataDifferential();
                rttb::core::DVH::DataDifferentialType dvhDataDifference;

                auto it1 = dvhData1.cbegin();
                auto it2 = dvhData2.cbegin();

                while (it1 != dvhData1.cend() && it2 != dvhData2.cend())
                {
                    dvhDataDifference.push_back(*it1-*it2);
                    ++it1;
                    ++it2;
                }

                auto differenceDVH = ::boost::make_shared<core::DVH>(dvhDataDifference, aDVH1->getDeltaD(), aDVH1->getDeltaV(), aDVH1->getStructureID(), aDVH1->getDoseID());
                return differenceDVH;
            }
            else {
                return aDVH1;
            }
        }

    }//testing
}//rttb


// Copyright (c) Microsoft Corporation.
// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception

#include <cerrno>
#include <cmath>
#include <limits>
#include <type_traits>
#include <utility>

#pragma warning(push)
#pragma warning(disable : 4619) // #pragma warning: there is no warning number '%d'
#pragma warning(disable : 4643) // Forward declaring '%s' in namespace std is not permitted by the C++ Standard
#pragma warning(disable : 4702) // unreachable code
#pragma warning(disable : 5219) // implicit conversion from '%s' to '%s', possible loss of data

#define BOOST_CHRONO_HEADER_ONLY
#define BOOST_CONFIG_SUPPRESS_OUTDATED_MESSAGE
#define BOOST_MATH_DOMAIN_ERROR_POLICY   errno_on_error
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

// Using headers from Boost.Math
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include <boost/math/special_functions/ellint_3.hpp>
#include <boost/math/special_functions/expint.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/math/special_functions/zeta.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/tools/precision.hpp>

#pragma warning(pop)

namespace {
    template <class _Func>
    _NODISCARD auto _Boost_call(const _Func& _Fn) noexcept {
        _TRY_BEGIN
        return _Fn();
        _CATCH_ALL
        errno     = EDOM;
        using _Ty = _STD decay_t<decltype(_Fn())>;
        return _STD numeric_limits<_Ty>::quiet_NaN();
        _CATCH_END
    }
} // unnamed namespace

_EXTERN_C
_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_assoc_laguerre(
    const unsigned int _Pn, const unsigned int _Pm, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::laguerre(_Pn, _Pm, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_assoc_laguerref(
    const unsigned int _Pn, const unsigned int _Pm, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::laguerre(_Pn, _Pm, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_assoc_legendre(
    const unsigned int _Pl, const unsigned int _Pm, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] {
        auto _Result = ::boost::math::legendre_p(_Pl, _Pm, _Px);
        // Boost includes the Condon-Shortley phase term (-1)^m, std does not
        if ((_Pm & 1u) != 0) {
            _Result = -_Result;
        }
        return _Result;
    });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_assoc_legendref(
    const unsigned int _Pl, const unsigned int _Pm, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] {
        auto _Result = ::boost::math::legendre_p(_Pl, _Pm, _Px);
        // Boost includes the Condon-Shortley phase term (-1)^m, std does not
        if ((_Pm & 1u) != 0) {
            _Result = -_Result;
        }
        return _Result;
    });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_beta(const double _Px, const double _Py) noexcept {
    return _Boost_call([=] { return ::boost::math::beta(_Px, _Py); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_betaf(const float _Px, const float _Py) noexcept {
    return _Boost_call([=] { return ::boost::math::beta(_Px, _Py); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_comp_ellint_1(const double _Pk) noexcept {
    return _Boost_call([=] { return ::boost::math::ellint_1(_Pk); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_comp_ellint_1f(const float _Pk) noexcept {
    return _Boost_call([=] { return ::boost::math::ellint_1(_Pk); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_comp_ellint_2(const double _Pk) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    return _Boost_call([=] { return ::boost::math::ellint_2(_Pk); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_comp_ellint_2f(const float _Pk) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    return _Boost_call([=] { return ::boost::math::ellint_2(_Pk); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_comp_ellint_3(const double _Pk, const double _Pnu) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    return _Boost_call([=] { return ::boost::math::ellint_3(_Pk, _Pnu); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_comp_ellint_3f(const float _Pk, const float _Pnu) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    return _Boost_call([=] { return ::boost::math::ellint_3(_Pk, _Pnu); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_cyl_bessel_i(const double _Pnu, const double _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_i(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_cyl_bessel_if(const float _Pnu, const float _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_i(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_cyl_bessel_j(const double _Pnu, const double _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_j(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_cyl_bessel_jf(const float _Pnu, const float _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_j(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_cyl_bessel_k(const double _Pnu, const double _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_k(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_cyl_bessel_kf(const float _Pnu, const float _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_bessel_k(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_cyl_neumann(const double _Pnu, const double _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_neumann(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_cyl_neumannf(const float _Pnu, const float _Px) noexcept {
    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::cyl_neumann(_Pnu, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_ellint_1(const double _Pk, const double _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_1(_Pk, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_ellint_1f(const float _Pk, const float _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_1(_Pk, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_ellint_2(const double _Pk, const double _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_2(_Pk, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_ellint_2f(const float _Pk, const float _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_2(_Pk, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_ellint_3(
    const double _Pk, const double _Pnu, const double _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_3(_Pk, _Pnu, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_ellint_3f(
    const float _Pk, const float _Pnu, const float _Pphi) noexcept {
    if (_STD isnan(_Pk)) {
        return _Pk;
    }

    if (_STD isnan(_Pnu)) {
        return _Pnu;
    }

    if (_STD isnan(_Pphi)) {
        return _Pphi;
    }

    return _Boost_call([=] { return ::boost::math::ellint_3(_Pk, _Pnu, _Pphi); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_expint(const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::expint(_Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_expintf(const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::expint(_Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_hermite(const unsigned int _Pn, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::hermite(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_hermitef(const unsigned int _Pn, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::hermite(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_laguerre(const unsigned int _Pn, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::laguerre(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_laguerref(const unsigned int _Pn, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::laguerre(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_legendre(const unsigned int _Pl, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::legendre_p(_Pl, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_legendref(const unsigned int _Pl, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::legendre_p(_Pl, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_riemann_zeta(const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::zeta(_Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_riemann_zetaf(const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::zeta(_Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_sph_bessel(const unsigned int _Pn, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::sph_bessel(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_sph_besself(const unsigned int _Pn, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::sph_bessel(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_sph_legendre(
    const unsigned int _Pl, const unsigned int _Pm, const double _Ptheta) noexcept {
    if (_STD isnan(_Ptheta)) {
        return _Ptheta;
    }

    return _Boost_call([=] { return ::boost::math::spherical_harmonic_r(_Pl, _Pm, _Ptheta, 0.0); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_sph_legendref(
    const unsigned int _Pl, const unsigned int _Pm, const float _Ptheta) noexcept {
    if (_STD isnan(_Ptheta)) {
        return _Ptheta;
    }

    return _Boost_call([=] { return ::boost::math::spherical_harmonic_r(_Pl, _Pm, _Ptheta, 0.0f); });
}

_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_sph_neumann(const unsigned int _Pn, const double _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::sph_neumann(_Pn, _Px); });
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_sph_neumannf(const unsigned int _Pn, const float _Px) noexcept {
    if (_STD isnan(_Px)) {
        return _Px;
    }

    return _Boost_call([=] { return ::boost::math::sph_neumann(_Pn, _Px); });
}
_END_EXTERN_C

namespace {
    template <class _Ty>
    _NODISCARD _Ty _Hypot3(_Ty _Dx, _Ty _Dy, _Ty _Dz) noexcept { // type-oblivious backend for 3-arg hypot
        static_assert(_STD is_floating_point_v<_Ty>);
        _Dx = _STD abs(_Dx);
        _Dy = _STD abs(_Dy);
        _Dz = _STD abs(_Dz);

        constexpr _Ty _Inf = _STD numeric_limits<_Ty>::infinity();
        if (_Dx == _Inf || _Dy == _Inf || _Dz == _Inf) {
            return _Inf;
        }

        if (_Dy > _Dx) {
            _STD swap(_Dx, _Dy);
        }

        if (_Dz > _Dx) {
            _STD swap(_Dx, _Dz);
        }

        constexpr _Ty _Eps = ::boost::math::tools::epsilon<_Ty>();
        if (_Dx * _Eps >= _Dy && _Dx * _Eps >= _Dz) {
            return _Dx;
        }

        const auto _DyDx = _Dy / _Dx;
        const auto _DzDx = _Dz / _Dx;

        return _Dx * _STD sqrt(1 + _DyDx * _DyDx + _DzDx * _DzDx);
    }
} // unnamed namespace

_EXTERN_C
_CRT_SATELLITE_2 _NODISCARD double __stdcall __std_smf_hypot3(
    const double _Dx, const double _Dy, const double _Dz) noexcept {
    return _Hypot3(_Dx, _Dy, _Dz);
}

_CRT_SATELLITE_2 _NODISCARD float __stdcall __std_smf_hypot3f(
    const float _Dx, const float _Dy, const float _Dz) noexcept {
    return _Hypot3(_Dx, _Dy, _Dz);
}
_END_EXTERN_C


/*
Lightweight Automated Planning Toolkit
Copyright (C) 2012
Miquel Ramirez <miquel.ramirez@rmit.edu.au>
Nir Lipovetzky <nirlipo@gmail.com>
Christian Muise <christian.muise@gmail.com>

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef __H_1__
#define __H_1__

#include <aptk/search_prob.hxx>
#include <aptk/heuristic.hxx>
#include <aptk/ext_math.hxx>
#include <strips_state.hxx>
#include <strips_prob.hxx>
#include <boost/circular_buffer.hpp>
#include <vector>
#include <deque>

namespace aptk {

namespace agnostic {

class H_Max_Evaluation_Function  {

public:

	H_Max_Evaluation_Function( std::vector<float>& value_table )
	: m_values( value_table ) {
	}

	float	operator()( Fluent_Vec::const_iterator begin, Fluent_Vec::const_iterator end, float v2 = 0.0f ) const {
		float v = v2;
		for ( Fluent_Vec::const_iterator it = begin; it != end; it++ ) {
			v = ( v < m_values[*it] ? m_values[*it] : v );
			//if ( v == infty ) return v;
		}
		return v;
	}

private:

	const std::vector<float>&	m_values;

};

class H_Add_Evaluation_Function {

public:
	H_Add_Evaluation_Function( std::vector<float>& value_table )
	: m_values( value_table ) {
	}

	float	operator()( Fluent_Vec::const_iterator begin, Fluent_Vec::const_iterator end, float v2 = 0.0f ) const {
		float v = v2;
		for ( Fluent_Vec::const_iterator it = begin; it != end; it++ ) {
			if ( m_values[*it] == infty )
				return infty;
			v += m_values[*it];
		}
		return v;
	}
private:

	const std::vector<float>&	m_values;

};

enum class H1_Cost_Function { Ignore_Costs, Use_Costs, LAMA};

template <typename Search_Model, typename Fluent_Set_Eval_Func, H1_Cost_Function cost_opt = H1_Cost_Function::Use_Costs >
class H1_Heuristic : public Heuristic<State> {
public:

	typedef STRIPS_Problem::Best_Supporter 	Best_Supporter;

	H1_Heuristic( const Search_Model& prob )
	: Heuristic<State>( prob ), m_strips_model( prob.task() ), eval_func(m_values) {
		m_values.resize( m_strips_model.num_fluents() );
		m_difficulties.resize( m_strips_model.num_fluents() );
		m_best_supporters.resize(  m_strips_model.num_fluents() );
		m_already_updated.resize( m_strips_model.num_fluents() );
		m_allowed_actions.resize( m_strips_model.num_actions() );
		m_updated.resize( m_strips_model.num_fluents() );

		// HAZ: Set up the relevant actions once here so we don't need
		//      to iterate through all of them when evaluating.
		m_relevant_actions.resize( m_strips_model.num_fluents() );

		for ( unsigned i = 0; i < m_strips_model.num_actions(); i++ ) {

			const Action& a = *(m_strips_model.actions()[i]);

			// Relevant if the fluent is in the precondition
			for ( unsigned j = 0; j < a.prec_vec().size(); ++j ) {
				m_relevant_actions[a.prec_vec()[j]].insert(i);
			}

			// Relevant if the fluent is in the head of a conditional effect
			for ( unsigned j = 0; j < a.ceff_vec().size(); ++j ) {

				const Conditional_Effect& ceff = *(a.ceff_vec()[j]);

				for ( unsigned k = 0; k < ceff.prec_vec().size(); ++k) {
					m_relevant_actions[ceff.prec_vec()[k]].insert(i);
				}
			}
		}
	}

	virtual ~H1_Heuristic() {
	}

	float 	value( unsigned p ) const { return m_values[p]; }
	std::vector<float>& values() { return m_values; }

        template <typename Search_Node>
        void eval( const Search_Node* n, float& h_val, std::vector<Action_Idx>& pref_ops) {
		eval(n->state(), h_val, pref_ops);
	}

        template <typename Search_Node>
        void eval( const Search_Node* n, float& h_val ) {

		eval(n->state(),h_val);
	}

	virtual void eval( const Fluent_Vec& s, float& h_val ) {
		h_val = eval_func( s.begin(), s.end() );
	}

	template <typename Cost_Type>
	 void eval( const State& s, Cost_Type& h_out ) {
		float h;
		m_already_updated.reset();
		m_updated.clear();
		initialize(s);
		compute();
		h = eval_func( m_strips_model.goal().begin(), m_strips_model.goal().end() );
		h_out = h == infty ? std::numeric_limits<Cost_Type>::max() : (Cost_Type)h;

	}

	virtual void eval_reachability( const State& s, float& h_val, Fluent_Vec* persist_fluents = NULL ) {
		m_already_updated.reset();
		m_updated.clear();
		initialize(s);
		compute_reachability( persist_fluents );
		h_val = eval_func( m_strips_model.goal().begin(), m_strips_model.goal().end() );

	}

	virtual void eval( const State& s, float& h_val,  std::vector<Action_Idx>& pref_ops ) {
		eval( s, h_val );
	}

	void print_values( std::ostream& os ) const {
		for ( unsigned p = 0; p < m_strips_model.fluents().size(); p++ ){
				os << "h1/add({ ";
				os << m_strips_model.fluents()[p]->signature();
				os << "}) = " << m_values[p] << std::endl;
			}
	}

	Best_Supporter	get_best_supporter( unsigned f ) const {
		return m_best_supporters[f];
	}

        void get_best_supporters( unsigned f, Action_Ptr_Const_Vec& bfs ) const {

		if(m_best_supporters[f].act_idx == no_such_index ) return;

	        const Action* bf = m_strips_model.actions()[ m_best_supporters[f].act_idx ];

		float h_val_bf = eval_func( bf->prec_vec().begin(), bf->prec_vec().end() );
		float h_val = 0;

		if ( !bf->asserts(f) ) { // added by conditional effect
			float min_cond_h = infty;
			for ( auto ceff : bf->ceff_vec() ) {
				if ( ceff->asserts( f ) ) {
					float h_cond = eval_func( ceff->prec_vec().begin(), ceff->prec_vec().end(), h_val_bf );
					if ( h_cond < min_cond_h )
						min_cond_h = h_cond;
				}
			}

			assert( !dequal(min_cond_h,infty) );
			h_val_bf = min_cond_h;
		}

		const std::vector<const Action*>& add_acts = m_strips_model.actions_adding( f );

		for ( unsigned k = 0; k < add_acts.size(); k++ ){
			const Action* a = add_acts[k];
			h_val = eval_func( a->prec_vec().begin(), a->prec_vec().end() );
			if ( !a->asserts( f ) ) { // added by conditional effect
				float min_cond_h = infty;
				for ( auto ceff : a->ceff_vec() ) {
					if ( ceff->asserts(f) ) {
						float h_cond = eval_func( ceff->prec_vec().begin(), ceff->prec_vec().end(), h_val );
						if ( h_cond < min_cond_h )
							min_cond_h = h_cond;
					}
				}
				assert( !dequal(min_cond_h,infty) );
				h_val = min_cond_h;
			}
			if ( dequal(h_val, h_val_bf ) )
				bfs.push_back(a);
		}
	}

protected:

	void	update( unsigned p, float v ) {
		if ( v >= m_values[p] ) return;
		m_values[p] = v;
		if ( !m_already_updated.isset( p ) ) {
			m_updated.push_back( p );
			m_already_updated.set( p );
		}
	}

	void	update( unsigned p, float v, Best_Supporter bs ) {
		update( p, v, bs.act_idx, bs.eff_idx );
	}

	float eval_diff( const Best_Supporter& bs ) const {
		float min_val = infty;
		if ( bs.act_idx == no_such_index )
			return 0;
		const Action* a =  m_strips_model.actions()[bs.act_idx];
		for ( auto p : a->prec_vec() )
			min_val = std::min( min_val, m_values[p] );
		if ( bs.eff_idx == no_such_index ) return min_val;
		for ( auto p : a->ceff_vec()[bs.eff_idx]->prec_vec() )
			min_val = std::min( min_val, m_values[p] );
		return min_val;
	}

	void	update( unsigned p, float v, unsigned act_idx, unsigned eff_idx ) {
		if ( v > m_values[p] ) return;
		if ( v > 0.0f && dequal( v, m_values[p] ) ) {
			Best_Supporter candidate( act_idx, eff_idx );
			float candidate_diff = eval_diff( candidate );
			if ( candidate_diff < m_difficulties[p] ) {
				m_best_supporters[p] = candidate;
				m_difficulties[p] = candidate_diff;
			}
			return;
		}
		m_values[p] = v;
		if ( !m_already_updated.isset( p ) ) {
			m_updated.push_back( p );
			m_already_updated.set( p );
		}
		m_best_supporters[p].act_idx = act_idx;
		m_best_supporters[p].eff_idx = eff_idx;
		m_difficulties[p] = eval_diff( m_best_supporters[p] );
	}

	void	set( unsigned p, float v ) {
		m_values[p] = v;
		if ( !m_already_updated.isset( p ) ) {
			m_updated.push_back( p );
			m_already_updated.set( p );
		}
	}

	void	initialize( const State& s )
	{
		for ( unsigned k = 0; k < m_strips_model.num_fluents(); k++ ) {
		        m_values[k] = m_difficulties[k] = infty;
			m_best_supporters[k] = Best_Supporter( no_such_index, no_such_index );
		}

		for ( unsigned k = 0; k < m_strips_model.empty_prec_actions().size(); k++ ) {
			const Action& a = *(m_strips_model.empty_prec_actions()[k]);
			float v =  ( cost_opt == H1_Cost_Function::Ignore_Costs ? 1.0f :
					( cost_opt == H1_Cost_Function::Use_Costs ? (float)a.cost()  : 1.0f + (float)a.cost()) );

			for ( Fluent_Vec::const_iterator it = a.add_vec().begin();
				it != a.add_vec().end(); it++ )
			    update( *it, v, a.index(), no_such_index );
			// Conditional effects
			for ( unsigned j = 0; j < a.ceff_vec().size(); j++ )
			{
				const Conditional_Effect& ceff = *(a.ceff_vec()[j]);
				if ( !ceff.prec_vec().empty() ) continue;
				float v_eff = v;
				for ( Fluent_Vec::const_iterator it = ceff.add_vec().begin();
					it != ceff.add_vec().end(); it++ )
				    update( *it, v_eff, a.index(), j );
			}
		}

		for ( Fluent_Vec::const_iterator it = s.fluent_vec().begin();
			it != s.fluent_vec().end(); it++ )
			set( *it, 0.0f );
	}

	void	compute(  )
	{

		while ( !m_updated.empty() ) {

			unsigned p = m_updated.front();
			//std::cout << p << ". " << m_strips_model.fluents()[p]->signature() << " " << m_values[p] << std::endl;
			m_updated.pop_front();
			m_already_updated.unset(p);

			//Successor_Generator::Heuristic_Iterator it( m_values, m_strips_model.successor_generator().nodes() );
			//int i = it.first();
			//std::cout << "First action: " << i << std::endl;
			//while ( i != -1 ) {
			for ( std::set<unsigned>::iterator action_it = m_relevant_actions[p].begin(); action_it != m_relevant_actions[p].end(); ++action_it) {

				const Action& a = *(m_strips_model.actions()[*action_it]);

				float h_pre = eval_func( a.prec_vec().begin(), a.prec_vec().end() );

				if ( h_pre == infty ) continue;
				//assert( h_pre != infty );

				//std::cout << "Action " << i << ". " << a.signature() << " relevant" << std::endl;

				float v = ( cost_opt == H1_Cost_Function::Ignore_Costs ?
						1.0f + h_pre :
						( cost_opt == H1_Cost_Function::Use_Costs ?
							(float)a.cost() + h_pre :
							1.0f + (float)a.cost() + h_pre
						) );

				for ( Fluent_Vec::const_iterator it = a.add_vec().begin();
					it != a.add_vec().end(); it++ )
				    update( *it, v, a.index(), no_such_index );
				// Conditional effects
				for ( unsigned j = 0; j < a.ceff_vec().size(); j++ )
				{
					const Conditional_Effect& ceff = *(a.ceff_vec()[j]);
					float h_cond = eval_func( ceff.prec_vec().begin(), ceff.prec_vec().end(), h_pre );
					if ( h_cond == infty ) continue;
					float v_eff = ( cost_opt == H1_Cost_Function::Ignore_Costs ?
						1.0f + h_cond :
						( cost_opt == H1_Cost_Function::Use_Costs ?
							(float)a.cost() + h_cond :
							1.0f + (float)a.cost() + h_cond
						) );
					for ( Fluent_Vec::const_iterator it = ceff.add_vec().begin();
						it != ceff.add_vec().end(); it++ )
					    update( *it, v_eff, a.index(), j );
				}

				//i = it.next();
			}
		}
		//print_values(std::cout);
	}

    /***************
     * Old Version *
     ***************/
	/*void	compute_old(  )
	{
		while ( !m_updated.empty() ) {

			unsigned p = m_updated.front();
			//std::cout << p << ". " << m_strips_model.fluents()[p]->signature() << " " << m_values[p] << std::endl;
			m_updated.pop_front();
			m_already_updated.unset(p);

			//Successor_Generator::Heuristic_Iterator it( m_values, m_strips_model.successor_generator().nodes() );
			//int i = it.first();
			//std::cout << "First action: " << i << std::endl;
			//while ( i != -1 ) {
			for ( unsigned i = 0; i < m_strips_model.num_actions(); i++ ) {
				const Action& a = *(m_strips_model.actions()[i]);

				//std::cout << "Action considered: " << a.signature() << std::endl;
				bool relevant =  a.prec_set().isset(p);

				for ( unsigned j = 0; j < a.ceff_vec().size() && !relevant; j++ ) {
					const Conditional_Effect& ceff = *(a.ceff_vec()[j]);
					relevant = relevant || ceff.prec_set().isset(p);
				}

				if ( !relevant ) {
					//i = it.next();
					continue;
				}

				float h_pre = eval_func( a.prec_vec().begin(), a.prec_vec().end() );

				if ( h_pre == infty ) continue;
				//assert( h_pre != infty );

				//std::cout << "Action " << i << ". " << a.signature() << " relevant" << std::endl;

				float v = ( cost_opt == H1_Cost_Function::Ignore_Costs ?
						1.0f + h_pre :
						( cost_opt == H1_Cost_Function::Use_Costs ?
							(float)a.cost() + h_pre :
							1.0f + (float)a.cost() + h_pre
						) );

				for ( Fluent_Vec::const_iterator it = a.add_vec().begin();
					it != a.add_vec().end(); it++ )
					update( *it, v, m_strips_model.actions()[i] );
				// Conditional effects
				for ( unsigned j = 0; j < a.ceff_vec().size(); j++ )
				{
					const Conditional_Effect& ceff = *(a.ceff_vec()[j]);
					float h_cond = eval_func( ceff.prec_vec().begin(), ceff.prec_vec().end(), h_pre );
					if ( h_cond == infty ) continue;
					float v_eff = ( cost_opt == H1_Cost_Function::Ignore_Costs ?
						1.0f + h_cond :
						( cost_opt == H1_Cost_Function::Use_Costs ?
							(float)a.cost() + h_cond :
							1.0f + (float)a.cost() + h_cond
						) );
					for ( Fluent_Vec::const_iterator it = ceff.add_vec().begin();
						it != ceff.add_vec().end(); it++ )
						update( *it, v_eff, m_strips_model.actions()[i] );
				}

				//i = it.next();
			}
		}
	}*/

	void	compute_reachability( Fluent_Vec* persist_fluents = NULL )
	{
		std::vector< const Action*>::const_iterator it_a =  m_strips_model.actions().begin();
		for (Bool_Vec::iterator it = m_allowed_actions.begin(); it != m_allowed_actions.end(); it++, it_a++){
			//		for ( unsigned i = 0; i < m_strips_model.num_actions(); i++ ) {
			//m_allowed_actions[i]=true;
			*it = true;

			if(! persist_fluents ) continue;

			//const Action& a = *(m_strips_model.actions()[i]);

			/**
			 * If actions edel or adds fluent that has to persist, exclude action.
			 */

			for(unsigned p = 0; p < persist_fluents->size(); p++){
				unsigned fl = persist_fluents->at(p);
				if( (*it_a)->asserts( fl ) || (*it_a)->edeletes( fl ) ){
					//m_allowed_actions[i] = false;
					*it = false;
					break;
				}
			}
		}

		while ( !m_updated.empty() ) {

			unsigned p = m_updated.front();
			//std::cout << p << ". " << m_strips_model.fluents()[p]->signature() << " " << m_values[p] << std::endl;
			m_updated.pop_front();
			m_already_updated.unset(p);

			//Successor_Generator::Heuristic_Iterator it( m_values, m_strips_model.successor_generator().nodes() );
			//int i = it.first();
			//std::cout << "First action: " << i << std::endl;
			//while ( i != -1 ) {
			Bool_Vec::iterator it_allowed = m_allowed_actions.begin();
			for ( unsigned i = 0; i < m_strips_model.num_actions(); i++, it_allowed++ ) {
				const Action& a = *(m_strips_model.actions()[i]);

				//if( ! m_allowed_actions[ i ] )
				if(! *it_allowed )
					continue;

				//std::cout << "Action considered: " << a.signature() << std::endl;
				bool relevant =  a.prec_set().isset(p);

				for ( unsigned j = 0; j < a.ceff_vec().size() && !relevant; j++ ) {
					const Conditional_Effect& ceff = *(a.ceff_vec()[j]);
					relevant = relevant || ceff.prec_set().isset(p);
				}

				if ( !relevant ) {
					//i = it.next();
					continue;
				}

				float h_pre = eval_func( a.prec_vec().begin(), a.prec_vec().end() );

				if ( h_pre == infty ) continue;
				//assert( h_pre != infty );

				//std::cout << "Action " << i << ". " << a.signature() << " relevant" << std::endl;

				float v = 0.0f;

				for ( Fluent_Vec::const_iterator it = a.add_vec().begin();
					it != a.add_vec().end(); it++ )
					update( *it, v );
				// Conditional effects
				for ( unsigned j = 0; j < a.ceff_vec().size(); j++ )
				{
					const Conditional_Effect& ceff = *(a.ceff_vec()[j]);
					float h_cond = std::max(eval_func( ceff.prec_vec().begin(), ceff.prec_vec().end() ), h_pre);
					if ( h_cond == infty ) continue;
					float v_eff = 0.0f;
					for ( Fluent_Vec::const_iterator it = ceff.add_vec().begin();
						it != ceff.add_vec().end(); it++ )
						update( *it, v_eff );
				}

				//i = it.next();
			}
		}
	}

protected:

	const STRIPS_Problem&			m_strips_model;
	std::vector<float>			m_values;
	std::vector<float>			m_difficulties;
	Fluent_Set_Eval_Func			eval_func;
	std::vector< Best_Supporter >		m_best_supporters;
	std::vector<const Action*>		m_app_set;
	std::vector< std::set<unsigned> > m_relevant_actions;
	//std::deque<unsigned> 			m_updated;
	boost::circular_buffer<int>		m_updated;
	Bit_Set					m_already_updated;
	Bool_Vec                                m_allowed_actions;
};

}

}

#endif // h_add.hxx


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/msix.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check msix behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::msix;
  using boost::simd::Msix;

  STF_TYPE_IS(decltype(Msix<T>()), T);
  STF_EQUAL(Msix<T>(), T(-6));
  STF_EQUAL(msix( as(T{}) ),T(-6));
}

STF_CASE_TPL( "Check msix behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::msix;
  using boost::simd::Msix;

  STF_TYPE_IS(decltype(Msix<T>()), T);
  STF_IEEE_EQUAL(Msix<T>(), T(-6));
  STF_IEEE_EQUAL(msix( as(T{}) ), T(-6));
}


#pragma once

#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/voxel_grid.hpp>
#include <voxelized_geometry_tools/collision_map.hpp>

// TODO(calderpg) Factor this out to support different voxel grid types.
namespace voxelized_geometry_tools
{
/// This sets all voxels intersected by the provided triangle to filled. Note
/// that it sets voxels filled regardless of whether or not the voxel cell
/// center is "inside" or "outside" the triangle.
/// Note: this uses a conservative approximation of triangle-box collision that
/// will identify some extraneous voxels as colliding. For its use so far, this
/// has better than adding too few, since we generally want a watertight layer
/// of surface voxels.
void RasterizeTriangle(
    const std::vector<Eigen::Vector3d>& vertices,
    const std::vector<Eigen::Vector3i>& triangles,
    const size_t triangle_index,
    voxelized_geometry_tools::CollisionMap& collision_map,
    const bool enforce_collision_map_contains_triangle = true);

/// This sets all voxels intersected by the provided mesh to filled. Note
/// that it sets voxels filled regardless of whether or not the voxel cell
/// center is "inside" or "outside" the intersecting triangle(s) of the mesh.
void RasterizeMesh(
    const std::vector<Eigen::Vector3d>& vertices,
    const std::vector<Eigen::Vector3i>& triangles,
    voxelized_geometry_tools::CollisionMap& collision_map,
    const bool enforce_collision_map_contains_mesh = true);

/// This sets all voxels intersected by the provided mesh to filled. Note
/// that it sets voxels filled regardless of whether or not the voxel cell
/// center is "inside" or "outside" the intersecting triangle(s) of the mesh.
/// The generated CollisionMap will fully contain the provided mesh.
voxelized_geometry_tools::CollisionMap RasterizeMeshIntoCollisionMap(
    const std::vector<Eigen::Vector3d>& vertices,
    const std::vector<Eigen::Vector3i>& triangles,
    const double resolution);
}  // namespace voxelized_geometry_tools