#ifndef BOOST_GEOMETRY_PROJECTIONS_NATEARTH_HPP
#define BOOST_GEOMETRY_PROJECTIONS_NATEARTH_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// The Natural Earth projection was designed by Tom Patterson, US National Park
// Service, in 2007, using Flex Projector. The shape of the original projection
// was defined at every 5 degrees and piece-wise cubic spline interpolation was
// used to compute the complete graticule.
// The code here uses polynomial functions instead of cubic splines and
// is therefore much simpler to program. The polynomial approximation was
// developed by Bojan Savric, in collaboration with Tom Patterson and Bernhard
// Jenny, Institute of Cartography, ETH Zurich. It slightly deviates from
// Patterson's original projection by adding additional curvature to meridians
// where they meet the horizontal pole line. This improvement is by intention
// and designed in collaboration with Tom Patterson.
// Port to PROJ.4 by Bernhard Jenny, 6 June 2011

// 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 conditions:

// 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.

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct natearth {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace natearth
    {

            static const double A0 = 0.8707;
            static const double A1 = -0.131979;
            static const double A2 = -0.013791;
            static const double A3 = 0.003971;
            static const double A4 = -0.001529;
            static const double B0 = 1.007226;
            static const double B1 = 0.015085;
            static const double B2 = -0.044475;
            static const double B3 = 0.028874;
            static const double B4 = -0.005916;
            static const double C0 = B0;
            static const double C1 = (3 * B1);
            static const double C2 = (7 * B2);
            static const double C3 = (9 * B3);
            static const double C4 = (11 * B4);
            static const double EPS = 1e-11;
            //static const double MAX_Y = (0.8707 * 0.52 * geometry::math::pi<double>());

            template <typename T>
            inline T MAX_Y() { return (0.8707 * 0.52 * detail::ONEPI<T>()); }

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_natearth_spheroid : public base_t_fi<base_natearth_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                inline base_natearth_spheroid(const Parameters& par)
                    : base_t_fi<base_natearth_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    CalculationType phi2, phi4;

                    phi2 = lp_lat * lp_lat;
                    phi4 = phi2 * phi2;
                    xy_x = lp_lon * (A0 + phi2 * (A1 + phi2 * (A2 + phi4 * phi2 * (A3 + phi2 * A4))));
                    xy_y = lp_lat * (B0 + phi2 * (B1 + phi4 * (B2 + B3 * phi2 + B4 * phi4)));
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    static const CalculationType MAX_Y = natearth::MAX_Y<CalculationType>();

                    CalculationType yc, tol, y2, y4, f, fder;

                    /* make sure y is inside valid range */
                    if (xy_y > MAX_Y) {
                        xy_y = MAX_Y;
                    } else if (xy_y < -MAX_Y) {
                        xy_y = -MAX_Y;
                    }

                    /* latitude */
                    yc = xy_y;
                        for (;;) { /* Newton-Raphson */
                        y2 = yc * yc;
                        y4 = y2 * y2;
                        f = (yc * (B0 + y2 * (B1 + y4 * (B2 + B3 * y2 + B4 * y4)))) - xy_y;
                        fder = C0 + y2 * (C1 + y4 * (C2 + C3 * y2 + C4 * y4));
                        yc -= tol = f / fder;
                        if (fabs(tol) < EPS) {
                            break;
                        }
                    }
                    lp_lat = yc;

                    /* longitude */
                    y2 = yc * yc;
                    lp_lon = xy_x / (A0 + y2 * (A1 + y2 * (A2 + y2 * y2 * y2 * (A3 + y2 * A4))));
                }

                static inline std::string get_name()
                {
                    return "natearth_spheroid";
                }

            };

            // Natural Earth
            template <typename Parameters>
            inline void setup_natearth(Parameters& par)
            {
                par.es = 0;
            }

    }} // namespace detail::natearth
    #endif // doxygen

    /*!
        \brief Natural Earth projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_natearth.gif
    */
    template <typename CalculationType, typename Parameters>
    struct natearth_spheroid : public detail::natearth::base_natearth_spheroid<CalculationType, Parameters>
    {
        inline natearth_spheroid(const Parameters& par) : detail::natearth::base_natearth_spheroid<CalculationType, Parameters>(par)
        {
            detail::natearth::setup_natearth(this->m_par);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::natearth, natearth_spheroid, natearth_spheroid)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class natearth_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<natearth_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void natearth_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("natearth", new natearth_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_NATEARTH_HPP


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef MINIMIZER_H
#define MINIMIZER_H

#include <iostream>
#include <cassert>
#include <utility>
#include <Eigen/Core>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>

#include "error.hpp"
#include "ewsb_solver.hpp"
#include "logger.hpp"
#include "gsl_utils.hpp"
#include "gsl_vector.hpp"

namespace flexiblesusy {

/**
 * @class Minimizer
 * @brief Function minimizer
 *
 * The user has to provide the function to be minimized of the type
 * Function_t.  This function gets as arguments an Eigen vector of
 * lenght `dimension' and returns a double.
 *
 * Example:
 * @code
 * auto parabola = [](const Eigen::Matrix<double,2,1>& x) -> double {
 *    const double y = x(0);
 *    const double z = x(1);
 *    return Sqr(y - 5.0) + Sqr(z - 1.0);
 * };
 *
 * Minimizer<2> minimizer(parabola, 100, 1.0e-5);
 * const double start[2] = { 10, 10 };
 * const int status = minimizer.minimize(start);
 * @endcode
 */
template <std::size_t dimension>
class Minimizer : public EWSB_solver {
public:
   using Vector_t = Eigen::Matrix<double,dimension,1>;
   using Function_t = std::function<double(const Vector_t&)>;
   enum Solver_type { GSLSimplex, GSLSimplex2, GSLSimplex2Rand };

   Minimizer() = default;
   template <typename F>
   Minimizer(F&&, std::size_t, double, Solver_type solver_type_ = GSLSimplex2);
   virtual ~Minimizer() = default;

   double get_minimum_value() const { return minimum_value; }
   template <typename F>
   void set_function(F&& f) { function = std::forward<F>(f); }
   void set_precision(double p) { precision = p; }
   void set_max_iterations(std::size_t n) { max_iterations = n; }
   void set_solver_type(Solver_type t) { solver_type = t; }
   int minimize(const Vector_t&);

   // EWSB_solver interface methods
   virtual std::string name() const override { return "Minimizer"; }
   virtual int solve(const Eigen::VectorXd&) override;
   virtual Eigen::VectorXd get_solution() const override { return minimum_point; }

private:
   std::size_t max_iterations{100};     ///< maximum number of iterations
   double precision{1.e-2};             ///< precision goal
   double minimum_value{0.};            ///< minimum function value found
   Vector_t minimum_point{Vector_t::Zero()}; ///< vector of minimum point
   Function_t function{nullptr};        ///< function to minimize
   Solver_type solver_type{GSLSimplex2};///< solver type

   void print_state(gsl_multimin_fminimizer*, std::size_t) const;
   static double gsl_function(const gsl_vector*, void*);
   const gsl_multimin_fminimizer_type* solver_type_to_gsl_pointer() const;
};

/**
 * Constructor
 *
 * @param function_ pointer to the function to minimize
 * @param max_iterations_ maximum number of iterations
 * @param precision_ precision goal
 * @param solver_type_ GSL multimin minimizer type
 */
template <std::size_t dimension>
template <typename F>
Minimizer<dimension>::Minimizer(
   F&& function_,
   std::size_t max_iterations_,
   double precision_,
   Solver_type solver_type_
)
   : max_iterations(max_iterations_)
   , precision(precision_)
   , function(std::forward<F>(function_))
   , solver_type(solver_type_)
{
}

/**
 * Start the minimization
 *
 * @param start starting point
 *
 * @return GSL error code (GSL_SUCCESS if minimum found)
 */
template <std::size_t dimension>
int Minimizer<dimension>::minimize(const Vector_t& start)
{
   if (!function)
      throw SetupError("Minimizer: function not callable");

   gsl_multimin_fminimizer *minimizer;
   gsl_multimin_function minex_func;

   GSL_vector min_point = to_GSL_vector(start);

   // Set initial step sizes
   GSL_vector step_size(dimension);
   step_size.set_all(1.0);

   // Initialize method and iterate
   minex_func.n = dimension;
   minex_func.f = gsl_function;
   minex_func.params = &function;

   minimizer = gsl_multimin_fminimizer_alloc(solver_type_to_gsl_pointer(), dimension);
   gsl_multimin_fminimizer_set(minimizer, &minex_func, min_point.raw(), step_size.raw());

   size_t iter = 0;
   int status;

   do {
      iter++;
      status = gsl_multimin_fminimizer_iterate(minimizer);

      if (status)
         break;

      const double size = gsl_multimin_fminimizer_size(minimizer);
      status = gsl_multimin_test_size(size, precision);

#ifdef ENABLE_VERBOSE
      print_state(minimizer, iter);
#endif
   } while (status == GSL_CONTINUE && iter < max_iterations);

   VERBOSE_MSG("\t\t\tMinimization status = " << gsl_strerror(status));

   // save minimum point and function value
   minimum_point = to_eigen_vector_fixed<dimension>(minimizer->x);
   minimum_value = minimizer->fval;

   gsl_multimin_fminimizer_free(minimizer);

   return status;
}

/**
 * Print state of the minimizer
 *
 * @param minimizer minimizer
 * @param iteration iteration number
 */
template <std::size_t dimension>
void Minimizer<dimension>::print_state(gsl_multimin_fminimizer* minimizer,
                                               std::size_t iteration) const
{
   VERBOSE_MSG("\t\t\tIteration " << iteration
               << ": x = " << GSL_vector(minimizer->x)
               << ", f(x) = " << minimizer->fval);
}

template <std::size_t dimension>
int Minimizer<dimension>::solve(const Eigen::VectorXd& start)
{
   return (minimize(start) == GSL_SUCCESS ?
           EWSB_solver::SUCCESS : EWSB_solver::FAIL);
}

template <std::size_t dimension>
double Minimizer<dimension>::gsl_function(const gsl_vector* x, void* params)
{
   if (!is_finite(x))
      return std::numeric_limits<double>::max();

   Function_t* fun = static_cast<Function_t*>(params);
   const Vector_t arg(to_eigen_vector_fixed<dimension>(x));
   double result = std::numeric_limits<double>::max();

   try {
      result = (*fun)(arg);
   } catch (const flexiblesusy::Error&) {
   }

   return result;
}

template <std::size_t dimension>
const gsl_multimin_fminimizer_type* Minimizer<dimension>::solver_type_to_gsl_pointer() const
{
   switch (solver_type) {
   case GSLSimplex     : return gsl_multimin_fminimizer_nmsimplex;
   case GSLSimplex2    : return gsl_multimin_fminimizer_nmsimplex2;
   case GSLSimplex2Rand: return gsl_multimin_fminimizer_nmsimplex2rand;
   default:
      throw SetupError("Unknown minimizer solver type: "
                       + std::to_string(solver_type));
   }

   return nullptr;
}

} // namespace flexiblesusy

#endif


// morpho/cdl/bgl_expansions/algorithms/ullmann.hpp header file//
// Copyright (c) 2003-2008 Vladimir J. Sykora
// Copyright (c) 2007-2008 Vladimir J. Sykora and NCU Studies Ltd
// Modifications by Greg Landrum, January 2009
//
//*****************************************************************************
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

//*****************************************************************************

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

// Algorithm due to JR Ullmann. "An Algorithm for Subgraph
// Isomorphism". Journal of the Association for
// Computing Machinery, Vol 23, No.1, January 1976, pp 31-42.

#ifndef MORPHO_CDL_BGL_EXP_ULLMANN_HPP
#define MORPHO_CDL_BGL_EXP_ULLMANN_HPP

// -------- boost
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/graph/graph_traits.hpp>
// -------- std
#include <utility>    // for std::pair<>
#include <algorithm>

namespace boost {
  namespace detail {

    template <class Graph, class UblasMatrix, class BackInsertionSequence, class EdgeLabeling>
    bool forward_checking(const Graph& g1, const Graph& g2,
                          UblasMatrix& M, size_t count, BackInsertionSequence& F, size_t num_vert_g1,
                          size_t num_vert_g2, EdgeLabeling& edge_labeling){
      typedef std::pair<typename graph_traits<Graph>::edge_descriptor,bool> edge_presence;
      typename BackInsertionSequence::iterator   fi, fi_end=F.end();
      for (size_t k=count+1; k<num_vert_g1; ++k) {
        for (size_t l=0; l<num_vert_g2; ++l) {
          if(!M(k,l)) continue;
          // check mapping:
          fi=F.begin();
          while(fi!=fi_end) {
            bool flag1(0), flag1_1(0), flag2(0), flag2_1(0);
            edge_presence ep1=edge(k,fi->first,g1);
            if(ep1.second) {
              flag1_1=true;
              edge_presence ep2=edge(l,fi->second,g2);
              if(ep2.second) flag1=edge_labeling(ep1.first,ep2.first);
            }
            if(flag1_1 && flag1) {
              M(k,l)=1;
              ++fi;
              continue;
            }
            edge_presence ep2=edge(l,fi->second,g2);
            if(ep2.second) {
              flag2_1=true;
              ep1=edge(k,fi->first,g1);
              if(ep1.second){
                flag2=edge_labeling(ep1.first,ep2.first);
              } else {
                // ring closed in main structure, not closed in query. This should pass
                flag2=true;
              }
            }
            if(flag2_1 && flag2) {  //if one edge exists, there must be a mapping
              M(k,l)=1;
            }
            else if ( !flag1_1 && !flag2_1 ) { // or both edges are not present
              M(k,l)=1;
            }
            else M(k,l)=0;         // if not, there's no mapping
            ++fi;
          }
        }
      }
      // TODO: change the data structure of the M matrix to sparse matrix. This wouldn't be necessary
      size_t cero_row(0);
      for (size_t k=0; k<num_vert_g1; ++k) {
        for (size_t l=0; l<num_vert_g2; ++l) {
          if(M(k,l)) break;
          else ++cero_row;
        }
        if(cero_row==num_vert_g2) return false; // if there is a cero row
        cero_row=0;
      }
      return true;
    }

    template <class Graph, class EdgeLabeling, class UblasMatrix, class BackInsertionSequence>
    bool backtrack(const Graph& g1, const Graph& g2, size_t count,
                   const UblasMatrix& M, BackInsertionSequence& F, const size_t num_vert_g1,
                   const size_t num_vert_g2, EdgeLabeling& edge_labeling){
      if(count==num_vert_g1) return true;
      for (size_t i=0; i<num_vert_g2; ++i) {
        if(M(count,i)) {
          F.push_back(std::make_pair(count,i));
          UblasMatrix M_prime(M);
          for (size_t m=count+1; m<num_vert_g1; ++m) {
            M_prime(m,i)=0;
          }

          if (forward_checking(g1,g2,M_prime,count,F,num_vert_g1,num_vert_g2,edge_labeling)){
            if(backtrack(g1,g2,count+1,M_prime,F,num_vert_g1,num_vert_g2,edge_labeling)){
              return true;
            }
          }
          F.erase(std::remove(F.begin(),F.end(),std::make_pair(count,i)), F.end());
        }
      }
      return false;
    }

    template <class Graph, class EdgeLabeling, class UblasMatrix, class DoubleBackInsertionSequence>
    void backtrack_all(const Graph& g1, const Graph& g2, size_t count,
                       const UblasMatrix& M, DoubleBackInsertionSequence& FF, const size_t num_vert_g1,
                       const size_t num_vert_g2, EdgeLabeling& edge_labeling) {
      if(count==num_vert_g1) return;
      DoubleBackInsertionSequence holdFF;
      holdFF.insert(holdFF.begin(),FF.begin(),FF.end());
      FF.clear();
      for (size_t i=0; i<num_vert_g2; ++i) {
        if(M(count,i)) {
          DoubleBackInsertionSequence tFF;

          UblasMatrix M_prime(M);
          for (size_t m=count+1; m<num_vert_g1; ++m){
            M_prime(m,i)=0;
          }

          if(holdFF.size()){
            for(typename DoubleBackInsertionSequence::const_iterator iter=holdFF.begin();
                iter!=holdFF.end();++iter){
              typename DoubleBackInsertionSequence::value_type F=*iter;
              F.push_back(std::make_pair(count,i));
              if (forward_checking(g1,g2,M_prime,count,F,num_vert_g1,num_vert_g2,edge_labeling)){
                tFF.push_back(F);
              }
            }
          } else {
            typename DoubleBackInsertionSequence::value_type F;
            F.push_back(std::make_pair(count,i));
            if (forward_checking(g1,g2,M_prime,count,F,num_vert_g1,num_vert_g2,edge_labeling)){
              tFF.push_back(F);
            }
          }
          backtrack_all(g1,g2,count+1,M_prime,tFF,num_vert_g1,num_vert_g2,edge_labeling);
          if(tFF.size()){
            for(typename DoubleBackInsertionSequence::const_iterator iter=tFF.begin();
                iter!=tFF.end();++iter){
              FF.push_back(*iter);
            }
          }
        }
      }
    }

    template <  class Graph
                , class VertexLabeling    // binary predicate
                ,  class UblasMatrix
                >
    void prepareM(const Graph& g1, const Graph& g2,
                  VertexLabeling& vertex_labeling,UblasMatrix &M){
      size_t rows(num_vertices(g1));
      size_t cols(num_vertices(g2));
      M.resize(rows,cols);
      // initialize the matrix:
      for (size_t i=0; i<rows; ++i) {
        for (size_t j=0; j<cols; ++j) {
          if(out_degree(j,g2)>=out_degree(i,g1) && vertex_labeling(i,j)) {
            M(i,j)=1;
          }
          else M(i,j)=0;
        }
      }
    }
  }  // namespace detail

  // test if g1 is a subgraph of g2. mapped vertices are returned in F
  // Mapping : first: g1 vertices, second : g2 vertices
  // O( num_vertices(g1)! num_vertices(g1) ^ 3 )
  // This function doesnt Work with filtered graphs!
  // The size of F doesn't indicate match!
  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class BackInsertionSequence   // contains std::pair<vertex_descriptor,vertex_descriptor>
              >
  bool ullmann(const Graph& g1, const Graph& g2,
               VertexLabeling& vertex_labeling, EdgeLabeling& edge_labeling, BackInsertionSequence& F) {
    typedef ::boost::numeric::ublas::matrix<int>   matrix_t;
    size_t rows(num_vertices(g1));
    size_t cols(num_vertices(g2));
    matrix_t M;
    detail::prepareM(g1,g2,vertex_labeling,M);
    size_t    count(0);
    return detail::backtrack(g1,g2,count,M,F,rows,cols,edge_labeling);
  }

  // test if g1 is a subgraph of g2.
  // F returns all mappings of g1 in g2. mapping in separate containers
  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class DoubleBackInsertionSequence   // contains a back insertion sequence
              >
  bool ullmann_all(const Graph& g1, const Graph& g2,
                   VertexLabeling& vertex_labeling, EdgeLabeling& edge_labeling, DoubleBackInsertionSequence& F) {
    typedef ::boost::numeric::ublas::matrix<int>   matrix_t;
    size_t rows(num_vertices(g1));
    size_t cols(num_vertices(g2));
    matrix_t M;
    detail::prepareM(g1,g2,vertex_labeling,M);
    size_t    count(0);
    detail::backtrack_all(g1,g2,count,M,F,rows,cols,edge_labeling);
    return !F.empty();
  }

}  // namespace boost

#endif // MORPHO_CDL_BGL_EXP_ULLMANN_HPP


/****************************************************************************
Copyright (c) 2019 Cognitics, Inc.

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 conditions:

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.
****************************************************************************/
//#pragma optimize( "", off )
#include <boost/foreach.hpp>
#include "sfa/Point.h"
#include "lmfit/lmmin.h"
#include "sfa/PlaneFitter.h"

//#define PLANEFIT_DEBUG_OUTPUT

namespace sfa
{
    struct PlaneFitter::PlaneFitterData
    {
        lm_control_struct   control_struct;
        lm_status_struct    status_struct;
        Point plane[3];
        PointList points;
        std::vector<bool> constrainedIndices;
        bool forceBelow;
        typedef std::pair<int, int> intpair_t;
        //Pairs of indices into the point list
        //each pair should be the same Z (to keep roads flat for instance)
        std::vector<intpair_t> samePoints;

        //Interpolate Z from a plane
        bool interpolatePointInPlane(sfa::Point *pt, double &z)
        {
            plane[0].setZ(plane[0].Z() + plane[0].M());
            plane[1].setZ(plane[1].Z() + plane[1].M());
            plane[2].setZ(plane[2].Z() + plane[2].M());
            sfa::Point P = *pt - plane[0];
            sfa::Point U = plane[1] - plane[0];
            sfa::Point V = plane[2] - plane[0];
            double denom = V.X() * U.Y() - V.Y() * U.X();
            double v = (P.X() * U.Y() - P.Y() * U.X()) / denom;
            double u = (P.Y() * V.X() - P.X() * V.Y()) / denom;
            z = (plane[0].Z() + (u * U.Z()) + (v * V.Z()));
            if (z != z)
                return false;
            return true;

        }
    };

    void PlaneFitter::constrainPair(int a, int b)
    {
        intpair_t ipair;
        ipair.first = a;
        ipair.second = b;
        data->samePoints.push_back(ipair);
    }
    void PlaneFitter::constrainPoint(size_t idx)
    {
        data->constrainedIndices[idx] = true;
    }

    PlaneFitter::~PlaneFitter()
    {
        delete data;
    }

    PlaneFitter::PlaneFitter(const sfa::PointList points, bool forceBelowPoints)
    {
        log.init("PlaneFitter", this);
        validFit = false;
        data = new PlaneFitterData;
        data->control_struct = lm_control_double;
        //data->control_struct.printflags = 3;
        data->points = points;
        data->forceBelow = forceBelowPoints;
        data->constrainedIndices.resize(points.size());
        for (size_t i = 0, ic = points.size(); i < ic; i++)
        {
            data->constrainedIndices[i] = false;
        }
        if (points.size() > 0)
        {
            //Use the first point as the origin, and two other points to form a right triangle as the plane's basis
            data->plane[0] = *points.at(0);
            data->plane[0].clearM();
            data->plane[1] = data->plane[0];
            data->plane[1].setX(data->plane[1].X() + 10);
            data->plane[2] = data->plane[0];
            data->plane[2].setY(data->plane[0].Y() + 10);
        }
    }

    void PlaneFitter::setInitialBasis(sfa::Point pt, int idx)
    {
        if (idx < 0 || idx > 2)
            return;
        data->plane[idx] = pt;
    }

    bool PlaneFitter::Fit()
    {
        double par[3];
        if (data->points.size() < 3)
            return true;

        // Use the first point's Z as the initial guess of a flat plane
        par[0] = data->plane[0].Z();
        par[1] = data->plane[1].Z();
        par[2] = data->plane[2].Z();

        data->status_struct.outcome = 0;
        data->control_struct.patience = 100;

        //the three parameters are our three Z values for the plane
        lmmin(3, par, data->points.size(), (void*)data, PlaneFitter::Callback, &data->control_struct, &data->status_struct);

        //If we ran out of patience, use the average of the constrained points.
        if (data->status_struct.outcome == 5)
        {
            int num_constrained = 0;
            double constrained_sum = 0;
            for (size_t i = 0, ic = data->points.size(); i < ic; i++)
            {
                if (data->constrainedIndices[i])
                {
                    num_constrained++;
                    constrained_sum += data->points[i]->Z();
                }
            }
            if (num_constrained)
            {
                double constrained_avg = constrained_sum/num_constrained;

#ifdef PLANEFIT_DEBUG_OUTPUT
                log << ccl::LERR << "Too many constraints (" << num_constrained << "), ran out of patience. Using average of (" << constrained_avg << ") contraints Z to create a flat plane" << log.endl;
                for (size_t i = 0, ic = data->points.size(); i < ic; i++)
                {
                    bool constrained = data->constrainedIndices.at(i);

                    log << ccl::LERR << "\tPoint: " << data->points[i]->X() << " , " << data->points[i]->Y() << " , " << data->points[i]->Z() << " , " << data->points[i]->M();
                    if (constrained)
                        log << " (C)";
                    log << log.endl;
                }
                log << "\tPlane" << log.endl;
                for (size_t i = 0, ic = 3; i < ic; i++)
                {
                    log << ccl::LERR << "\t\tPoint: " << data->plane[i].X() << " , " << data->plane[i].Y() << " , " << data->plane[i].Z() << " , " << data->plane[i].M() << log.endl;
                }
#endif
                data->plane[0].setZ(constrained_avg);
                data->plane[1].setZ(constrained_avg);
                data->plane[2].setZ(constrained_avg);
                validFit = true;
                return true;
            }
            else
            {
#ifdef PLANEFIT_DEBUG_OUTPUT
                log << ccl::LERR << "Ran out of patience fitting a plane for " << log.endl;
                for (size_t i = 0, ic = data->points.size(); i < ic; i++)
                {
                    log << ccl::LERR << "\tPoint: " << data->points[i]->X() << " , " << data->points[i]->Y() << " , " << data->points[i]->Z() << " , " << data->points[i]->M() << log.endl;
                }
                log << "\tPlane" << log.endl;
                for (size_t i = 0, ic = 3; i < ic; i++)
                {
                    log << ccl::LERR << "\t\tPoint: " << data->plane[i].X() << " , " << data->plane[i].Y() << " , " << data->plane[i].Z() << " , " << data->plane[i].M() << log.endl;
                }
#endif
            }

        }

        if (data->status_struct.outcome!=5  && data->status_struct.outcome > 3)
        {
            validFit = false;
            if (data->status_struct.outcome > 12 || data->status_struct.outcome < 0)
                log << ccl::LERR << "Unable to fit plane. lmmin returns an invalid info message" << log.endl;
            else
                log << ccl::LERR << "Unable to fit plane. lmmin returns " << lm_infmsg[data->status_struct.outcome] << log.endl;
            return false;
        }
        validFit = true;
        return true;
    }

    void PlaneFitter::Callback(const double *par, int m_dat, const void *data, double *fvec, int *info)
    {
        PlaneFitterData* mfd = (PlaneFitterData*)(data);
        mfd->plane[0].setZ(par[0]);
        mfd->plane[1].setZ(par[1]);
        mfd->plane[2].setZ(par[2]);
        double *deltas = new double[mfd->points.size()];
        size_t num_points = mfd->points.size();
        for (size_t i = 0; i < num_points; i++)
        {
            sfa::Point *pt = mfd->points.at(i);
            double z = 0;
            if (mfd->interpolatePointInPlane(pt, z))
            {
                //printf("idx=%d error=%f\n", i, pt->Z() - z);
                double delta = abs(pt->Z() - z);
                deltas[i] = delta;
                fvec[i] = exp(delta);
                if (mfd->forceBelow && (z > pt->Z()))
                    fvec[i] = fvec[i] * mfd->points.size();
                if (mfd->constrainedIndices[i])
                {
                    fvec[i] = fvec[i] * mfd->points.size();//Constraints scale proportionally to the number of points
                }
            }
        }
        for (int j = 0, jc = mfd->samePoints.size(); j < jc; j++)
        {
            int idxa = mfd->samePoints[j].first;
            int idxb = mfd->samePoints[j].second;
            if (idxa < mfd->points.size())
            {
                if (idxb < mfd->points.size())
                {
                    double pairdelta = abs(deltas[idxa] - deltas[idxb]);
                    fvec[idxa] += ((1 + pairdelta) * (1 + pairdelta))* mfd->points.size();
                    fvec[idxb] += ((1 + pairdelta) * (1 + pairdelta))* mfd->points.size();
                }
            }
        }

        delete[] deltas;

    }

    bool PlaneFitter::getPlane(sfa::Point &p1, sfa::Point &p2, sfa::Point &p3)
    {
        PlaneFitterData* mfd = (PlaneFitterData*)(data);
        p1 = mfd->plane[0];
        p2 = mfd->plane[1];
        p3 = mfd->plane[2];
        if (!validFit)
        {
#ifdef PLANEFIT_DEBUG_OUTPUT
            log << ccl::LERR << "Unable to fit plane, using average Z to form a flat plane." << log.endl;
#endif
            double sum = 0;
            PointList::iterator iter = data->points.begin();
            while (iter != data->points.end())
            {
                sfa::Point *pt = *iter++;
                sum += pt->Z();
            }
            double flatz = sum / data->points.size();
            p1.setZ(flatz);
            p2.setZ(flatz);
            p3.setZ(flatz);
        }
        return true;
    }

}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2021.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Timo Sachsenberg  $
// $Authors: $
// --------------------------------------------------------------------------

#include <OpenMS/CONCEPT/Macros.h>
#include <OpenMS/MATH/STATISTICS/LinearRegression.h>
#include <OpenMS/MATH/STATISTICS/StatisticFunctions.h>

#include "Wm5Vector2.h"
#include "Wm5ApprLineFit2.h"
#include "Wm5LinearSystem.h"

#include <boost/math/distributions/normal.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/distributions.hpp>

using boost::math::detail::inverse_students_t;

namespace OpenMS::Math
{
    double LinearRegression::getIntercept() const
    {
      return intercept_;
    }

    double LinearRegression::getSlope() const
    {
      return slope_;
    }

    double LinearRegression::getXIntercept() const
    {
      return x_intercept_;
    }

    double LinearRegression::getLower() const
    {
      return lower_;
    }

    double LinearRegression::getUpper() const
    {
      return upper_;
    }

    double LinearRegression::getTValue() const
    {
      return t_star_;
    }

    double LinearRegression::getRSquared() const
    {
      return r_squared_;
    }

    double LinearRegression::getStandDevRes() const
    {
      return stand_dev_residuals_;
    }

    double LinearRegression::getMeanRes() const
    {
      return mean_residuals_;
    }

    double LinearRegression::getStandErrSlope() const
    {
      return stand_error_slope_;
    }

    double LinearRegression::getChiSquared() const
    {
      return chi_squared_;
    }

    double LinearRegression::getRSD() const
    {
      return rsd_;
    }

    void LinearRegression::computeGoodness_(const std::vector<Wm5::Vector2d>& points, double confidence_interval_P)
    {
      OPENMS_PRECONDITION(static_cast<unsigned>(points.size()) > 2,
          "Cannot compute goodness of fit for regression with less than 3 data points");
      // specifically, boost throws an exception for a t-distribution with zero df

      unsigned N = static_cast<unsigned>(points.size());
      std::vector<double> X; X.reserve(N);
      std::vector<double> Y; Y.reserve(N);
      for (unsigned i = 0; i < N; ++i)
      {
        X.push_back(points[i].X());
        Y.push_back(points[i].Y());
      }

      // Mean of abscissa and ordinate values
      double x_mean = Math::mean(X.begin(), X.end());
      double y_mean = Math::mean(Y.begin(), Y.end());

      // Variance and Covariances
      double var_X = Math::variance(X.begin(), X.end(), x_mean);
      double var_Y = Math::variance(Y.begin(), Y.end(), y_mean);
      double cov_XY = Math::covariance(X.begin(), X.end(), Y.begin(), Y.end());

      // S_xx
      double s_XX = var_X * (N-1);
      /*for (unsigned i = 0; i < N; ++i)
      {
        double d = (X[i] - x_mean);
        s_XX += d * d;
      }*/

      // Compute the squared Pearson coefficient
      r_squared_ = (cov_XY * cov_XY) / (var_X * var_Y);

      // The standard deviation of the residuals
      double sum = 0;
      for (unsigned i = 0; i < N; ++i)
      {
        double x_i = fabs(Y[i] - (intercept_ + slope_ * X[i]));
        sum += x_i;
      }
      mean_residuals_       = sum / N;
      stand_dev_residuals_ = sqrt((chi_squared_ - (sum * sum) / N) / (N - 1));

      // The Standard error of the slope
      stand_error_slope_ = stand_dev_residuals_ / sqrt(s_XX);

      // and the intersection of Y_hat with the x-axis
      x_intercept_ = -(intercept_ / slope_);

      double P = 1 - (1 - confidence_interval_P) / 2;
      boost::math::students_t tdist(N - 2);
      t_star_ = boost::math::quantile(tdist, P);

      //Compute the asymmetric 95% confidence interval of around the X-intercept
      double g = (t_star_ / (slope_ / stand_error_slope_));
      g *= g;
      double left = (x_intercept_ - x_mean) * g;
      double bottom = 1 - g;
      double d = (x_intercept_ - x_mean);
      double right = t_star_ * (stand_dev_residuals_ / slope_) * sqrt((d * d) / s_XX + (bottom / N));

      // Confidence interval lower_ <= X_intercept <= upper_
      lower_ = x_intercept_ + (left + right) / bottom;
      upper_ = x_intercept_ + (left - right) / bottom;

      if (lower_ > upper_)
      {
        std::swap(lower_, upper_);
      }

      double tmp = 0;
      for (unsigned i = 0; i < N; ++i)
      {
        tmp += (X[i] - x_mean) * (X[i] - x_mean);
      }

      //            cout << "100.0 / abs( x_intercept_ ) " << (100.0 / fabs( x_intercept_ )) << endl;
      //            cout << "tmp : " << tmp << endl;
      //            cout << "slope_ " << slope_ << endl;
      //            cout << "y_mean " << y_mean << endl;
      //            cout << "N " << N << endl;
      //            cout << "stand_dev_residuals_ " << stand_dev_residuals_ << endl;
      //            cout << " (1.0/ (double) N)  " <<  (1.0/ (double) N)  << endl;
      //            cout << "sx hat " << (stand_dev_residuals_ / slope_) * sqrt(  (1.0/ (double) N) * (y_mean / (slope_ * slope_ * tmp ) ) ) << endl;

      // compute relative standard deviation (non-standard formula, taken from Mayr et al. (2006) )
      rsd_ = (100.0 / fabs(x_intercept_)) * (stand_dev_residuals_ / slope_) * sqrt((1.0 / (double) N) * (y_mean / (slope_ * slope_ * tmp)));

      if (rsd_ < 0.0)
      {
        std::cout << "rsd < 0.0 " << std::endl;
        std::cout << "Intercept                                  " << intercept_
                  << "\nSlope                                    " << slope_
                  << "\nSquared pearson coefficient              " << r_squared_
                  << "\nValue of the t-distribution              " << t_star_
                  << "\nStandard deviation of the residuals      " << stand_dev_residuals_
                  << "\nStandard error of the slope              " << stand_error_slope_
                  << "\nThe X intercept                          " << x_intercept_
                  << "\nThe lower border of confidence interval  " << lower_
                  << "\nThe higher border of confidence interval " << upper_
                  << "\nChi squared value                        " << chi_squared_
                  << "\nx mean                                   " << x_mean
                  << "\nstand_error_slope/slope_                 " << (stand_dev_residuals_ / slope_)
                  << "\nCoefficient of Variation                 " << (stand_dev_residuals_ / slope_) / x_mean * 100  << std::endl
                  << "========================================="
                  << std::endl;
      }
    }

    void LinearRegression::computeRegression(double confidence_interval_P,
        std::vector<double>::const_iterator x_begin,
        std::vector<double>::const_iterator x_end,
        std::vector<double>::const_iterator y_begin,
        bool compute_goodness)
    {
      std::vector<Wm5::Vector2d> points;
      for(std::vector<double>::const_iterator xIter = x_begin, yIter = y_begin; xIter!=x_end; ++xIter, ++yIter)
      {
        points.emplace_back(*xIter, *yIter);
      }

      // Compute the unweighted linear fit.
      // Get the intercept and the slope of the regression Y_hat=intercept_+slope_*X
      // and the value of Chi squared (sum( (y - evel(x))^2)
      bool pass = Wm5::HeightLineFit2<double>(static_cast<int>(points.size()), &points.front(), slope_, intercept_);
      chi_squared_ = computeChiSquare(x_begin, x_end, y_begin, slope_, intercept_);

      if (!pass)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-LinearRegression", String("Could not fit a linear model to the data (") + points.size() + " points).");
      }

      if (compute_goodness && points.size() > 2)
      {
        computeGoodness_(points, confidence_interval_P);
      }
    }

    void LinearRegression::computeRegressionWeighted(double confidence_interval_P,
        std::vector<double>::const_iterator x_begin,
        std::vector<double>::const_iterator x_end,
        std::vector<double>::const_iterator y_begin,
        std::vector<double>::const_iterator w_begin,
        bool compute_goodness)
     {
      // Compute the weighted linear fit.
      // Get the intercept and the slope of the regression Y_hat=intercept_+slope_*X
      // and the value of Chi squared, the covariances of the intercept and the slope
      std::vector<Wm5::Vector2d> points;
      for(std::vector<double>::const_iterator xIter = x_begin, yIter = y_begin; xIter!=x_end; ++xIter, ++yIter)
      {
        points.emplace_back(*xIter, *yIter);
      }

      // Compute sums for linear system. copy&paste from GeometricTools Wm5ApprLineFit2.cpp
      // and modified to allow weights
      int numPoints = static_cast<int>(points.size());
      double sumX = 0, sumY = 0;
      double sumXX = 0, sumXY = 0;
      double sumW = 0;
      auto wIter = w_begin;

      for (int i = 0; i < numPoints; ++i, ++wIter)
      {
        sumX += (*wIter) * points[i].X();
        sumY += (*wIter) * points[i].Y();
        sumXX += (*wIter) * points[i].X() * points[i].X();
        sumXY += (*wIter) * points[i].X() * points[i].Y();
        sumW += (*wIter);
      }
      //create matrices to solve Ax = B
      double A[2][2] =
      {
        {sumXX, sumX},
        {sumX, sumW}
      };
      double B[2] =
      {
        sumXY,
        sumY
      };
      double X[2];

      bool nonsingular = Wm5::LinearSystem<double>().Solve2(A, B, X);
      if (nonsingular)
      {
        slope_ = X[0];
        intercept_ = X[1];
      }
      chi_squared_ = computeWeightedChiSquare(x_begin, x_end, y_begin, w_begin, slope_, intercept_);

      if (nonsingular)
      {
        if (compute_goodness && points.size() > 2) computeGoodness_(points, confidence_interval_P);
      }
      else
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION,
            "UnableToFit-LinearRegression", "Could not fit a linear model to the data");
      }
    }

} // OpenMS //Math


