//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_ComptonProfilePolicy_def.hpp
//! \author Alex Robinson
//! \brief  Policies (defs.) for using Compton profiles
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_COMPTON_PROFILE_POLICY_DEF_HPP
#define MONTE_CARLO_COMPTON_PROFILE_POLICY_DEF_HPP

// Std Lib Includes
#include <cmath>

// Boost Includes
#include <boost/units/cmath.hpp>

// FRENSIE Includes
#include "MonteCarlo_ComptonProfile.hpp"
#include "Utility_RandomNumberGenerator.hpp"

namespace MonteCarlo{

// Check if the full Compton profile is valid
/*! \details A full Compton profile must have a range between +/- m_e*c. The
 * upper bound can be greater than m_e*c but no less.
 */
inline bool FullComptonProfilePolicy::isValidProfile(
                                                const ComptonProfile& profile )
{
  return profile.getLowerBoundOfMomentum() == -1.0*ComptonProfile::MomentumUnit()
    && profile.getUpperBoundOfMomentum() >= 1.0*ComptonProfile::MomentumUnit();
}

// Get the lower bound of the momentum
inline ComptonProfile::MomentumQuantity
FullComptonProfilePolicy::getLowerBoundOfMomentum(
                                                const ComptonProfile& profile )
{
  return profile.getLowerBoundOfMomentum();
}

// Get the upper bound of the momentum
inline ComptonProfile::MomentumQuantity
FullComptonProfilePolicy::getUpperBoundOfMomentum(
                                                const ComptonProfile& profile )
{
  return profile.getUpperBoundOfMomentum();
}

// Get lower limit of integration
/*! \details For a full profile the lower limit of integration will always
 * be -1.0
 */
inline ComptonProfile::MomentumQuantity
FullComptonProfilePolicy::getLowerLimitOfIntegration(
                           const ComptonProfile::MomentumQuantity upper_limit )
{
  return -1.0*ComptonProfile::MomentumUnit();
}

// Get upper limit of integration
/*! \details This method simply checks if the upper limit passed is less than
 * the table max. If it is greater, the table max is returned.
 */
inline ComptonProfile::MomentumQuantity
FullComptonProfilePolicy::getUpperLimitOfIntegration(
                           const ComptonProfile& profile,
                           const ComptonProfile::MomentumQuantity upper_limit )
{
  if( upper_limit > profile.getUpperBoundOfMomentum() )
    return profile.getUpperBoundOfMomentum();
  else
    return upper_limit;
}

// Evaluate a full Compton profile
inline ComptonProfile::ProfileQuantity FullComptonProfilePolicy::evaluate(
                              const ComptonProfile& profile,
                              const ComptonProfile::MomentumQuantity momentum )
{
  return profile.evaluate( momentum );
}

// Evaluate a full Compton profile using the max momentum as a limit
/*! \details With a full Compton profile there is no check to see if the
 * momentum is greater than -max_momentum (as with the half profiles).
 */
inline ComptonProfile::ProfileQuantity
FullComptonProfilePolicy::evaluateWithPossibleLimit(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity momentum,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  if( momentum <= max_momentum )
    return profile.evaluate( momentum );
  else
    return 0.0*ComptonProfile::ProfileUnit();
}

// Sample from a full Compton profile
inline ComptonProfile::MomentumQuantity FullComptonProfilePolicy::sample(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  // Make sure the max momentum is valid
  testPrecondition( max_momentum >= -1.0*ComptonProfile::MomentumUnit() );

  if( max_momentum >= profile.getUpperBoundOfMomentum() )
    return profile.sample();
  else
    return profile.sampleInSubrange( max_momentum );
}

// Check if the half Compton profile is valid
/*! \details Using only half of the profile is a way to approximate the
 * true profile. The lower momentum bound must be zero. The upper bound
 * can be anything (since this is an approximation).
 */
inline bool HalfComptonProfilePolicyHelper::isValidProfile(
                                               const ComptonProfile& profile )
{
  return profile.getLowerBoundOfMomentum() == 0.0*ComptonProfile::MomentumUnit()
    && profile.getUpperBoundOfMomentum() <= 1.0*ComptonProfile::MomentumUnit();
}

// Get the lower bound of the momentum
inline ComptonProfile::MomentumQuantity
HalfComptonProfilePolicyHelper::getLowerBoundOfMomentum(
                                                const ComptonProfile& profile )
{
  return -profile.getUpperBoundOfMomentum();
}

// Get the upper bound of the momentum
inline ComptonProfile::MomentumQuantity
HalfComptonProfilePolicyHelper::getUpperBoundOfMomentum(
                                                const ComptonProfile& profile )
{
  return profile.getUpperBoundOfMomentum();
}

// Get lower limit of integration
/*! \details For a half profile the lower limit of integration will be
 * -upper_limit unless the upper limit is <= 0.0, in which case it is equal
 * to the upper_limit (which ensures that the integral is zero).
 */
inline ComptonProfile::MomentumQuantity
HalfComptonProfilePolicyHelper::getLowerLimitOfIntegration(
                           const ComptonProfile::MomentumQuantity upper_limit )
{
  if( upper_limit > 0.0*ComptonProfile::MomentumUnit() )
    return -upper_limit;
  else
    return upper_limit;
}

// Get upper limit of integration
/*! \details This method simply checks if the upper limit passed is less than
 * the table max. If it is greater, the table max is returned.
 */
inline ComptonProfile::MomentumQuantity
HalfComptonProfilePolicyHelper::getUpperLimitOfIntegration(
                           const ComptonProfile& profile,
                           const ComptonProfile::MomentumQuantity upper_limit )
{
  if( upper_limit > profile.getUpperBoundOfMomentum() )
    return profile.getUpperBoundOfMomentum();
  else
    return upper_limit;
}

// Sample from a half Compton profile
/*! \details Using only half of the profile is a way to approximate the
 * true profile. The momentum sampled from the half distribution will be
 * randomly multiplied by -1 to account for the other half of the distribution
 * (part of it at least). As the max momentum approaches zero this
 * approximation of the true distribution becomes poor. If the max momentum
 * is negative, which is physically acceptable, a momentum of zero is returned,
 * which is the value associated with no Doppler broadening (it corresponds
 * to the Compton line photon energy).
 */
inline ComptonProfile::MomentumQuantity HalfComptonProfilePolicyHelper::sample(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  // Make sure the max momentum is valid
  testPrecondition( max_momentum >= -1.0*ComptonProfile::MomentumUnit() );

  if( max_momentum > 0.0*ComptonProfile::MomentumUnit() )
  {
    ComptonProfile::MomentumQuantity pz;

    if( max_momentum >= profile.getUpperBoundOfMomentum() )
      pz = profile.sample();
    else
      pz = profile.sampleInSubrange( max_momentum );

    if( Utility::RandomNumberGenerator::getRandomNumber<double>() <= 0.5 )
      pz *= -1.0;

    return pz;
  }
  else
    return 0.0*ComptonProfile::MomentumUnit();
}

// Evaluate a half Compton profile
/*! \details The absolute value of the momentum will be used to
 * evaluate the profile.
 */
inline ComptonProfile::ProfileQuantity HalfComptonProfilePolicy::evaluate(
                              const ComptonProfile& profile,
                              const ComptonProfile::MomentumQuantity momentum )
{
  return profile.evaluate( boost::units::fabs( momentum ) );
}

// Evaluate a half Compton profile using the max momentum as a limit
/*! \details The half Compton profile will only be evaluated between
 * [-max_momentum,max_momentum]. If max_momentum is < 0.0, a value of
 * 0.0 will be returned.
 */
inline ComptonProfile::ProfileQuantity
HalfComptonProfilePolicy::evaluateWithPossibleLimit(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity momentum,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  if( max_momentum >= 0.0*ComptonProfile::MomentumUnit() )
  {
    if( momentum <= max_momentum && momentum >= -max_momentum )
      return profile.evaluate( boost::units::fabs( momentum ) );
    else
      return 0.0*ComptonProfile::ProfileUnit();
  }
  else
    return 0.0*ComptonProfile::ProfileUnit();
}

// Evaluate a double half Compton profile
/*! \details The doubled half Compton profile has been doubled so that it
 * remains normalized. The evaluated profile value will be divided by two to
 * account for the doubling. The absolute value of the momentum will be used
 * to evaluate the profile.
 */
inline ComptonProfile::ProfileQuantity
DoubledHalfComptonProfilePolicy::evaluate(
                              const ComptonProfile& profile,
                              const ComptonProfile::MomentumQuantity momentum )
{
  return profile.evaluate( boost::units::fabs( momentum ) )/2.0;
}

// Evaluate a doubled half Compton profile using the max momentum as a limit
/*! \details The half Compton profile will only be evaluated between
 * [-max_momentum,max_momentum]. If max_momentum is < 0.0, a value of
 * 0.0 will be returned.
 */
inline ComptonProfile::ProfileQuantity
DoubledHalfComptonProfilePolicy::evaluateWithPossibleLimit(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity momentum,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  if( max_momentum >= 0.0*ComptonProfile::MomentumUnit() )
  {
    if( momentum <= max_momentum && momentum >= -max_momentum )
      return profile.evaluate( boost::units::fabs( momentum ) )/2.0;
    else
      return 0.0*ComptonProfile::ProfileUnit();
  }
  else
    return 0.0*ComptonProfile::ProfileUnit();
}

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_COMPTON_PROFILE_POLICY_DEF_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ComptonProfilePolicy_def.hpp
//---------------------------------------------------------------------------//


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MAT_MAT_TIMES_EXPR_INCLUDE
#define MTL_MAT_MAT_TIMES_EXPR_INCLUDE

#include <boost/mpl/if.hpp>
#include <boost/mpl/and.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/type_traits/is_base_of.hpp>

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/matrix/mat_mat_op_expr.hpp>
#include <boost/numeric/mtl/operation/sfunctor.hpp>
#include <boost/numeric/mtl/operation/compute_factors.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>

namespace mtl { namespace mat {

template <typename E1, typename E2>
struct mat_mat_times_expr
  : public mat_mat_op_expr< E1, E2, mtl::sfunctor::times<typename Collection<E1>::value_type, typename Collection<E2>::value_type> >,
    public mat_expr< mat_mat_times_expr<E1, E2> >
{
    typedef mat_mat_op_expr< E1, E2, mtl::sfunctor::times<typename Collection<E1>::value_type, typename Collection<E2>::value_type> > op_base;
    typedef mat_expr< mat_mat_times_expr<E1, E2> >                                                       crtp_base;
    typedef mat_mat_times_expr                   self;
    typedef E1                                   first_argument_type ;
    typedef E2                                   second_argument_type ;
    typedef typename E1::orientation             orientation;
    typedef mtl::non_fixed::dimensions           dim_type;
    typedef typename E1::key_type                key_type;

    typedef typename Collection<E1>::value_type  first_value_type;
    typedef typename Collection<E2>::value_type  second_value_type;
    typedef typename Multiplicable<first_value_type, second_value_type>::result_type result_value_type;

#if 0 // Just an idea
    typedef typename boost::mpl::if_<
                boost::mpl::and_<
	            boost::is_base_of<tag::sparse, typename traits::category<E1>::type>
	          , boost::is_base_of<tag::sparse, typename traits::category<E2>::type>
		>
	      , compressed2D<result_value_type>
	      , dense2D<result_value_type, parameters<> >
            >::type                                       evaluated_result_type;

    // Convert into matrix

    operator evaluated_result_type() const
    {
	return evaluated_result_type(first * second);
    }
#endif

    mat_mat_times_expr( E1 const& v1, E2 const& v2 )
      : op_base( v1, v2 ), crtp_base(*this), first(v1), second(v2)
    {}

    // To prevent that cout << A * B prints the element-wise product, suggestion by Hui Li
    // It is rather inefficient, esp. for multiple products (complexity increases with the number of arguments :-!)
    //    or sparse matrices.
    // Better compute your product first and print it then when compute time is an issue,
    // this is ONLY for convenience.
    result_value_type
    operator()(std::size_t r, std::size_t c) const
    {
	using math::zero;
	MTL_THROW_IF(num_cols(first) != num_rows(second), incompatible_size());

	result_value_type ref, sum(zero(ref));
	for (std::size_t i= 0; i < num_cols(first); i++)
	    sum+= first(r, i) * second(i, c);
	return sum;
    }

    result_value_type
    operator()(std::size_t r, std::size_t c)
    {
	return (*const_cast<const self*>(this))(r, c);
    }

    first_argument_type const&  first ;
    second_argument_type const& second ;
};

template <typename E1, typename E2>
std::size_t inline num_rows(const mat_mat_times_expr<E1, E2>& expr)
{ return num_rows(expr.first); }

template <typename E1, typename E2>
std::size_t inline num_cols(const mat_mat_times_expr<E1, E2>& expr)
{ return num_cols(expr.second); }

template <typename E1, typename E2>
std::size_t inline size(const mat_mat_times_expr<E1, E2>& expr)
{ return num_rows(expr) * num_cols(expr); }

}} // Namespace mtl::matrix

#endif // MTL_MAT_MAT_TIMES_EXPR_INCLUDE


// Copyright (C) 2015 Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/python.h>
#include <dlib/geometry.h>
#include <pybind11/stl_bind.h>
#include "indexing.h"
#include "opaque_types.h"
#include <dlib/filtering.h>

using namespace dlib;
using namespace std;

namespace py = pybind11;

// ----------------------------------------------------------------------------------------

long left(const rectangle& r) { return r.left(); }
long top(const rectangle& r) { return r.top(); }
long right(const rectangle& r) { return r.right(); }
long bottom(const rectangle& r) { return r.bottom(); }
long width(const rectangle& r) { return r.width(); }
long height(const rectangle& r) { return r.height(); }
unsigned long area(const rectangle& r) { return r.area(); }

double dleft(const drectangle& r) { return r.left(); }
double dtop(const drectangle& r) { return r.top(); }
double dright(const drectangle& r) { return r.right(); }
double dbottom(const drectangle& r) { return r.bottom(); }
double dwidth(const drectangle& r) { return r.width(); }
double dheight(const drectangle& r) { return r.height(); }
double darea(const drectangle& r) { return r.area(); }

template <typename rect_type>
bool is_empty(const rect_type& r) { return r.is_empty(); }

template <typename rect_type>
point center(const rect_type& r) { return center(r); }

template <typename rect_type>
point dcenter(const rect_type& r) { return dcenter(r); }

template <typename rect_type, typename ptype>
bool contains(const rect_type& r, const ptype& p) { return r.contains(p); }

template <typename rect_type>
bool contains_xy(const rect_type& r, const long x, const long y) { return r.contains(point(x, y)); }

template <typename rect_type>
bool contains_rec(const rect_type& r, const rect_type& r2) { return r.contains(r2); }

template <typename rect_type>
rect_type intersect(const rect_type& r, const rect_type& r2) { return r.intersect(r2); }

template <typename rect_type>
string print_rectangle_str(const rect_type& r)
{
    std::ostringstream sout;
    sout << r;
    return sout.str();
}

string print_rectangle_repr(const rectangle& r)
{
    std::ostringstream sout;
    sout << "rectangle(" << r.left() << "," << r.top() << "," << r.right() << "," << r.bottom() << ")";
    return sout.str();
}

string print_drectangle_repr(const drectangle& r)
{
    std::ostringstream sout;
    sout << "drectangle(" << r.left() << "," << r.top() << "," << r.right() << "," << r.bottom() << ")";
    return sout.str();
}

string print_rect_filter(const rect_filter& r)
{
    std::ostringstream sout;
    sout << "rect_filter(";
    sout << "measurement_noise="<<r.get_left().get_measurement_noise();
    sout << ", typical_acceleration="<<r.get_left().get_typical_acceleration();
    sout << ", max_measurement_deviation="<<r.get_left().get_max_measurement_deviation();
    sout << ")";
    return sout.str();
}

// ----------------------------------------------------------------------------------------

void bind_rectangles(py::module& m)
{
    {
    typedef rectangle type;
    py::class_<type>(m, "rectangle", "This object represents a rectangular area of an image.")
        .def(py::init<long,long,long,long>(), py::arg("left"),py::arg("top"),py::arg("right"),py::arg("bottom"))
        .def(py::init<drectangle>(), py::arg("rect"))
        .def(py::init<rectangle>(), py::arg("rect"))
        .def(py::init())
        .def("area",   &::area)
        .def("left",   &::left)
        .def("top",    &::top)
        .def("right",  &::right)
        .def("bottom", &::bottom)
        .def("width",  &::width)
        .def("height", &::height)
        .def("tl_corner", &type::tl_corner, "Returns the top left corner of the rectangle.")
        .def("tr_corner", &type::tr_corner, "Returns the top right corner of the rectangle.")
        .def("bl_corner", &type::bl_corner, "Returns the bottom left corner of the rectangle.")
        .def("br_corner", &type::br_corner, "Returns the bottom right corner of the rectangle.")
        .def("is_empty", &::is_empty<type>)
        .def("center", &::center<type>)
        .def("dcenter", &::dcenter<type>)
        .def("contains", &::contains<type,point>, py::arg("point"))
        .def("contains", &::contains<type,dpoint>, py::arg("point"))
        .def("contains", &::contains_xy<type>, py::arg("x"), py::arg("y"))
        .def("contains", &::contains_rec<type>, py::arg("rectangle"))
        .def("intersect", &::intersect<type>, py::arg("rectangle"))
        .def("__str__", &::print_rectangle_str<type>)
        .def("__repr__", &::print_rectangle_repr)
        .def(py::self += point())
        .def(py::self + point())
        .def(py::self += rectangle())
        .def(py::self + rectangle())
        .def(py::self == py::self)
        .def(py::self != py::self)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }
    {
    typedef drectangle type;
    py::class_<type>(m, "drectangle", "This object represents a rectangular area of an image with floating point coordinates.")
        .def(py::init<double,double,double,double>(), py::arg("left"), py::arg("top"), py::arg("right"), py::arg("bottom"))
        .def(py::init<rectangle>(), py::arg("rect"))
        .def(py::init<drectangle>(), py::arg("rect"))
        .def(py::init<>())
        .def("area",   &::darea)
        .def("left",   &::dleft)
        .def("top",    &::dtop)
        .def("right",  &::dright)
        .def("bottom", &::dbottom)
        .def("width",  &::dwidth)
        .def("height", &::dheight)
        .def("is_empty", &::is_empty<type>)
        .def("center", &::center<type>)
        .def("dcenter", &::dcenter<type>)
        .def("tl_corner", &type::tl_corner, "Returns the top left corner of the rectangle.")
        .def("tr_corner", &type::tr_corner, "Returns the top right corner of the rectangle.")
        .def("bl_corner", &type::bl_corner, "Returns the bottom left corner of the rectangle.")
        .def("br_corner", &type::br_corner, "Returns the bottom right corner of the rectangle.")
        .def("contains", &::contains<type,point>, py::arg("point"))
        .def("contains", &::contains<type,dpoint>, py::arg("point"))
        .def("contains", &::contains_xy<type>, py::arg("x"), py::arg("y"))
        .def("contains", &::contains_rec<type>, py::arg("rectangle"))
        .def("intersect", &::intersect<type>, py::arg("rectangle"))
        .def("__str__", &::print_rectangle_str<type>)
        .def("__repr__", &::print_drectangle_repr)
        .def(py::self == py::self)
        .def(py::self != py::self)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }

    {
        typedef rect_filter type;
        py::class_<type>(m, "rect_filter",
            R"asdf(
                This object is a simple tool for filtering a rectangle that
                measures the location of a moving object that has some non-trivial
                momentum.  Importantly, the measurements are noisy and the object can
                experience sudden unpredictable accelerations.  To accomplish this
                filtering we use a simple Kalman filter with a state transition model of:

                    position_{i+1} = position_{i} + velocity_{i}
                    velocity_{i+1} = velocity_{i} + some_unpredictable_acceleration

                and a measurement model of:

                    measured_position_{i} = position_{i} + measurement_noise

                Where some_unpredictable_acceleration and measurement_noise are 0 mean Gaussian
                noise sources with standard deviations of typical_acceleration and
                measurement_noise respectively.

                To allow for really sudden and large but infrequent accelerations, at each
                step we check if the current measured position deviates from the predicted
                filtered position by more than max_measurement_deviation*measurement_noise
                and if so we adjust the filter's state to keep it within these bounds.
                This allows the moving object to undergo large unmodeled accelerations, far
                in excess of what would be suggested by typical_acceleration, without
                then experiencing a long lag time where the Kalman filter has to "catches
                up" to the new position.  )asdf"
        )
        .def(py::init<double,double,double>(), py::arg("measurement_noise"), py::arg("typical_acceleration"), py::arg("max_measurement_deviation"))
        .def("measurement_noise",   [](const rect_filter& a){return a.get_left().get_measurement_noise();})
        .def("typical_acceleration",   [](const rect_filter& a){return a.get_left().get_typical_acceleration();})
        .def("max_measurement_deviation",   [](const rect_filter& a){return a.get_left().get_max_measurement_deviation();})
        .def("__call__", [](rect_filter& f, const dlib::rectangle& r){return rectangle(f(r)); }, py::arg("rect"))
        .def("__repr__", print_rect_filter)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }

    m.def("find_optimal_rect_filter",
        [](const std::vector<rectangle>& rects, const double smoothness ) { return find_optimal_rect_filter(rects, smoothness); },
        py::arg("rects"),
        py::arg("smoothness")=1,
"requires \n\
    - rects.size() > 4 \n\
    - smoothness >= 0 \n\
ensures \n\
    - This function finds the \"optimal\" settings of a rect_filter based on recorded \n\
      measurement data stored in rects.  Here we assume that rects is a complete \n\
      track history of some object's measured positions.  Essentially, what we do \n\
      is find the rect_filter that minimizes the following objective function: \n\
         sum of abs(predicted_location[i] - measured_location[i]) + smoothness*abs(filtered_location[i]-filtered_location[i-1]) \n\
         Where i is a time index. \n\
      The sum runs over all the data in rects.  So what we do is find the \n\
      filter settings that produce smooth filtered trajectories but also produce \n\
      filtered outputs that are as close to the measured positions as possible. \n\
      The larger the value of smoothness the less jittery the filter outputs will \n\
      be, but they might become biased or laggy if smoothness is set really high. "
    /*!
        requires
            - rects.size() > 4
            - smoothness >= 0
        ensures
            - This function finds the "optimal" settings of a rect_filter based on recorded
              measurement data stored in rects.  Here we assume that rects is a complete
              track history of some object's measured positions.  Essentially, what we do
              is find the rect_filter that minimizes the following objective function:
                 sum of abs(predicted_location[i] - measured_location[i]) + smoothness*abs(filtered_location[i]-filtered_location[i-1])
                 Where i is a time index.
              The sum runs over all the data in rects.  So what we do is find the
              filter settings that produce smooth filtered trajectories but also produce
              filtered outputs that are as close to the measured positions as possible.
              The larger the value of smoothness the less jittery the filter outputs will
              be, but they might become biased or laggy if smoothness is set really high.
    !*/
    );

    {
    typedef std::vector<rectangle> type;
    py::bind_vector<type>(m, "rectangles", "An array of rectangle objects.")
        .def(py::init<size_t>(), py::arg("initial_size"))
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def("extend", extend_vector_with_python_list<rectangle>)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }

    {
    typedef std::vector<std::vector<rectangle>> type;
    py::bind_vector<type>(m, "rectangless", "An array of arrays of rectangle objects.")
        .def(py::init<size_t>(), py::arg("initial_size"))
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def("extend", extend_vector_with_python_list<rectangle>)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }

    m.def("translate_rect", [](const rectangle& rect, const point& p){return translate_rect(rect,p);},
" returns rectangle(rect.left()+p.x, rect.top()+p.y, rect.right()+p.x, rect.bottom()+p.y) \n\
  (i.e. moves the location of the rectangle but doesn't change its shape)",
        py::arg("rect"), py::arg("p"));

    m.def("translate_rect", [](const drectangle& rect, const point& p){return translate_rect(rect,p);},
" returns rectangle(rect.left()+p.x, rect.top()+p.y, rect.right()+p.x, rect.bottom()+p.y) \n\
  (i.e. moves the location of the rectangle but doesn't change its shape)",
        py::arg("rect"), py::arg("p"));

    m.def("translate_rect", [](const rectangle& rect, const dpoint& p){return translate_rect(rect,point(p));},
" returns rectangle(rect.left()+p.x, rect.top()+p.y, rect.right()+p.x, rect.bottom()+p.y) \n\
  (i.e. moves the location of the rectangle but doesn't change its shape)",
        py::arg("rect"), py::arg("p"));

    m.def("translate_rect", [](const drectangle& rect, const dpoint& p){return translate_rect(rect,p);},
" returns rectangle(rect.left()+p.x, rect.top()+p.y, rect.right()+p.x, rect.bottom()+p.y) \n\
  (i.e. moves the location of the rectangle but doesn't change its shape)",
        py::arg("rect"), py::arg("p"));

    m.def("shrink_rect", [](const rectangle& rect, long num){return shrink_rect(rect,num);},
" returns rectangle(rect.left()+num, rect.top()+num, rect.right()-num, rect.bottom()-num) \n\
  (i.e. shrinks the given rectangle by shrinking its border by num)",
        py::arg("rect"), py::arg("num"));

    m.def("grow_rect", [](const rectangle& rect, long num){return grow_rect(rect,num);},
"- return shrink_rect(rect, -num) \n\
  (i.e. grows the given rectangle by expanding its border by num)",
        py::arg("rect"), py::arg("num"));

    m.def("scale_rect", [](const rectangle& rect, double scale){return scale_rect(rect,scale);},
"- return scale_rect(rect, scale) \n\
(i.e. resizes the given rectangle by a scale factor)",
        py::arg("rect"), py::arg("scale"));

    m.def("centered_rect", [](const point& p, unsigned long width, unsigned long height) {
        return centered_rect(p, width, height); },
        py::arg("p"), py::arg("width"), py::arg("height"));

    m.def("centered_rects", [](const std::vector<point>& p, unsigned long width, unsigned long height) {
        return centered_rects(p, width, height); },
        py::arg("pts"), py::arg("width"), py::arg("height"));

    m.def("centered_rect", [](const dpoint& p, unsigned long width, unsigned long height) {
        return centered_rect(p, width, height); },
        py::arg("p"), py::arg("width"), py::arg("height"));

    m.def("centered_rect", [](const rectangle& rect, unsigned long width, unsigned long height) {
        return centered_rect(rect, width, height); },
        py::arg("rect"), py::arg("width"), py::arg("height"));

    m.def("centered_rect", [](const drectangle& rect, unsigned long width, unsigned long height) {
        return centered_rect(rect, width, height); },
        py::arg("rect"), py::arg("width"), py::arg("height"));

    m.def("center", [](const rectangle& rect){return center(rect); }, py::arg("rect"),
        "    returns the center of the given rectangle");
    m.def("center", [](const drectangle& rect){return center(rect); }, py::arg("rect"),
        "    returns the center of the given rectangle");
}

// ----------------------------------------------------------------------------------------


#pragma once

#include <boost/dynamic_bitset.hpp>
#include <regex>
#include <vector>

#include "state.hpp"
#include "type.hpp"

enum {
    PAULI_ID_I = 0,
    PAULI_ID_X = 1,
    PAULI_ID_Y = 2,
    PAULI_ID_Z = 3,
};

class DllExport MultiQubitPauliOperator {
private:
    std::vector<UINT> _target_index;
    std::vector<UINT> _pauli_id;
    boost::dynamic_bitset<> _x;
    boost::dynamic_bitset<> _z;