#ifndef DXF_TYPES_HPP
#define DXF_TYPES_HPP

/// SYSTEM
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/strategies/transform/matrix_transformers.hpp>
#include <boost/geometry/strategies/transform/inverse_transformer.hpp>

#include <cmath>

#include <vector>
#include <algorithm>

namespace cslibs_boost_geometry {
namespace types {

class Mask {
public:
    Mask(const unsigned int _size) :
        mask(new bool[_size]),
        size(_size)
    {
        std::fill(mask, mask + _size, true);
    }

    bool& operator[](const unsigned int i)
    {
        return mask[i];
    }

    void resize(const unsigned int _size)
    {
        if(mask != nullptr) {
            delete [] mask;
            mask = new bool[_size];
        }

        size = _size;
        std::fill(mask, mask + _size, true);
    }

    virtual ~Mask()
    {
        if(mask != nullptr)
            delete [] mask;
    }

private:
    bool        *mask;
    unsigned int size;
};

/**
 * @brief Type definition for point sets.
 */
template<typename PointT>
struct PointSet {
    typedef std::vector<PointT>
    type;
};
/**
 * @brief Type definition of a polygon.
 */
template<typename PointT>
struct Polygon {
    typedef boost::geometry::model::polygon<PointT>
    type;
};

/**
 * @brief Type definition for a line.
 */
template<typename PointT>
struct Line {
    typedef boost::geometry::model::segment<PointT>
    type;
};
/**
 * @brief Type definition for a line set.
 */
template<typename PointT>
struct LineSet {
    typedef std::vector<boost::geometry::model::segment<PointT> >
    type;

    static const boost::geometry::model::segment<PointT>& getSegment(const typename type::const_iterator& it) {
        return *it;
    }

    static boost::geometry::model::segment<PointT>& getSegment(const typename type::iterator& it) {
        return *it;
    }
};
/**
 * @brief Type definition for an indexed line set.
 */
template<typename PointT>
struct IndexedLineSet {
    typedef std::vector<const boost::geometry::model::segment<PointT>* >
    type;

    static const boost::geometry::model::segment<PointT>& getSegment(const typename type::const_iterator& it) {
        return **it;
    }
};

/**
 * @brief Type definition for a box.
 */
template<typename PointT>
struct Box {
    typedef boost::geometry::model::box<PointT>
    type;
};

/**
 * @brief Type definition for point sets that
 *        can be declared invalid.
 */
template<typename PointT>
struct IntersectionResult {
    typedef IntersectionResult<PointT>
    type;

    IntersectionResult() :
        valid(false)
    {
    }

    typename PointSet<PointT>::type result;
    bool                            valid;
};
/**
 * @brief Type definition for a set of
 *        invalidatable point sets.
 */
template<typename PointT>
struct IntersectionResultSet {
    typedef std::vector<IntersectionResult<PointT> >
    type;
};

/**
 * @brief Type defintion for a translation.
 */
template<typename PointT>
struct Translation {
#if BOOST_VERSION >= 105500
    typedef boost::geometry::strategy::transform::translate_transformer<double, 2, 2>
#else
    typedef boost::geometry::strategy::transform::translate_transformer<PointT, PointT>
#endif
    type;
};

template<typename PointT>
struct Rotation {
#if BOOST_VERSION >= 105500
    typedef boost::geometry::strategy::transform::rotate_transformer<boost::geometry::radian, double, 2, 2>
#else
    typedef boost::geometry::strategy::transform::rotate_transformer<PointT, PointT,
                                                                     boost::geometry::radian>
#endif
    type;
};

/**
 * @brief Invert a translation.
 * @param translation   the translation to be inverted
 * @return              the inverted translation
 */
template<typename PointT, typename TransT>
TransT invert(const TransT &translation)
{
#if BOOST_VERSION >= 105500
    return boost::geometry::strategy::transform::inverse_transformer<double, 2, 2>::inverse_transformer(translation);
#else
    return boost::geometry::strategy::transform::inverse_transformer<PointT, PointT>::inverse_transformer(translation);
#endif
}

struct periodic {
    inline static double sin(const double rad)
    {
        return std::sin(rad);
    }