    void set_bit(const UINT pauli_id, const UINT target_index);

public:
    MultiQubitPauliOperator(){};

    MultiQubitPauliOperator(const std::vector<UINT>& target_qubit_index_list,
        const std::vector<UINT>& pauli_id_list)
        : _target_index(target_qubit_index_list), _pauli_id(pauli_id_list) {
        for (ITYPE i = 0; i < pauli_id_list.size(); i++) {
            set_bit(pauli_id_list[i], target_qubit_index_list[i]);
        }
    };

    explicit MultiQubitPauliOperator(std::string pauli_string) {
        std::string pattern = "([IXYZ])\\s*([0-9]+)\\s*";
        std::regex re(pattern);
        std::cmatch result;
        while (std::regex_search(pauli_string.c_str(), result, re)) {
            std::string pauli = result[1].str();
            UINT index = (UINT)std::stoul(result[2].str());
            _target_index.push_back(index);
            UINT pauli_id;
            if (pauli == "I")
                pauli_id = PAULI_ID_I;
            else if (pauli == "X")
                pauli_id = PAULI_ID_X;
            else if (pauli == "Y")
                pauli_id = PAULI_ID_Y;
            else if (pauli == "Z")
                pauli_id = PAULI_ID_Z;
            else
                assert(false && "Error in regex");
            _pauli_id.push_back(pauli_id);
            set_bit(pauli_id, index);
            pauli_string = result.suffix();
        }
        assert(_target_index.size() == _pauli_id.size());
    }

    MultiQubitPauliOperator(
        const boost::dynamic_bitset<>& x, const boost::dynamic_bitset<>& z)
        : _x(x), _z(z) {
        ITYPE index;
        _z.resize(_x.size());
        for (index = 0; index < this->_x.size(); index++) {
            UINT pauli_id;
            if (!this->_x[index] && !this->_z[index])
                pauli_id = PAULI_ID_I;
            else if (!this->_x[index] && this->_z[index])
                pauli_id = PAULI_ID_Z;
            else if (this->_x[index] && !this->_z[index])
                pauli_id = PAULI_ID_X;
            else if (this->_x[index] && this->_z[index])
                pauli_id = PAULI_ID_Y;
            if (pauli_id != PAULI_ID_I) {
                _target_index.push_back(index);
                _pauli_id.push_back(pauli_id);
            }
        }
    };

    ~MultiQubitPauliOperator(){};

    const std::vector<UINT>& get_pauli_id_list() const;
    const std::vector<UINT>& get_index_list() const;
    const boost::dynamic_bitset<>& get_x_bits() const { return this->_x; }
    const boost::dynamic_bitset<>& get_z_bits() const { return this->_z; }

    void add_single_Pauli(UINT qubit_index, UINT pauli_type);

    CPPCTYPE get_expectation_value(const QuantumStateBase* state) const;

    CPPCTYPE get_transition_amplitude(const QuantumStateBase* state_bra,
        const QuantumStateBase* state_ket) const;

    MultiQubitPauliOperator* copy() const;

    bool operator==(const MultiQubitPauliOperator& target) const;

    MultiQubitPauliOperator operator*(
        const MultiQubitPauliOperator& target) const;

    MultiQubitPauliOperator& operator*=(const MultiQubitPauliOperator& target);