    inline static double cos(const double rad)
    {
        return std::cos(rad);
    }

    inline static void sin_cos(const double x,
                               double &sinx,
                               double &cosx)
    {
        sinx = std::sin(x);
        cosx = std::cos(x);
    }
};

struct periodicApprox {
    inline static double angleClamp(const double rad)
    {
        double rad_(rad);
        while (rad_ < -M_PI)
            rad_ += 2*M_PI;

        while (rad_ >= M_PI)
            rad_ -= 2*M_PI;

        return rad_;
    }

    inline static double sin(const double rad)
    {
        double angle = angleClamp(rad);
        return fastersinfull(angle);

    }

    inline static double cos(const double rad)
    {
        double angle = angleClamp(rad);
        return fastercosfull(angle);
    }

    inline static void sin_cos(const double rad,
                               double &sinx,
                               double &cosx)
    {
        double angle = angleClamp(rad);
        sinx = fastersinfull(angle);
        cosx = fastercosfull(angle);
    }

    ///__________________ FAST MATH _________________///

    static inline float
    fastsin (float x)
    {
      static const float fouroverpi = 1.2732395447351627f;
      static const float fouroverpisq = 0.40528473456935109f;
      static const float q = 0.78444488374548933f;
      union { float f; uint32_t i; } p = { 0.20363937680730309f };
      union { float f; uint32_t i; } r = { 0.015124940802184233f };
      union { float f; uint32_t i; } s = { -0.0032225901625579573f };

      union { float f; uint32_t i; } vx = { x };
      uint32_t sign = vx.i & 0x80000000;
      vx.i = vx.i & 0x7FFFFFFF;

      float qpprox = fouroverpi * x - fouroverpisq * x * vx.f;
      float qpproxsq = qpprox * qpprox;

      p.i |= sign;
      r.i |= sign;
      s.i ^= sign;

      return q * qpprox + qpproxsq * (p.f + qpproxsq * (r.f + qpproxsq * s.f));
    }