    std::string to_string() const;
};

using PauliOperator = MultiQubitPauliOperator;


/*
==============================================================================
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain

Permission is hereby granted, free  of charge, to any person obtaining
a  copy  of this  software  and  associated  documentation files  (the
"Software"), to  deal in  the Software without  restriction, including
without limitation  the rights to  use, copy, modify,  merge, publish,
distribute,  sublicense and/or  sell copies  of the  Software,  and to
permit persons to whom the Software  is furnished to do so, subject to
the following condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNER.

The  above  copyright  notice  and  this permission  notice  shall  be
included in all copies or substantial portions of the Software.

THE  SOFTWARE IS  PROVIDED  "AS  IS", WITHOUT  WARRANTY  OF ANY  KIND,
EXPRESS OR  IMPLIED, INCLUDING  BUT NOT LIMITED  TO THE  WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT  SHALL THE AUTHORS OR COPYRIGHT HOLDERS  BE LIABLE FOR ANY
CLAIM, DAMAGES OR  OTHER LIABILITY, WHETHER IN AN  ACTION OF CONTRACT,
TORT  OR OTHERWISE, ARISING  FROM, OUT  OF OR  IN CONNECTION  WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

==============================================================================
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: janosch $
//   Date:                $Date: 2007-12-13 14:50:05 $
//   Revision:            $Revision: 1.4 $
//
//

// System includes
#include <cstddef>

// External includes
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

// Project includes
#include "includes/define.h"
#include "integration/quadrature.h" // to bo changed to the new version below.
#include "integration/gauss_legendre_integration_points.h"
//#include "quadratures/gauss_legendre_integration_points.h"
//#include "quadratures/quadrature.h"
#include "python/container_from_python.h"

namespace Kratos
{

	template<std::size_t TDimension>
  std::stringstream& operator << (std::stringstream& rOStream,
				  std::vector<IntegrationPoint<TDimension> > const & rThis)
    {
 	typename std::vector<IntegrationPoint<TDimension> >::size_type i;
 	for(i = 0 ; i < rThis.size() - 1 ; i++)
	  rOStream << rThis[i] << " , " << std::endl;
 	rOStream << rThis[i];
      return rOStream;
    }

namespace Python
{

  using namespace boost::python;

  template<class TArrayType>
  void AddIntegrationPointsArray(std::string const& rArrayName, TArrayType const & Dummy)
  {
    ContainerFromPython< TArrayType >();

     class_<TArrayType>(rArrayName.c_str(), init<int>())
	.def(init<const TArrayType&>())
	.def(vector_indexing_suite<TArrayType>())
 	.def(self_ns::str(self))
	;

  }

  void  AddQuadraturesToPython()
  {
    AddIntegrationPointsArray("IntegrationPointsArray", Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints1,1,Kratos::IntegrationPoint<3> >::IntegrationPointsArrayType());
    //AddIntegrationPointsArray("IntegrationPoint2DsArray", Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints<1>,2,Kratos::IntegrationPoint<3> >::IntegrationPointsArrayType());
    //AddIntegrationPointsArray("IntegrationPoint3DsArray", Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints<1>,3,Kratos::IntegrationPoint<3> >::IntegrationPointsArrayType());
    //AddIntegrationPointsArray("IntegrationPoint4DsArray", Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints<1>,4,Kratos::IntegrationPoint<3> >::IntegrationPointsArrayType());

    scope().attr("GaussLegendreQuadrature1D1") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints1,1,Kratos::IntegrationPoint<3> >::IntegrationPoints();
    scope().attr("GaussLegendreQuadrature1D2") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints2,1,Kratos::IntegrationPoint<3> >::IntegrationPoints();
    scope().attr("GaussLegendreQuadrature1D3") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints3,1,Kratos::IntegrationPoint<3> >::IntegrationPoints();
    scope().attr("GaussLegendreQuadrature2D1") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints1,2,Kratos::IntegrationPoint<3> >::IntegrationPoints();
    scope().attr("GaussLegendreQuadrature2D2") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints2,2,Kratos::IntegrationPoint<3> >::IntegrationPoints();
    scope().attr("GaussLegendreQuadrature2D3") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints3,2,Kratos::IntegrationPoint<3> >::IntegrationPoints();
    scope().attr("GaussLegendreQuadrature3D1") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints1,3,Kratos::IntegrationPoint<3> >::IntegrationPoints();
    scope().attr("GaussLegendreQuadrature3D2") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints2,3,Kratos::IntegrationPoint<3> >::IntegrationPoints();
    scope().attr("GaussLegendreQuadrature3D3") = Kratos::Quadrature<Kratos::GaussLegendreIntegrationPoints3,3,Kratos::IntegrationPoint<3> >::IntegrationPoints();
  }

}  // namespace Python.

} // Namespace Kratos


/*!
*	@file	psalm.cpp
*	@brief	Main file for "psalm"
*
*	"psalm" (Pretty Subdivision ALgorithms on Meshes) is a CLI tool that
*	implements the Doo-Sabin and Catmull-Clark subdivision schemes.
*/

#include <iostream>
#include <sstream>
#include <fstream>
#include <algorithm>
#include <set>

#include <boost/program_options.hpp>

#include <cerrno>
#include <cstring>

#include <unistd.h>
#include <getopt.h>
#include <libgen.h>

#include "SubdivisionAlgorithms/CatmullClark.h"
#include "SubdivisionAlgorithms/DooSabin.h"
#include "SubdivisionAlgorithms/Loop.h"
#include "SubdivisionAlgorithms/Liepa.h"

#include "FairingAlgorithms/FairingAlgorithm.h"

// FIXME: This should be conditional
#if 0
  #include "FairingAlgorithms/CurvatureFlow.h"
#endif

#include "mesh.h"

psalm::mesh scene_mesh;
std::string input;
std::string output;

namespace po = boost::program_options;

/*!
*	Reads a map of weights for a sudivision algorithm from a file. Each
*	line of the file must be of the following form:
*
*		<k> <a_1> ... <a_k>
*
*	Where <k> is the number of vertices for a face and <a_i> are the
*	weights that are used for the vertices of the face. The weights are
*	supposed to be arranged in counterclockwise order.
*
*	@param filename Filename of weights map
*	@return Associative array for quickly looking up the weights.
*/

psalm::weights_map load_weights_map(const std::string& filename)
{
	psalm::weights_map res;

	std::ifstream in;
	errno = 0;
	in.open(filename.c_str());

	if(!in.good() || errno)
	{
		std::string error = strerror(errno);
		std::cerr	<< "psalm: Could not load weights map file \""
				<< filename << "\": "
				<< error << "\n";

		return(res);
	}

	std::string line;
	std::istringstream converter;
	while(getline(in, line))
	{
		converter.clear();
		converter.str(line);

		std::vector<double> weights;
		size_t k = 0;

		// Read k and try to read k weights afterwards

		converter >> k;
		if(converter.fail())
		{
			std::cerr	<< "psalm: Unable to read number of weights "
					<< "from line \"" << line << "\"\n";
			return(res);
		}

		double w = 0.0;
		for(size_t i = 0; i < k; i++)
		{
			converter >> w;
			if(converter.fail())
			{
				std::cerr	<< "psalm: Unable to read weights "
						<< "from line \"" << line << "\"\n";
				return(res);
			}

			weights.push_back(w);
		}

		res[k] = weights;
	}

	return(res);
}

/*!
*	Parses a string of comma-separated numbers.
*
*	@param	argument Argument string
*	@return Set of numbers.
*/

std::set<size_t> parse_value_string(std::string argument)
{
	std::set<size_t> res;

	std::istringstream val_stream(argument);
	std::istringstream converter;
	std::string val_str;
	while(getline(val_stream, val_str, ','))
	{
		converter.clear();
		converter.str(val_str);

		size_t value;
		converter >> value;
		if(converter.fail())
		{
			std::cerr << "psalm: Unable to convert \"" << val_str << "\" to a number.\n";
			return(res);
		}

		res.insert(value);
	}

	return(res);
}

/*!
*	Handles user interaction.
*
*	@param argc Number of command-line arguments
*	@param argv Vector of command-line arguments
*/

int main(int argc, char* argv[])
{
	psalm::mesh::file_type type = psalm::mesh::TYPE_EXT;

	std::set<size_t> remove_faces;
	std::set<size_t> remove_vertices;
	psalm::weights_map extra_weights;

	size_t steps	= 0;

	psalm::SubdivisionAlgorithm* subdivision_algorithm	= NULL;
	psalm::FairingAlgorithm* fairing_algorithm		= NULL;

	// Add general program options

	po::options_description general("General", 80);
	general.add_options()
		(	"algorithm,a",
			po::value<std::string>(),
			"Selects subdivision algorithm to use on the input mesh. Valid values:\n"\
			"* catmull-clark, catmull, clark, cc [default]\n"\
			"* doo-sabin, doo, sabin, ds\n"\
			"* loop, l")

		(	"type,t",
			po::value<std::string>(),
			"Selects type of input data. Valid values:\n"\
			"* ply (Stanford PLY files)\n"\
			"* obj (Wavefront OBJ files)\n"\
			"* off (Geomview object files)")

		(	"output,o",
			po::value<std::string>(&output)->default_value(""),
			"Sets output file")

		(	"steps,n",
			po::value<size_t>(&steps),
			"Sets number of subdivision steps to perform on the input mesh.")

		(	"statistics,s",
			"Prints statistics to STDERR")

		(	"help,h",
			"Shows this screen");

	// Add tuning program options

	po::options_description tuning("Algorithm tuning", 80);
	tuning.add_options()
		(	"preserve-boundaries,p",
			"Forces subdivision algorithms to preserve the original boundaries of a mesh. "\
			"This is especially useful for open meshes. This flag is not taken into "\
			"account by every subdivision algorithm.")

		(	"handle-creases,c",
			"Subdivides crease and boundary edges whenever the algorithm supports this. "\
			"For the Catmull-Clark scheme, for example, midpoints of edges are used for "\
			"the new control points")

		(	"geometric,g",
			"Forces algorithms to compute new points using "\
			"geometric methods and forbids the use of parametric (i.e. weight-based) ones.")

		(	"b-spline-weights,b",
			"Forces algorithms to use the B-spline weights in regular cases, thereby "\
			"overriding any other weights scheme.")

		(	"extra-weights,e",
			po::value<std::string>(),
			"Overrides default weights of subdivision schemes by reading them from <arg>. "\
			"The exact format of this file depends on the subdivision algorithm that is used.")

		(	"weights,w",
			po::value<std::string>(),
			"Selects type of weights that are used for the subdivision scheme. Algorithms "\
			"may ignore unknown values. Valid values:\n"\
			"* catmull-clark, catmull, clark, cc\n"\
			"* default\n"\
			"* degenerate\n"\
			"* doo-sabin, doo, sabin, ds\n")

		(	"fair,f",
			"Performs a fairing step after working with the mesh.");

	// Add pruning program options

	po::options_description pruning("Pruning", 80);
	pruning.add_options()
		(	"remove-faces",
			po::value<std::string>(),
			"Remove faces whose number of sides matches one of the numbers in the list. "\
			"Use commas to separate list values.")

		(	"remove-vertices",
			po::value<std::string>(),
			"Remove vertices whose valency matches one of the numbers in the list. "\
			"Use commas to separate list values.");

	// Add hidden program options that are used for multiple input files

	po::options_description hidden_options("");
	hidden_options.add_options()
		(	"input",
			po::value< std::vector<std::string> >());

	// Describe visible options of the program and parse the command-line
	// using _all_ available options

	po::options_description visible_options("Usage: psalm [arguments] [files]");
	po::options_description all_options("");

	visible_options.add(general);
	visible_options.add(tuning);
	visible_options.add(pruning);

	all_options.add(visible_options);
	all_options.add(hidden_options);

	po::positional_options_description p;
	p.add("input", -1);

	po::variables_map vm;
	try
	{
		po::store(po::command_line_parser(argc, argv)	.options(all_options)
								.positional(p)
								.run(), vm);
		po::notify(vm);
	}
	catch(const boost::program_options::error& e)
	{
		std::cerr << "psalm: Options error: " << e.what() << "\n";
		return(-1);
	}

	// Actual handling of command-line options; mostly, parameters are set

	if(vm.count("help"))
	{
		std::cerr << all_options << "\n";
		return(0);
	}

	if(vm.count("type"))
	{
		std::string type_str = vm["type"].as<std::string>();
		std::transform(type_str.begin(), type_str.end(), type_str.begin(), (int(*)(int)) tolower);
		if(type_str == "ply")
			type = psalm::mesh::TYPE_PLY;
		else if(type_str == "obj")
			type = psalm::mesh::TYPE_OBJ;
		else if(type_str == "off")
			type = psalm::mesh::TYPE_OFF;
		else
		{
			std::cerr << "psalm: \"" << type_str << "\" is an unknown mesh data type.\n";
			return(-1);
		}
	}

	// As there is currently only _one_ fairing algorithm, there is really
	// not much choice here
	if(vm.count("fair"))
        {
// FIXME: Should be conditionally disabled...
#if 0
		fairing_algorithm = new psalm::CurvatureFlow();
#endif
        }

	// We use this instance to create an instance of a subdivision
	// algorithm class. Further class parameters are set _afterwards_,
	// sometimes depending on the type of subdivision algorithm.
	if(vm.count("algorithm"))
	{
		std::string algorithm_str = vm["algorithm"].as<std::string>();
		std::transform(algorithm_str.begin(), algorithm_str.end(), algorithm_str.begin(), (int(*)(int)) tolower);

		if(	algorithm_str == "catmull-clark"	||
			algorithm_str == "catmull"		||
			algorithm_str == "clark"		||
			algorithm_str == "cc")
		{
			subdivision_algorithm = new psalm::CatmullClark();
		}
		else if(algorithm_str == "doo-sabin"		||
			algorithm_str == "doo"			||
			algorithm_str == "sabin"		||
			algorithm_str == "ds")
		{
			subdivision_algorithm = new psalm::DooSabin();
		}
		else if(algorithm_str == "loop"	||
			algorithm_str == "l")
		{
			subdivision_algorithm = new psalm::Loop();
		}
		else if(algorithm_str == "liepa")
		{
			subdivision_algorithm = new psalm::Liepa();
		}
		else
		{
			std::cerr << "psalm: \"" << algorithm_str << "\" is an unknown algorithm.\n";
			return(-1);
		}
	}

	// Only applicable if the subdivision algorithm is the Doo-Sabin
	// subdivision scheme
	if(vm.count("extra-weights"))
	{
		psalm::DooSabin* ds_algorithm = dynamic_cast<psalm::DooSabin*>(subdivision_algorithm);
		if(ds_algorithm)
		{
			extra_weights = load_weights_map(vm["extra-weights"].as<std::string>());
			if(extra_weights.size() == 0)
			{
				std::cerr << "psalm: Unwilling to continue with empty weights file.\n";
				return(-1);
			}

			ds_algorithm->set_custom_weights(extra_weights);
		}
		else
			std::cerr << "psalm: Warning: Weights file specified, but no Doo-Sabin algorithm.\n";
	}

	if(vm.count("weights"))
	{
		std::string weights_str = vm["weights"].as<std::string>();
		std::transform(weights_str.begin(), weights_str.end(), weights_str.begin(), (int(*)(int)) tolower);

		if(	weights_str == "catmull-clark"	||
			weights_str == "catmull"	||
			weights_str == "clark"		||
			weights_str == "cc")
		{
			subdivision_algorithm->set_weights(psalm::SubdivisionAlgorithm::catmull_clark);
		}
		else if(weights_str == "doo-sabin"	||
			weights_str == "doo"		||
			weights_str == "sabin"		||
			weights_str == "ds")
		{
			subdivision_algorithm->set_weights(psalm::SubdivisionAlgorithm::doo_sabin);
		}
		else if(weights_str == "degenerate")
		{
			subdivision_algorithm->set_weights(psalm::SubdivisionAlgorithm::degenerate);
		}
		else
		{
			std::cerr << "psalm: \"" << weights_str << "\" is an unknown weight scheme.\n";
			return(-1);
		}
	}

	// This is parsed using an external function because the parameter
	// string consists of comma-separated values

	if(vm.count("remove-faces"))
		remove_faces = parse_value_string(vm["remove-faces"].as<std::string>());

	if(vm.count("remove-vertices"))
		remove_vertices = parse_value_string(vm["remove-vertices"].as<std::string>());

	// Various small flags

	if(vm.count("handle-creases"))
		subdivision_algorithm->set_crease_handling_flag();

	if(vm.count("geometric"))
		subdivision_algorithm->set_geometric_point_creation_flag();

	if(vm.count("preserve-boundaries"))
		subdivision_algorithm->set_boundary_preservation_flag();

	if(vm.count("statistics"))
		subdivision_algorithm->set_statistics_flag();

	// This only works for B-spline-based subdivision algorithms, hence the
	// dynamic_cast.
	if(vm.count("b-spline-weights"))
	{
		psalm::BsplineSubdivisionAlgorithm* b_spline_algorithm = dynamic_cast<psalm::BsplineSubdivisionAlgorithm*>(subdivision_algorithm);
		if(b_spline_algorithm)
			b_spline_algorithm->set_bspline_weights_usage();
	}

	// Read further command-line parameters; these are all supposed to be
	// input files. If the user already specified an output file, only one
	// input file will be accepted.

	std::vector<std::string> files;
	if(vm.count("input"))
	{
		files = vm["input"].as< std::vector<std::string> >();
		if(output.length() != 0 && files.size() > 1)
		{
			std::cerr << "psalm: Output file specified, but more than one input file present.\n";
			return(-1);
		}

	}

	// Replace the special file "-" by an empty string, thereby signalling
	// that standard input and standard output are to be used as file
	// streams.

	for(std::vector<std::string>::iterator it = files.begin(); it != files.end(); it++)
	{
		if(it->length() == 1 && (*it)[0] == '-')
			*it = "";
	}

	bool output_set = (output.length() > 0);
	if(output.length() == 1 && output[0] == '-')
		output = "";

	// Try to read from STDIN if no input files have been specified
	if(files.size() == 0)
		files.push_back("");

	// Apply subdivision algorithm to all files

	for(std::vector<std::string>::iterator it = files.begin(); it != files.end(); it++)
	{
		scene_mesh.load(*it, type);

		// It is possible that the user did not choose a subdivision
		// algorithm. psalm tries to work as a mesh converter in this
		// instance.
		if(subdivision_algorithm)
			subdivision_algorithm->apply_to(scene_mesh, steps);

		// Ditto for the fairing algorithm.
		if(fairing_algorithm)
			fairing_algorithm->apply_to(scene_mesh);

		scene_mesh.prune(remove_faces, remove_vertices);

		// If an output file has been set (even if it is empty), it
		// will be used.
		if(output_set)
			scene_mesh.save(output, type);

		// If no output file has been set and the input file name is
		// not empty, the output will be written to a file.
		else if(it->length() > 0)
		{
			size_t ext_pos = (*it).find_last_of(".");
			if(ext_pos == std::string::npos)
				scene_mesh.save(*it+".subdivided", type);
			else
				scene_mesh.save( (*it).substr(0, ext_pos) + "_subdivided"
						+(*it).substr(ext_pos));
		}

		// If no output file has been set and the input file name is
		// empty, the output will be written to STDOUT.
		else
			scene_mesh.save("", type);
	}

	delete(subdivision_algorithm);
	delete(fairing_algorithm);

	return(0);
}


//
//  IglUtils.hpp
//  IPC
//
//  Created by Minchen Li on 8/30/17.
//

#ifndef IglUtils_hpp
#define IglUtils_hpp

#include "Mesh.hpp"

#ifdef USE_CLOSEDFORMSVD2D
#include "ClosedFormSVD2d.hpp"
#else
#include "AutoFlipSVD.hpp"
#endif

#include "LinSysSolver.hpp"

#ifdef USE_PREDICATES
#include <igl/predicates/predicates.h>
// #include <ECCD.hpp>
#endif

#include <Eigen/Eigen>

#include <iostream>
#include <fstream>

namespace IPC {

// a static class implementing basic geometry processing operations that are not provided in libIgl
class IglUtils {

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
    template <int dim>
    static void addBlockToMatrix(const Eigen::Matrix<double, dim, dim*(dim + 1)>& block,
        const Eigen::Matrix<int, 1, dim + 1>& index, int rowIndI,
        LinSysSolver<Eigen::VectorXi, Eigen::VectorXd>* linSysSolver)
    {
        int rowStart = index[rowIndI] * dim;
        if (rowStart < 0) {
            rowStart = -rowStart - dim;
            linSysSolver->setCoeff(rowStart, rowStart, 1.0);
            linSysSolver->setCoeff(rowStart + 1, rowStart + 1, 1.0);
            if constexpr (dim == 3) {
                linSysSolver->setCoeff(rowStart + 2, rowStart + 2, 1.0);
            }
            return;
        }

        if (index[0] >= 0) {
            int _dimIndex0 = index[0] * dim;
            linSysSolver->addCoeff(rowStart, _dimIndex0, block(0, 0));
            linSysSolver->addCoeff(rowStart, _dimIndex0 + 1, block(0, 1));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex0, block(1, 0));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex0 + 1, block(1, 1));
            if constexpr (dim == 3) {
                linSysSolver->addCoeff(rowStart, _dimIndex0 + 2, block(0, 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex0 + 2, block(1, 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex0, block(2, 0));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex0 + 1, block(2, 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex0 + 2, block(2, 2));
            }
        }

        if (index[1] >= 0) {
            int _dimIndex1 = index[1] * dim;
            linSysSolver->addCoeff(rowStart, _dimIndex1, block(0, dim));
            linSysSolver->addCoeff(rowStart, _dimIndex1 + 1, block(0, dim + 1));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex1, block(1, dim));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex1 + 1, block(1, dim + 1));
            if constexpr (dim == 3) {
                linSysSolver->addCoeff(rowStart, _dimIndex1 + 2, block(0, dim + 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex1 + 2, block(1, dim + 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex1, block(2, dim));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex1 + 1, block(2, dim + 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex1 + 2, block(2, dim + 2));
            }
        }

        if (index[2] >= 0) {
            int _2dim = 2 * dim;
            int _dimIndex2 = index[2] * dim;
            linSysSolver->addCoeff(rowStart, _dimIndex2, block(0, _2dim));
            linSysSolver->addCoeff(rowStart, _dimIndex2 + 1, block(0, _2dim + 1));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex2, block(1, _2dim));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex2 + 1, block(1, _2dim + 1));
            if constexpr (dim == 3) {
                linSysSolver->addCoeff(rowStart, _dimIndex2 + 2, block(0, _2dim + 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex2 + 2, block(1, _2dim + 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex2, block(2, _2dim));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex2 + 1, block(2, _2dim + 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex2 + 2, block(2, _2dim + 2));
            }
        }

        if constexpr (dim == 3) {
            if (index[3] >= 0) {
                int _3dim = 3 * dim;
                int _dimIndex3 = index[3] * dim;
                linSysSolver->addCoeff(rowStart, _dimIndex3, block(0, _3dim));
                linSysSolver->addCoeff(rowStart, _dimIndex3 + 1, block(0, _3dim + 1));
                linSysSolver->addCoeff(rowStart, _dimIndex3 + 2, block(0, _3dim + 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex3, block(1, _3dim));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex3 + 1, block(1, _3dim + 1));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex3 + 2, block(1, _3dim + 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex3, block(2, _3dim));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex3 + 1, block(2, _3dim + 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex3 + 2, block(2, _3dim + 2));
            }
        }
    }

    // project a symmetric real matrix to the nearest SPD matrix
    template <typename Scalar, int size>
    static void makePD(Eigen::Matrix<Scalar, size, size>& symMtr)
    {
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, size, size>> eigenSolver(symMtr);
        if (eigenSolver.eigenvalues()[0] >= 0.0) {
            return;
        }
        Eigen::DiagonalMatrix<Scalar, size> D(eigenSolver.eigenvalues());
        int rows = ((size == Eigen::Dynamic) ? symMtr.rows() : size);
        for (int i = 0; i < rows; i++) {
            if (D.diagonal()[i] < 0.0) {
                D.diagonal()[i] = 0.0;
            }
            else {
                break;
            }
        }
        symMtr = eigenSolver.eigenvectors() * D * eigenSolver.eigenvectors().transpose();
    }
    template <typename Scalar, int size>
    static void makePD2d(Eigen::Matrix<Scalar, size, size>& symMtr)
    {
        // based on http://www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/

        if (size == Eigen::Dynamic) {
            assert(symMtr.rows() == 2);
        }
        else {
            assert(size == 2);
        }

        const double a = symMtr(0, 0);
        const double b = (symMtr(0, 1) + symMtr(1, 0)) / 2.0;
        const double d = symMtr(1, 1);

        double b2 = b * b;
        const double D = a * d - b2;
        const double T_div_2 = (a + d) / 2.0;
        const double sqrtTT4D = std::sqrt(T_div_2 * T_div_2 - D);
        const double L2 = T_div_2 - sqrtTT4D;
        if (L2 < 0.0) {
            const double L1 = T_div_2 + sqrtTT4D;
            if (L1 <= 0.0) {
                symMtr.setZero();
            }
            else {
                if (b2 == 0.0) {
                    symMtr << L1, 0.0, 0.0, 0.0;
                }
                else {
                    const double L1md = L1 - d;
                    const double L1md_div_L1 = L1md / L1;
                    symMtr(0, 0) = L1md_div_L1 * L1md;
                    symMtr(0, 1) = symMtr(1, 0) = b * L1md_div_L1;
                    symMtr(1, 1) = b2 / L1;
                }
            }
        }
    }
    template <typename Scalar, int size>
    static void flipDet_SVD(Eigen::Matrix<Scalar, size, size>& mtr)
    {
        Eigen::JacobiSVD<Eigen::Matrix<Scalar, size, size>> svd(mtr, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::Matrix<Scalar, size, size> U = svd.matrixU(), V = svd.matrixV();
        if (U.determinant() < 0) {
            U.col(U.cols() - 1) *= -1.0;
        }
        if (V.determinant() < 0) {
            V.col(V.cols() - 1) *= -1.0;
        }
        mtr = U * Eigen::DiagonalMatrix<Scalar, size>(svd.singularValues()) * V.transpose();
    }

    static void writeSparseMatrixToFile(const std::string& filePath,
        const Eigen::SparseMatrix<double>& mtr,
        bool MATLAB = false);
    static void writeSparseMatrixToFile(const std::string& filePath,
        const std::map<std::pair<int, int>, double>& mtr,
        bool MATLAB = false);
    static void writeSparseMatrixToFile(const std::string& filePath,
        LinSysSolver<Eigen::VectorXi, Eigen::VectorXd>* linSysSolver,
        bool MATLAB = false);
    static void loadSparseMatrixFromFile(const std::string& filePath,
        Eigen::SparseMatrix<double>& mtr);

    static void writeVectorToFile(const std::string& filePath,
        const Eigen::VectorXd& vec);
    static void readVectorFromFile(const std::string& filePath,
        Eigen::VectorXd& vec);

    // read segment mesh file
    static bool readSEG(const std::string& filePath, Eigen::MatrixXd& V, Eigen::MatrixXi& E);
    static void writeSEG(const std::string& filePath, const Eigen::MatrixXd& V, const Eigen::MatrixXi& E);

    static bool segTriIntersect(const Eigen::RowVector3d& ve0, const Eigen::RowVector3d& ve1,
        const Eigen::RowVector3d& vt0, const Eigen::RowVector3d& vt1, const Eigen::RowVector3d& vt2)
    {
        Eigen::Matrix3d coefMtr;
        coefMtr.col(0) = vt1 - vt0;
        coefMtr.col(1) = vt2 - vt0;
        coefMtr.col(2) = ve0 - ve1;

#ifdef USE_PREDICATES
        igl::predicates::exactinit();
        const auto ori1 = igl::predicates::orient3d(vt0, vt1, vt2, ve0);
        const auto ori2 = igl::predicates::orient3d(vt0, vt1, vt2, ve1);
        if (ori1 == igl::predicates::Orientation::COPLANAR || ori2 == igl::predicates::Orientation::COPLANAR) {
            // coplanar, we can detect it by d(EE)=0 or d(PT)=0
            return false;
        }

        if (ori1 == ori2) {
            // edge is on one side of the plane that triangle is in
            return false;
        }
#else
        Eigen::RowVector3d n = coefMtr.col(0).cross(coefMtr.col(1));
        if (n.dot(ve0 - vt0) * n.dot(ve1 - vt0) > 0.0) {
            return false; // edge is on one side of the plane that triangle is in
        }

        if (coefMtr.determinant() == 0.0) {
            return false; // coplanar, we can detect it by d(EE)=0 or d(PT)=0
        }
#endif

#ifdef USE_PREDICATES
        // int res = eccd::segment_triangle_inter(ve0, ve1, vt0, vt1, vt2);
        // if(res == 1){
        // std::cout << ve0 << std::endl;
        // std::cout << ve1 << std::endl;
        // std::cout << vt0 << std::endl;
        // std::cout << vt1 << std::endl;
        // std::cout << vt2 << std::endl;
        // exit(0);
        // }
        // return res == 1 ? true : false;
#endif
        Eigen::Vector3d uvt = coefMtr.fullPivLu().solve((ve0 - vt0).transpose());
        if (uvt[0] >= 0.0 && uvt[1] >= 0.0 && uvt[0] + uvt[1] <= 1.0 && uvt[2] >= 0.0 && uvt[2] <= 1.0) {
            return true;
        }
        else {
            return false;
        }
    }

    static bool
    pointBehindTri(
        const Eigen::RowVector3d& vt0, const Eigen::RowVector3d& vt1,
        const Eigen::RowVector3d& vt2, const Eigen::RowVector3d& v0)
    {
        return (vt1 - vt0).cross(vt2 - vt0).dot(v0 - vt0) <= 0.0;
    }

    static bool
    pointInsideTetrahedron(const Eigen::RowVector3d& v0,
        const Eigen::RowVector3d& vt0, const Eigen::RowVector3d& vt1,
        const Eigen::RowVector3d& vt2, const Eigen::RowVector3d& vt3)
    {
#ifdef USE_PREDICATES
        igl::predicates::exactinit();
        const auto ori1 = igl::predicates::orient3d(vt0, vt2, vt1, v0);
        if (ori1 != igl::predicates::Orientation::NEGATIVE) {
            const auto ori2 = igl::predicates::orient3d(vt0, vt3, vt2, v0);
            if (ori2 != igl::predicates::Orientation::NEGATIVE) {
                const auto ori3 = igl::predicates::orient3d(vt0, vt1, vt3, v0);
                if (ori3 != igl::predicates::Orientation::NEGATIVE) {
                    const auto ori4 = igl::predicates::orient3d(vt1, vt2, vt3, v0);
                    if (ori4 != igl::predicates::Orientation::NEGATIVE) {
                        return true;
                    }
                }
            }
        }
#else
        const bool ori1 = pointBehindTri(vt0, vt2, vt1, v0);
        if (ori1) {
            const bool ori2 = pointBehindTri(vt0, vt3, vt2, v0);
            if (ori2) {
                const bool ori3 = pointBehindTri(vt0, vt1, vt3, v0);
                if (ori3) {
                    const bool ori4 = pointBehindTri(vt1, vt2, vt3, v0);
                    if (ori4) {
                        return true;
                    }
                }
            }
        }
#endif
        return false;
    }

    static void findSurfaceTris(const Eigen::MatrixXi& TT, Eigen::MatrixXi& F);
    static void buildSTri2Tet(const Eigen::MatrixXi& F, const Eigen::MatrixXi& SF,
        std::vector<int>& sTri2Tet);

    static void saveTetMesh(const std::string& filePath,
        const Eigen::MatrixXd& TV, const Eigen::MatrixXi& TT,
        const Eigen::MatrixXi& F = Eigen::MatrixXi(),
        bool findSurface = true);
    static void saveTetMesh_msh4(const std::string& filePath,
        const Eigen::MatrixXd& TV, const Eigen::MatrixXi& TT,
        const Eigen::MatrixXi& F = Eigen::MatrixXi(),
        bool findSurface = true);
    static void saveTetMesh_vtk(const std::string& filePath,
        const Eigen::MatrixXd& TV, const Eigen::MatrixXi& TT);
    static bool readTetMesh(const std::string& filePath,
        Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
        Eigen::MatrixXi& F, bool findSurface = true);
    static bool readTetMesh_msh4(const std::string& filePath,
        Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
        Eigen::MatrixXi& F, bool findSurface = true);
    static void readNodeEle(const std::string& filePath,
        Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
        Eigen::MatrixXi& F);

    template <int colSize>
    static void dF_div_dx_mult(const Eigen::Matrix<double, DIM * DIM, colSize>& right,
        const Eigen::Matrix<double, DIM, DIM>& A,
        Eigen::Matrix<double, DIM*(DIM + 1), colSize>& result,
        bool symmetric)
    {
        if (colSize == Eigen::Dynamic) {
            assert(right.cols() > 0);
        }
        else {
            assert(colSize > 0);
        }
#if (DIM == 2)
        if (symmetric) {
            if (colSize == Eigen::Dynamic) {
                assert(right.cols() == 6);
            }
            else {
                assert(colSize == 6);
            }
            // int colI = 0;
            const double _0000 = right(0, 0) * A(0, 0);
            const double _0010 = right(0, 0) * A(1, 0);
            const double _1001 = right(1, 0) * A(0, 1);
            const double _1011 = right(1, 0) * A(1, 1);
            const double _2000 = right(2, 0) * A(0, 0);
            const double _2010 = right(2, 0) * A(1, 0);
            const double _3001 = right(3, 0) * A(0, 1);
            const double _3011 = right(3, 0) * A(1, 1);
            result(2, 0) = result(0, 2) = _0000 + _1001;
            result(3, 0) = result(0, 3) = _2000 + _3001;
            result(4, 0) = result(0, 4) = _0010 + _1011;
            result(5, 0) = result(0, 5) = _2010 + _3011;
            result(0, 0) = -result(2, 0) - result(4, 0);
            result(1, 0) = result(0, 1) = -result(3, 0) - result(5, 0);
            // colI = 1;
            const double _2100 = right(2, 1) * A(0, 0);
            const double _2110 = right(2, 1) * A(1, 0);
            const double _3101 = right(3, 1) * A(0, 1);
            const double _3111 = right(3, 1) * A(1, 1);
            result(2, 1) = result(1, 2) = right(0, 1) * A(0, 0) + right(1, 1) * A(0, 1);
            result(3, 1) = result(1, 3) = _2100 + _3101;
            result(4, 1) = result(1, 4) = right(0, 1) * A(1, 0) + right(1, 1) * A(1, 1);
            result(5, 1) = result(1, 5) = _2110 + _3111;
            result(1, 1) = -result(3, 1) - result(5, 1);
            // colI = 2;
            result(2, 2) = right(0, 2) * A(0, 0) + right(1, 2) * A(0, 1);
            result(3, 2) = result(2, 3) = right(2, 2) * A(0, 0) + right(3, 2) * A(0, 1);
            result(4, 2) = result(2, 4) = right(0, 2) * A(1, 0) + right(1, 2) * A(1, 1);
            result(5, 2) = result(2, 5) = right(2, 2) * A(1, 0) + right(3, 2) * A(1, 1);
            // colI = 3;
            result(3, 3) = right(2, 3) * A(0, 0) + right(3, 3) * A(0, 1);
            result(4, 3) = result(3, 4) = right(0, 3) * A(1, 0) + right(1, 3) * A(1, 1);
            result(5, 3) = result(3, 5) = right(2, 3) * A(1, 0) + right(3, 3) * A(1, 1);
            // colI = 4;
            result(4, 4) = right(0, 4) * A(1, 0) + right(1, 4) * A(1, 1),
                      result(5, 4) = result(4, 5) = right(2, 4) * A(1, 0) + right(3, 4) * A(1, 1);
            // colI = 5;
            result(5, 5) = right(2, 5) * A(1, 0) + right(3, 5) * A(1, 1);
        }
        else {
            for (int colI = 0; colI < right.cols(); colI++) {
                const double _000 = right(0, colI) * A(0, 0);
                const double _010 = right(0, colI) * A(1, 0);
                const double _101 = right(1, colI) * A(0, 1);
                const double _111 = right(1, colI) * A(1, 1);
                const double _200 = right(2, colI) * A(0, 0);
                const double _210 = right(2, colI) * A(1, 0);
                const double _301 = right(3, colI) * A(0, 1);
                const double _311 = right(3, colI) * A(1, 1);

                result(2, colI) = _000 + _101;
                result(3, colI) = _200 + _301;
                result(4, colI) = _010 + _111;
                result(5, colI) = _210 + _311;
                result(0, colI) = -result(2, colI) - result(4, colI);
                result(1, colI) = -result(3, colI) - result(5, colI);
            }
        }
#else
        for (int colI = 0; colI < right.cols(); colI++) {
            result(3, colI) = (A.row(0) * right.block(0, colI, DIM, 1))[0];
            result(4, colI) = (A.row(0) * right.block(DIM, colI, DIM, 1))[0];
            result(5, colI) = (A.row(0) * right.block(DIM * 2, colI, DIM, 1))[0];
            result(6, colI) = (A.row(1) * right.block(0, colI, DIM, 1))[0];
            result(7, colI) = (A.row(1) * right.block(DIM, colI, DIM, 1))[0];
            result(8, colI) = (A.row(1) * right.block(DIM * 2, colI, DIM, 1))[0];
            result(9, colI) = (A.row(2) * right.block(0, colI, DIM, 1))[0];
            result(10, colI) = (A.row(2) * right.block(DIM, colI, DIM, 1))[0];
            result(11, colI) = (A.row(2) * right.block(DIM * 2, colI, DIM, 1))[0];
            result(0, colI) = -result(3, colI) - result(6, colI) - result(9, colI);
            result(1, colI) = -result(4, colI) - result(7, colI) - result(10, colI);
            result(2, colI) = -result(5, colI) - result(8, colI) - result(11, colI);
        }
#endif
    }
    static void dF_div_dx_mult(const Eigen::Matrix<double, DIM, DIM>& right,
        const Eigen::Matrix<double, DIM, DIM>& A,
        Eigen::Matrix<double, DIM*(DIM + 1), 1>& result);
    template <int dim>
    static void computeCofactorMtr(const Eigen::Matrix<double, dim, dim>& F,
        Eigen::Matrix<double, dim, dim>& A)
    {
        switch (dim) {
        case 2:
            A(0, 0) = F(1, 1);
            A(0, 1) = -F(1, 0);
            A(1, 0) = -F(0, 1);
            A(1, 1) = F(0, 0);
            break;

        case 3:
            A(0, 0) = F(1, 1) * F(2, 2) - F(1, 2) * F(2, 1);
            A(0, 1) = F(1, 2) * F(2, 0) - F(1, 0) * F(2, 2);
            A(0, 2) = F(1, 0) * F(2, 1) - F(1, 1) * F(2, 0);
            A(1, 0) = F(0, 2) * F(2, 1) - F(0, 1) * F(2, 2);
            A(1, 1) = F(0, 0) * F(2, 2) - F(0, 2) * F(2, 0);
            A(1, 2) = F(0, 1) * F(2, 0) - F(0, 0) * F(2, 1);
            A(2, 0) = F(0, 1) * F(1, 2) - F(0, 2) * F(1, 1);
            A(2, 1) = F(0, 2) * F(1, 0) - F(0, 0) * F(1, 2);
            A(2, 2) = F(0, 0) * F(1, 1) - F(0, 1) * F(1, 0);
            break;

        default:
            assert(0 && "dim not 2 or 3");
            break;
        }
    }

    static void findBorderVerts(const Eigen::MatrixXd& V,
        std::vector<std::vector<int>>& borderVerts,
        double ratio);

    static void Init_Dirichlet(
        const Eigen::MatrixXd& X,
        const Eigen::Vector3d& relBoxMin,
        const Eigen::Vector3d& relBoxMax,
        const std::vector<bool>& isNodeOnBoundary,
        std::vector<int>& selectedVerts);
}; // namespace IPC

} // namespace IPC

#endif /* IglUtils_hpp */


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_ElasticBasicBivariateDistribution.hpp
//! \author Luke Kersting
//! \brief  The elastic basic bivariate dist. class decl.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_HPP
#define MONTE_CARLO_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_HPP

// Boost Includes
#include <boost/units/physical_dimensions/energy.hpp>

// FRENSIE Includes
#include "Utility_InterpolatedFullyTabularBasicBivariateDistribution.hpp"

namespace MonteCarlo{

/*! The unit-aware interpolated fully tabular two-dimensional distribution
 * \ingroup two_d_distribution
 */
template<typename TwoDGridPolicy,
         typename PrimaryIndependentUnit,
         typename SecondaryIndependentUnit,
         typename DependentUnit>
class UnitAwareElasticBasicBivariateDistribution : public Utility::UnitAwareInterpolatedFullyTabularBasicBivariateDistribution<TwoDGridPolicy,PrimaryIndependentUnit,SecondaryIndependentUnit,DependentUnit>
{
  // Only allow construction when the primary independent unit corresponds to energy
  RESTRICT_UNIT_TO_BOOST_DIMENSION( PrimaryIndependentUnit, energy_dimension );