    static inline float
    fastersin (float x)
    {
      static const float fouroverpi = 1.2732395447351627f;
      static const float fouroverpisq = 0.40528473456935109f;
      static const float q = 0.77633023248007499f;
      union { float f; uint32_t i; } p = { 0.22308510060189463f };

      union { float f; uint32_t i; } vx = { x };
      uint32_t sign = vx.i & 0x80000000;
      vx.i &= 0x7FFFFFFF;

      float qpprox = fouroverpi * x - fouroverpisq * x * vx.f;

      p.i |= sign;

      return qpprox * (q + p.f * qpprox);
    }

    static inline float
    fastsinfull (float x)
    {
      static const float twopi = 6.2831853071795865f;
      static const float invtwopi = 0.15915494309189534f;

      int k = x * invtwopi;
      float half = (x < 0) ? -0.5f : 0.5f;
      return fastsin ((half + k) * twopi - x);
    }

    static inline float
    fastersinfull (float x)
    {
      static const float twopi = 6.2831853071795865f;
      static const float invtwopi = 0.15915494309189534f;

      int k = x * invtwopi;
      float half = (x < 0) ? -0.5f : 0.5f;
      return fastersin ((half + k) * twopi - x);
    }

    static inline float
    fastcosfull (float x)
    {
      static const float halfpi = 1.5707963267948966f;
      return fastsinfull (x + halfpi);
    }