  // The typedef for this type
  typedef UnitAwareElasticBasicBivariateDistribution<TwoDGridPolicy,PrimaryIndependentUnit,SecondaryIndependentUnit,DependentUnit> ThisType;

  // The parent distribution type
  typedef Utility::UnitAwareInterpolatedFullyTabularBasicBivariateDistribution<TwoDGridPolicy,PrimaryIndependentUnit,SecondaryIndependentUnit,DependentUnit> BaseType;

  // The base one-dimensional distribution type (UnitAwareTabularUnivariateDist)
  typedef typename BaseType::BaseUnivariateDistributionType BaseUnivariateDistributionType;

  // Typedef for QuantityTraits<double>
  typedef typename BaseType::QT QT;

  // Typedef for QuantityTraits<PrimaryIndepQuantity>
  typedef typename BaseType::PIQT PIQT;

  // Typedef for QuantityTraits<SecondaryIndepQuantity>
  typedef typename BaseType::SIQT SIQT;

  // Typedef for QuantityTriats<InverseSecondaryIndepQuantity>
  typedef typename BaseType::ISIQT ISIQT;

  // Typedef for QuantityTraits<DepQuantity>
  typedef typename BaseType::DQT DQT;

  // The primary independent variable processing tag
  typedef typename TwoDGridPolicy::TwoDInterpPolicy::FirstIndepVarProcessingTag FirstIndepVarProcessingTag;

  // The secondary independent quantity type
  typedef typename TwoDGridPolicy::TwoDInterpPolicy::SecondIndepVarProcessingTag SecondIndepVarProcessingTag;

  // The distribution data const iterator
  typedef typename BaseType::DistributionDataConstIterator DistributionDataConstIterator;

public:

  //! The primary independent quantity type
  typedef typename BaseType::PrimaryIndepQuantity PrimaryIndepQuantity;

  //! The secondary independent quantity type
  typedef typename BaseType::SecondaryIndepQuantity SecondaryIndepQuantity;

  //! The inverse secondary independent quantity type
  typedef typename BaseType::InverseSecondaryIndepQuantity InverseSecondaryIndepQuantity;

  //! The dependent quantity type
  typedef typename BaseType::DepQuantity DepQuantity;

  //! Constructor
  UnitAwareElasticBasicBivariateDistribution(
        const std::vector<PrimaryIndepQuantity>& primary_indep_grid,
        const std::vector<std::shared_ptr<const Utility::UnitAwareTabularUnivariateDistribution<SecondaryIndependentUnit,DependentUnit> > >& secondary_distributions,
        const SecondaryIndepQuantity upper_bound_conditional_indep_var = SIQT::one(),
        const double fuzzy_boundary_tol = 1e-7,
        const double evaluate_relative_error_tol = 1e-7,
        const double evaluate_error_tol = 1e-12 );