    static inline float
    fastercosfull (float x)
    {
      static const float halfpi = 1.5707963267948966f;
      return fastersinfull (x + halfpi);
    }
};

/// PREDEFINED TYPES
typedef boost::geometry::model::d2::point_xy<double> Point2d;
typedef boost::geometry::model::d2::point_xy<float>  Point2f;
typedef boost::geometry::model::d2::point_xy<int>    Point2i;
typedef Point2i                                      Dim2i;
typedef Point2f                                      Dim2f;
typedef Point2d                                      Dim2d;
typedef Point2i                                      Vec2i;
typedef Point2f                                      Vec2f;
typedef Point2d                                      Vec2d;
typedef Line<Point2d>::type                          Line2d;
typedef Line<Point2f>::type                          Line2f;
typedef Line<Point2i>::type                          Line2i;
typedef Box<Point2d>::type                           Box2d;
typedef Box<Point2f>::type                           Box2f;
typedef Box<Point2i>::type                           Box2i;
typedef PointSet<Point2i>::type                      PointSet2i;
typedef PointSet<Point2f>::type                      PointSet2f;
typedef PointSet<Point2d>::type                      PointSet2d;
typedef Polygon<Point2i>::type                       Polygon2i;
typedef Polygon<Point2f>::type                       Polygon2f;
typedef Polygon<Point2d>::type                       Polygon2d;
typedef IntersectionResultSet<Point2i>::type         Intersection2iResultSet;
typedef IntersectionResultSet<Point2f>::type         Intersection2fResultSet;
typedef IntersectionResultSet<Point2d>::type         Intersection2dResultSet;
typedef IntersectionResult<Point2i>::type            Intersection2iResult;
typedef IntersectionResult<Point2f>::type            Intersection2fResult;
typedef IntersectionResult<Point2d>::type            Intersection2dResult;
typedef LineSet<Point2i>::type                       Line2iSet;
typedef LineSet<Point2f>::type                       Line2fSet;
typedef LineSet<Point2d>::type                       Line2dSet;
typedef Translation<Point2i>::type                   Translation2i;
typedef Translation<Point2f>::type                   Translation2f;
typedef Translation<Point2d>::type                   Translation2d;
typedef Rotation<Point2i>::type                      Rotation2i;
typedef Rotation<Point2f>::type                      Rotation2f;
typedef Rotation<Point2d>::type                      Rotation2d;
typedef std::vector<const Line2i*>                   Line2iPtrSet;
typedef std::vector<const Line2f*>                   Line2fPtrSet;
typedef std::vector<const Line2d*>                   Line2dPtrSet;
}
}
#endif // DXF_TYPES_HPP


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

   Feature expectations for the expectation semiring.

   the relationship between FeatureWeight (with nonnegative feature values only)
   and ExpectationWeight is, for a linear derivation (single path):

   FeatureWeight(cost, [1=val1,...])

   is the same as

   ExpectationWeight(cost=-log(prob), [1=-log(val1*prob)=-log(val1)+cost,...])

   times in both cases adds the costs, and the values accumulate in the same way
   modulo the above isomorphism (exception: FeatureWeight with allowed negative
   values would not be isomorphic; we would need to introduce a sign bit to the
   ExpectationWeight's feature values)

   we could have kept the interpretations the same, but this way is more
   computationally efficient (because FeatureWeight plus selects the best path,
   while ExpectationWeight sums probabilities/expectations of things)

   think of ExpectationWeight as representing disjoint events.

   the sum of two ExpectationWeight is then a more probable single event.

   the feature vectors' values are -log(p(event)*E[count of feature|event]) - joint, not conditional

   (the overall probability is also a float value that's -log(prob))

   so adding two ExpectationWeight just separately (-LogPlus) adds the probs and
   expectation vectors

   further, the default value for weight[feature_id] is +infinity, not 0 as it
   is for featureweight

   the times (product) simply concatenates the events together (multiplying the
   two probabilities), and the resulting expectations are counts of features over
   the event (so the product event is less likely, but has more features on it, so
   higher conditional expectation)

   but because we don't store expectations conditional on the event, but rather
   the joint expectation in the overall probability distribution, this means that
   we have (a*b)[i]=-log(prob(a)*prob(b)*E[feature i|a*b]

   where E[feature i|a*b]) := E[feature i|a]+E[feature i|b])

   Now, times in detail:

   the algorithm for C=A*B is:

   costC = costA+costB
   fC[i] = costA+costB+logplus(fA[i]-costA, fB[i]-costB)
   =(probs)probA*probB*(pA[i]/probA+pB[i]/probB) = pA[i]*probB + pB[i]*probA
   = logplus(fA[i]+costB, fB[i]+costA)

   or to add C += B

   fC[i] = logplus(costB + fC[i], costC + fB[i])

   in case fC[i] = fB[i] (same map)

   fC[i] = logplus(costB + fC[i], costC + fC[i]) =(prob) probB*pC[i] + probC*pC[i] = (probB+probC)*pC[i]
   = logplus(costB, costC) + fC[i]

   in case C==B (so further costB == costC), you can take logplus(costC, costC) = costC-log(2)

   graehl thinks it really simplifies thinking about expectations if we use
   conditional (log) expectations in the feature values map. however, it's
   currently correct so don't touch it.

   \author Markus Dreyer

*/

#ifndef HYP__HYPERGRAPH_EXPECTATION_WEIGHT_HPP
#define HYP__HYPERGRAPH_EXPECTATION_WEIGHT_HPP
#pragma once

#include <sdl/Hypergraph/FeatureWeight.hpp>
#include <sdl/Hypergraph/Weight.hpp>
#include <sdl/Util/Constants.hpp>
#include <sdl/Util/LogHelper.hpp>
#include <sdl/Util/LogMath.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <cassert>
#include <iostream>
#include <stdexcept>
#include <string>

namespace sdl {
namespace Hypergraph {

/// ExpectationWeight is a type of FeatureWeight, where we take the
/// sum of values rather than the min:
typedef FeatureWeightTpl<FeatureValue, Features, Expectation> ExpectationWeight;

/**
   Adds another weight to this weight (using semiring
   plus). This is the version for Expectation policy.
*/
template <class FloatT, class MapT, class SumPolicy>
void FeatureWeightTpl<FloatT, MapT, SumPolicy>::plusBy(FeatureWeightTpl<FloatT, MapT, Expectation> const& b) {
  checkSumPolicy<Expectation>();
  if (b.isZero()) {
    // adding zero to this means do nothing. the expectations must all be zero too.
  } else if (isZero()) {
    *this = b;
  } else if (&b == this) {
    // initially, I thought the pmap equal by pointer case would handle this, but
    // i was wrong. ownMap() only copies if pointer wasn't unique. but the
    // pointer will be unique if the objects are identical.
    const FloatT neglog2 = -std::log((FloatT)2);
    this->value_ += neglog2;
    if (pMap_) {
      ownMap();
      for (iterator i = pMap_->begin(), e = pMap_->end(); i != e; ++i)
        i->second += neglog2;  // in prob space, we're adding p1+p2
    }
  } else {
    Util::neglogPlusBy(b.value_, this->value_);
    assert(!isZero());  // adding two nonzero things can never give a zero result
    if (pMap_ == b.pMap_) {  // maps equal by pointer. doesn't imply probabilities are the same too
      if (pMap_) {
        ownMap();
        const FloatT neglog2 = -std::log((FloatT)2);
        for (iterator i = pMap_->begin(), e = pMap_->end(); i != e; ++i)
          i->second += neglog2;  // in prob space, we're adding p1+p2
        // by multiplying by 2 (since the maps are the same, the joint probs are all the same)
      }
    } else {
      // add (joint) expectations:
      if (!b.empty()) {
        ownMap();
        MapT& thismap = *pMap_;
        MapT* bmap = b.pMap_.get();
        assert(&thismap != bmap);
        if (bmap)
          for (typename MapT::const_iterator i = bmap->begin(), e = bmap->end(); i != e; ++i)
            Util::mapAddNeglogPlus(thismap, i->first, i->second);
      }
    }
  }
}

/**
   Multiplies this weight by another weight (using semiring
   times). This is the version for Expectation policy.
*/
template <class FloatT, class MapT, class SumPolicy>
void FeatureWeightTpl<FloatT, MapT, SumPolicy>::timesBy(FeatureWeightTpl<FloatT, MapT, Expectation> const& b) {
  checkSumPolicy<Expectation>();
  if (isZero()) {
  } else if (b.isZero()) {
    setZero();
  } else if (b.isOne()) {
  } else if (isOne()) {
    *this = b;
  } else {
    FloatT const aval = this->value_;
    FloatT const bval = b.value_;
    this->value_ = aval + bval;  // pa' = pa*pb
    if (pMap_ == b.pMap_) {  // special (rare) case to avoid modifying map while reading from it
      ownMap();  // note: this must occur after the test above
      // fC[i] <= logplus(costB + fC[i], costC + fB[i]) = logplus(costB, costC) + fC[i]
      FloatT const aplusbval = Util::neglogPlus(aval, bval);  // we're setting (in prob space) v'=v*(pa+pb)
      SDL_DEBUG_ALWAYS(Hypergraph.ExpectationWeight, "same map for a*b; result[i] = a[i] + neglogplus("
                                                         << aval << ", " << bval << ") = " << aplusbval);
      for (typename MapT::iterator i = pMap_->begin(), e = pMap_->end(); i != e; ++i)
        i->second += aplusbval;  // *=(pa+pb)
    } else {
      assert(this != &b);
      ownMap();
      // fC[i] = logplus(costB + fC[i], costC + fB[i])
      // multiply A expectations by probB
      for (typename MapT::iterator i = pMap_->begin(), e = pMap_->end(); i != e; ++i) i->second += bval;
      // then add (B expectations multiplied by probA).
      if (!b.empty()) {
        MapT& thismap = *pMap_;
        MapT const* bmap = b.pMap_.get();
        assert(&thismap != bmap);
        if (bmap)
          for (typename MapT::const_iterator i = bmap->begin(), e = bmap->end(); i != e; ++i)
            Util::mapAddNeglogPlus(thismap, i->first, i->second + aval);
      }
    }
    assert(!isZero());  // we shouldn't ever underflow by adding two non-zero probs in logspace
  }
}

template <class FloatT, class MapT>
inline FeatureWeightTpl<FloatT, MapT, Expectation> plus(FeatureWeightTpl<FloatT, MapT, Expectation> const& w1,
                                                        FeatureWeightTpl<FloatT, MapT, Expectation> const& w2) {
  FeatureWeightTpl<FloatT, MapT, Expectation> w3(w1, true);
  w3.plusBy(w2);
  return w3;
}

/// result has p=w1.p*w2.p and feats=w1.p*w2.feats+w2.p*w1.feats
template <class FloatT, class MapT>
inline FeatureWeightTpl<FloatT, MapT, Expectation> times(FeatureWeightTpl<FloatT, MapT, Expectation> const& w1,
                                                         FeatureWeightTpl<FloatT, MapT, Expectation> const& w2) {
  FeatureWeightTpl<FloatT, MapT, Expectation> w3(w1, true);
  w3.timesBy(w2);
  return w3;
}

}}

#endif


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#ifndef KINDR_ROTATIONS_EIGEN_EULERANGLESXYZ_HPP_
#define KINDR_ROTATIONS_EIGEN_EULERANGLESXYZ_HPP_

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationBase.hpp"
#include "kindr/rotations/eigen/RotationEigenFunctions.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {

/*! \class EulerAnglesXyz
 *  \brief Implementation of Euler angles (X-Y'-Z'' / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
 *
 *  The following typedefs are provided for convenience:
 *   - \ref eigen_impl::EulerAnglesXyzAD "EulerAnglesXyzAD" for active rotation and double primitive type
 *   - \ref eigen_impl::EulerAnglesXyzAF "EulerAnglesXyzAF" for active rotation and float primitive type
 *   - \ref eigen_impl::EulerAnglesXyzPD "EulerAnglesXyzPD" for passive rotation and double primitive type
 *   - \ref eigen_impl::EulerAnglesXyzPF "EulerAnglesXyzPF" for passive rotation and float primitive type
 *   - EulerAnglesRpyAD = EulerAnglesXyzAD
 *   - EulerAnglesRpyAF = EulerAnglesXyzAF
 *   - EulerAnglesRpyPD = EulerAnglesXyzPD
 *   - EulerAnglesRpyPF = EulerAnglesXyzPF
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *  \tparam Usage_ the rotation usage which is either active or passive
 *
 *  \ingroup rotations
 */
template<typename PrimType_, enum RotationUsage Usage_>
class EulerAnglesXyz : public EulerAnglesXyzBase<EulerAnglesXyz<PrimType_, Usage_>, Usage_> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::Matrix<PrimType_, 3, 1> Base;

  /*! \brief vector of Euler angles [roll; pitch; yaw]
   */
  Base xyz_;

 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  /*! \brief Rotation Vector as 3x1-matrix
   */
  typedef Base Vector;

  /*! \brief Default constructor using identity rotation.
   */
  EulerAnglesXyz()
    : xyz_(Base::Zero()) {
  }

  /*! \brief Constructor using three scalars.
   *  \param roll     first rotation angle around X axis
   *  \param pitch    second rotation angle around Y' axis
   *  \param yaw      third rotation angle around Z'' axis
   */
  EulerAnglesXyz(Scalar roll, Scalar pitch, Scalar yaw)
    : xyz_(roll,pitch,yaw) {
  }

  /*! \brief Constructor using Eigen::Matrix.
   *  \param other   Eigen::Matrix<PrimType_,3,1> [roll; pitch; yaw]
   */
  explicit EulerAnglesXyz(const Base& other)
  : xyz_(other) {
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit EulerAnglesXyz(const RotationBase<OtherDerived_, Usage_>& other)
    : xyz_(internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation()) {
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns referece
   */
  template<typename OtherDerived_>
  EulerAnglesXyz& operator =(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Parenthesis operator to convert from another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  EulerAnglesXyz& operator ()(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  EulerAnglesXyz inverted() const {
    return EulerAnglesXyz(eigen_internal::getInverseRpy<PrimType_, PrimType_>(this->toImplementation()));
  }

  /*! \brief Inverts the rotation.
   *  \returns reference
   */
  EulerAnglesXyz& invert() {
    *this = this->inverted();
    return *this;
  }

  /*! \brief Returns the Euler angles in a vector.
   *  \returns  vector Eigen::Matrix<Scalar,3, 1>
   */
  inline const Vector vector() const {
    return this->toImplementation();
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline Base& toImplementation() {
    return static_cast<Base&>(xyz_);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline const Base& toImplementation() const {
    return static_cast<const Base&>(xyz_);
  }

  /*! \brief Returns roll (X) angle.
   *  \returns roll angle (scalar)
   */
  inline Scalar roll() const {
    return xyz_(0);
  }

  /*! \brief Returns pitch (Y') angle.
   *  \returns pitch angle (scalar)
   */
  inline Scalar pitch() const {
    return xyz_(1);
  }

  /*! \brief Returns yaw (Z'') angle.
   *  \returns yaw angle (scalar)
   */
  inline Scalar yaw() const {
    return xyz_(2);
  }

  /*! \brief Sets roll (X) angle.
   */
  inline void setRoll(Scalar roll) {
    xyz_(0) = roll;
  }

  /*! \brief Sets pitch (Y') angle.
   */
  inline void setPitch(Scalar pitch) {
    xyz_(1) = pitch;
  }

  /*! \brief Sets yaw (Z'') angle.
   */
  inline void setYaw(Scalar yaw) {
    xyz_(2) = yaw;
  }

  /*! \brief Gets roll (X) angle.
   *  \returns roll angle (scalar)
   */
  inline Scalar x() const {
    return xyz_(0);
  }

  /*! \brief Gets pitch (Y') angle.
   *  \returns pitch angle (scalar)
   */
  inline Scalar y() const {
    return xyz_(1);
  }

  /*! \brief Gets yaw (Z'') angle.
   *  \returns yaw angle (scalar)
   */
  inline Scalar z() const {
    return xyz_(2);
  }

  /*! \brief Sets roll (X) angle.
   */
  inline void setX(Scalar x) {
    xyz_(0) = x;
  }

  /*! \brief Sets pitch (Y') angle.
   */
  inline void setY(Scalar y) {
    xyz_(1) = y;
  }

  /*! \brief Sets yaw (Z'') angle.
   */
  inline void setZ(Scalar z) {
    xyz_(2) = z;
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  EulerAnglesXyz& setIdentity() {
    xyz_.setZero();
    return *this;
  }

  /*! \brief Returns a unique Euler angles rotation with angles in [-pi,pi),[-pi/2,pi/2),[-pi,pi).
   *  This function is used to compare different rotations.
   *  \returns copy of the Euler angles rotation which is unique
   */
  EulerAnglesXyz getUnique() const {
    Base xyz(kindr::common::floatingPointModulo(x()+M_PI,2*M_PI)-M_PI,
             kindr::common::floatingPointModulo(y()+M_PI,2*M_PI)-M_PI,
             kindr::common::floatingPointModulo(z()+M_PI,2*M_PI)-M_PI); // wrap all angles into [-pi,pi)

    const double tol = 1e-3;

    // wrap angles into [-pi,pi),[-pi/2,pi/2),[-pi,pi)
    if(xyz.y() < -M_PI/2 - tol)
    {
      if(xyz.x() < 0) {
        xyz.x() = xyz.x() + M_PI;
      } else {
        xyz.x() = xyz.x() - M_PI;
      }

      xyz.y() = -(xyz.y() + M_PI);

      if(xyz.z() < 0) {
        xyz.z() = xyz.z() + M_PI;
      } else {
        xyz.z() = xyz.z() - M_PI;
      }
    }
    else if(-M_PI/2 - tol <= xyz.y() && xyz.y() <= -M_PI/2 + tol)
    {
      xyz.x() -= xyz.z();
      xyz.z() = 0;
    }
    else if(-M_PI/2 + tol < xyz.y() && xyz.y() < M_PI/2 - tol)
    {
      // ok
    }
    else if(M_PI/2 - tol <= xyz.y() && xyz.y() <= M_PI/2 + tol)
    {
      // todo: M_PI/2 should not be in range, other formula?
      xyz.x() += xyz.z();
      xyz.z() = 0;
    }
    else // M_PI/2 + tol < xyz.y()
    {
      if(xyz.x() < 0) {
        xyz.x() = xyz.x() + M_PI;
      } else {
        xyz.x() = xyz.x() - M_PI;
      }

      xyz.y() = -(xyz.y() - M_PI);

      if(xyz.z() < 0) {
        xyz.z() = xyz.z() + M_PI;
      } else {
        xyz.z() = xyz.z() - M_PI;
      }
    }

    return EulerAnglesXyz(xyz);
  }

  /*! \brief Modifies the Euler angles rotation such that the angles lie in [-pi,pi),[-pi/2,pi/2),[-pi,pi).
   *  \returns reference
   */
  EulerAnglesXyz& setUnique() {
    *this = getUnique();
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using EulerAnglesXyzBase<EulerAnglesXyz<PrimType_, Usage_>, Usage_>::operator*;

  /*! \brief Equivalence operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator==.
   *  \returns true if two rotations are similar.
   */
  using EulerAnglesXyzBase<EulerAnglesXyz<PrimType_, Usage_>, Usage_>::operator==;

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const EulerAnglesXyz& xyz) {
    out << xyz.toImplementation().transpose();
    return out;
  }
};

//! \brief Active Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with double primitive type
typedef EulerAnglesXyz<double, RotationUsage::ACTIVE>  EulerAnglesXyzAD;
//! \brief Active Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with float primitive type
typedef EulerAnglesXyz<float,  RotationUsage::ACTIVE>  EulerAnglesXyzAF;
//! \brief Passive Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with double primitive type
typedef EulerAnglesXyz<double, RotationUsage::PASSIVE> EulerAnglesXyzPD;
//! \brief Passive Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with float primitive type
typedef EulerAnglesXyz<float,  RotationUsage::PASSIVE> EulerAnglesXyzPF;

//! \brief Equivalent Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) class
template <typename PrimType_, enum RotationUsage Usage_>
using EulerAnglesRpy = EulerAnglesXyz<PrimType_, Usage_>;

//! \brief Active Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with double primitive type
typedef EulerAnglesRpy<double, RotationUsage::ACTIVE>  EulerAnglesRpyAD;
//! \brief Active Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with float primitive type
typedef EulerAnglesRpy<float,  RotationUsage::ACTIVE>  EulerAnglesRpyAF;
//! \brief Passive Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with double primitive type
typedef EulerAnglesRpy<double, RotationUsage::PASSIVE> EulerAnglesRpyPD;
//! \brief Passive Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with float primitive type
typedef EulerAnglesRpy<float,  RotationUsage::PASSIVE> EulerAnglesRpyPF;

} // namespace eigen_impl

namespace internal {

template<typename PrimType_, enum RotationUsage Usage_>
class get_scalar<eigen_impl::EulerAnglesXyz<PrimType_, Usage_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_, enum RotationUsage Usage_>
class get_matrix3X<eigen_impl::EulerAnglesXyz<PrimType_, Usage_>>{
 public:
  typedef int  IndexType;

  template <IndexType Cols>
  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>> {
 public:
  typedef eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE> OtherUsage;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>> {
 public:
  typedef eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE> OtherUsage;
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::AngleAxis<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::AngleAxis<SourcePrimType_, Usage_>& aa) {
//    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::getRpyFromAngleAxis<SourcePrimType_, DestPrimType_>(aa.toImplementation()));
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(aa));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::RotationVector<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::RotationVector<SourcePrimType_, Usage_>& rotationVector) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::AngleAxis<SourcePrimType_, Usage_>(rotationVector));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>& q) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getRpyFromQuaternion<SourcePrimType_, DestPrimType_>(q.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::RotationMatrix<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::RotationMatrix<SourcePrimType_, Usage_>& R) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getRpyFromRotationMatrix<SourcePrimType_, DestPrimType_>(R.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>& xyz) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(xyz.toImplementation().template cast<DestPrimType_>());
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>& zyx) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getRpyFromYpr<SourcePrimType_, DestPrimType_>(zyx.toImplementation()));
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Multiplication Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

//template<typename PrimType_>
//class MultiplicationTraits<RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>, RotationUsage::ACTIVE>, RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>, RotationUsage::ACTIVE>> {
// public:
//  inline static eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE> mult(const eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>& a, const eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>& b) {
//    return eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>(eigen_impl::RotationQuaternion<PrimType_, RotationUsage::ACTIVE>(
//                                                                 eigen_impl::RotationQuaternion<PrimType_, RotationUsage::ACTIVE>(a).toImplementation()*
//                                                                 eigen_impl::RotationQuaternion<PrimType_, RotationUsage::ACTIVE>(b).toImplementation()));
//  }
//};
//
//template<typename PrimType_>
//class MultiplicationTraits<RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>, RotationUsage::PASSIVE>, RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>, RotationUsage::PASSIVE>> {
// public:
//  inline static eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE> mult(const eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>& a, const eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>& b) {
//    return eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>(eigen_impl::RotationQuaternion<PrimType_, RotationUsage::PASSIVE>(
//                                                                 eigen_impl::RotationQuaternion<PrimType_, RotationUsage::PASSIVE>(a).toImplementation()*
//                                                                 eigen_impl::RotationQuaternion<PrimType_, RotationUsage::PASSIVE>(b).toImplementation()));
//  }
//};

//template<typename PrimType_, enum RotationUsage Usage_>
//class MultiplicationTraits<RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, Usage_>, Usage_>, RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, Usage_>, Usage_>> {
// public:
//  inline static eigen_impl::EulerAnglesXyz<PrimType_, Usage_> mult(const eigen_impl::EulerAnglesXyz<PrimType_, Usage_>& a, const eigen_impl::EulerAnglesXyz<PrimType_, Usage_>& b) {
//    return eigen_impl::EulerAnglesXyz<PrimType_, Usage_>(eigen_impl::RotationQuaternion<PrimType_, Usage_>(
//                                                                 eigen_impl::RotationQuaternion<PrimType_, Usage_>(a).toImplementation()*
//                                                                 eigen_impl::RotationQuaternion<PrimType_, Usage_>(b).toImplementation()));
//  }
//};
//

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

} // namespace internal
} // namespace rotations
} // namespace kindr

#endif /* KINDR_ROTATIONS_EIGEN_EULERANGLESXYZ_HPP_ */