  //! Destructor
  ~UnitAwareElasticBasicBivariateDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  DepQuantity evaluate(
       const PrimaryIndepQuantity primary_indep_var_value,
       const SecondaryIndepQuantity secondary_indep_var_value ) const override;

  //! Evaluate the secondary conditional PDF
  InverseSecondaryIndepQuantity evaluateSecondaryConditionalPDF(
       const PrimaryIndepQuantity primary_indep_var_value,
       const SecondaryIndepQuantity secondary_indep_var_value ) const override;

  //! Evaluate the secondary conditional PDF
  InverseSecondaryIndepQuantity evaluateSecondaryConditionalPDF(
            const PrimaryIndepQuantity primary_indep_var_value,
            const SecondaryIndepQuantity secondary_indep_var_value,
            const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
            min_secondary_indep_var_functor,
            const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
            max_secondary_indep_var_functor ) const override;

  //! Evaluate the secondary conditional CDF
  double evaluateSecondaryConditionalCDF(
       const PrimaryIndepQuantity primary_indep_var_value,
       const SecondaryIndepQuantity secondary_indep_var_value ) const override;

  //! Return a random sample from the secondary conditional PDF
  SecondaryIndepQuantity sampleSecondaryConditional(
           const PrimaryIndepQuantity primary_indep_var_value ) const override;

  //! Return a random sample from the secondary conditional PDF
  SecondaryIndepQuantity sampleSecondaryConditional(
            const PrimaryIndepQuantity primary_indep_var_value,
            const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
            min_secondary_indep_var_functor,
            const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
            max_secondary_indep_var_functor ) const override;

  //! Return a random sample and record the number of trials
  SecondaryIndepQuantity sampleSecondaryConditionalAndRecordTrials(
            const PrimaryIndepQuantity primary_indep_var_value,
            Utility::DistributionTraits::Counter& trials ) const override;

// Return a random sample from the secondary conditional PDF and the index
  SecondaryIndepQuantity sampleSecondaryConditionalAndRecordBinIndices(
            const PrimaryIndepQuantity primary_indep_var_value,
            size_t& primary_bin_index,
            size_t& secondary_bin_index ) const override;

  //! Return a random sample from the secondary conditional PDF and the index
  SecondaryIndepQuantity sampleSecondaryConditionalAndRecordBinIndices(
            const PrimaryIndepQuantity primary_indep_var_value,
            SecondaryIndepQuantity& raw_sample,
            size_t& primary_bin_index,
            size_t& secondary_bin_index ) const override;

  //! Return a random sample from the secondary conditional PDF at the CDF val
  SecondaryIndepQuantity sampleSecondaryConditionalWithRandomNumber(
                const PrimaryIndepQuantity primary_indep_var_value,
                const double random_number ) const override;

  //! Return a random sample from the secondary conditional PDF in the subrange
  SecondaryIndepQuantity sampleSecondaryConditionalInSubrange(
   const PrimaryIndepQuantity primary_indep_var_value,
   const SecondaryIndepQuantity max_secondary_indep_var_value ) const override;

  //! Return a random sample from the secondary conditional PDF in the subrange
  SecondaryIndepQuantity sampleSecondaryConditionalWithRandomNumberInSubrange(
   const PrimaryIndepQuantity primary_indep_var_value,
   const double random_number,
   const SecondaryIndepQuantity max_secondary_indep_var_value ) const override;

  //! Return the lower bound of the conditional distribution
  SecondaryIndepQuantity getLowerBoundOfSecondaryConditionalIndepVar(
           const PrimaryIndepQuantity primary_indep_var_value ) const override;

  //! Return the upper bound of the conditional distribution
  SecondaryIndepQuantity getUpperBoundOfSecondaryConditionalIndepVar(
           const PrimaryIndepQuantity primary_indep_var_value ) const override;

  //! Method for placing the object in an output stream
  void toStream( std::ostream& os ) const override;

private:

  //! Default constructor
  UnitAwareElasticBasicBivariateDistribution();

  //! Evaluate the distribution using the desired evaluation method
  template<typename ReturnType,
           typename EvaluationMethod>
  ReturnType evaluateImpl(
                    const PrimaryIndepQuantity incoming_energy,
                    const SecondaryIndepQuantity angle_cosine,
                    EvaluationMethod evaluate ) const;

  //! Evaluate the distribution using the desired evaluation method
  template<typename ReturnType,
           typename EvaluationMethod>
  ReturnType evaluateImpl(
    const PrimaryIndepQuantity incoming_energy,
    const SecondaryIndepQuantity angle_cosine,
    const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
      min_secondary_indep_var_functor,
    const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
      max_secondary_indep_var_functor,
    EvaluationMethod evaluate,
    unsigned max_number_of_iterations = 500 ) const;

  //! Evaluate the distribution using the desired evaluation method
  template<typename EvaluationMethod>
  double evaluateCDFImpl(
                    const PrimaryIndepQuantity incoming_energy,
                    const SecondaryIndepQuantity angle_cosine,
                    EvaluationMethod evaluateCDF ) const;

  //! Evaluate the distribution using the desired evaluation method
  template<typename EvaluationMethod>
  double evaluateCDFImpl(
    const PrimaryIndepQuantity incoming_energy,
    const SecondaryIndepQuantity angle_cosine,
    const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
      min_secondary_indep_var_functor,
    const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
      max_secondary_indep_var_functor,
    EvaluationMethod evaluateCDF,
    unsigned max_number_of_iterations = 500 ) const;

  //! Sample from the distribution using the desired sampling functor
  template<typename SampleFunctor>
  SecondaryIndepQuantity sampleDetailedImpl(
                        const PrimaryIndepQuantity primary_indep_var_value,
                        SampleFunctor sample_functor,
                        SecondaryIndepQuantity& raw_sample,
                        size_t& primary_bin_index ) const;

  //! Sample from the distribution using the desired sampling functor
  template<typename SampleFunctor>
  SecondaryIndepQuantity sampleImpl(
                        const PrimaryIndepQuantity primary_indep_var_value,
                        SampleFunctor sample_functor ) const;

  //! Verify that the second independent variable processing type is compatible with Cosine processing
  static void verifyValidSecondIndepVarProcessingType();

  // Save the distribution to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const;

  // Load the distribution from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version );

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;

  // The max upper bound of the conditional distribution ( 1.0 )
  SecondaryIndepQuantity d_max_upper_bound_conditional_indep_var;

  // The upper bound of the conditional distribution ( -1.0 < mu_cut <= 1.0 )
  SecondaryIndepQuantity d_upper_bound_conditional_indep_var;

  // The lower bound of the conditional distribution ( -1.0 )
  SecondaryIndepQuantity d_lower_bound_conditional_indep_var;
};

/*! \brief The interpolated fully tabular two-dimensional distribution
 * (unit-agnostic)
 * \ingroup two_d_distributions
 */
template<typename TwoDGridPolicy> using ElasticBasicBivariateDistribution =
  UnitAwareElasticBasicBivariateDistribution<TwoDGridPolicy,void,void,void>;

} // end MonteCarlo namespace

BOOST_SERIALIZATION_CLASS4_VERSION( UnitAwareElasticBasicBivariateDistribution, MonteCarlo, 0 );

#define BOOST_SERIALIZATION_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_EXPORT_STANDARD_KEY() \
  BOOST_SERIALIZATION_CLASS4_EXPORT_STANDARD_KEY( UnitAwareElasticBasicBivariateDistribution, MonteCarlo ) \
  BOOST_SERIALIZATION_TEMPLATE_CLASS_EXPORT_KEY_IMPL(                   \
    UnitAwareElasticBasicBivariateDistribution, MonteCarlo, \
    __BOOST_SERIALIZATION_FORWARD_AS_SINGLE_ARG__( std::string( "ElasticBasicBivariateDistribution<" ) + Utility::typeName<GridPolicy>() + ">" ), \
    __BOOST_SERIALIZATION_FORWARD_AS_SINGLE_ARG__( typename GridPolicy ), \
    __BOOST_SERIALIZATION_FORWARD_AS_SINGLE_ARG__( GridPolicy, void, void, void ) )

BOOST_SERIALIZATION_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_EXPORT_STANDARD_KEY();

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "MonteCarlo_ElasticBasicBivariateDistribution_def.hpp"

//---------------------------------------------------------------------------//

#endif // end MONTE_CARLO_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ElasticBasicBivariateDistribution.hpp
//---------------------------------------------------------------------------//