/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file LinearizedFactor.cpp
 * @brief A dummy factor that allows a linear factor to act as a nonlinear factor
 * @author Alex Cunningham
 */

#include <iostream>
#include <boost/foreach.hpp>

#include <gtsam_unstable/nonlinear/LinearizedFactor.h>

namespace gtsam {

using namespace std;

/* ************************************************************************* */
LinearizedFactor::LinearizedFactor(JacobianFactor::shared_ptr lin_factor,
		const KeyLookup& decoder, const Values& lin_points)
: Base(lin_factor->get_model()), b_(lin_factor->getb()), model_(lin_factor->get_model()) {
	BOOST_FOREACH(const Index& idx, *lin_factor) {
		// find full symbol
		KeyLookup::const_iterator decode_it = decoder.find(idx);
		if (decode_it == decoder.end())
			throw runtime_error("LinearizedFactor: could not find index in decoder!");
		Key key(decode_it->second);

		// extract linearization point
		assert(lin_points.exists(key));
		this->lin_points_.insert(key, lin_points.at(key));  // NOTE: will not overwrite

		// extract Jacobian
		Matrix A = lin_factor->getA(lin_factor->find(idx));
		this->matrices_.insert(std::make_pair(key, A));

		// store keys
		this->keys_.push_back(key);
	}
}

/* ************************************************************************* */
LinearizedFactor::LinearizedFactor(JacobianFactor::shared_ptr lin_factor,
			const Ordering& ordering, const Values& lin_points)
: Base(lin_factor->get_model()), b_(lin_factor->getb()), model_(lin_factor->get_model()) {
	const KeyLookup decoder = ordering.invert();
	BOOST_FOREACH(const Index& idx, *lin_factor) {
		// find full symbol
		KeyLookup::const_iterator decode_it = decoder.find(idx);
		if (decode_it == decoder.end())
			throw runtime_error("LinearizedFactor: could not find index in decoder!");
		Key key(decode_it->second);

		// extract linearization point
		assert(lin_points.exists(key));
		this->lin_points_.insert(key, lin_points.at(key));  // NOTE: will not overwrite

		// extract Jacobian
		Matrix A = lin_factor->getA(lin_factor->find(idx));
		this->matrices_.insert(std::make_pair(key, A));

		// store keys
		this->keys_.push_back(key);
	}
}

/* ************************************************************************* */
Vector LinearizedFactor::unwhitenedError(const Values& c,
		boost::optional<std::vector<Matrix>&> H) const {
	// extract the points in the new values
	Vector ret = - b_;

	if (H) H->resize(this->size());
	size_t i=0;
	BOOST_FOREACH(Key key, this->keys()) {
		const Value& newPt = c.at(key);
		const Value& linPt = lin_points_.at(key);
		Vector d = linPt.localCoordinates_(newPt);
		const Matrix& A = matrices_.at(key);
		ret += A * d;
		if (H) (*H)[i++] = A;
	}

	return ret;
}

/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedFactor::linearize(const Values& c, const Ordering& ordering) const {

	// sort by varid - only known at linearization time
	typedef std::map<Index, Matrix> VarMatrixMap;
	VarMatrixMap sorting_terms;
	BOOST_FOREACH(const KeyMatrixMap::value_type& p, matrices_)
		sorting_terms.insert(std::make_pair(ordering[p.first], p.second));

	// move into terms
	std::vector<std::pair<Index, Matrix> > terms;
	BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms)
		terms.push_back(p);

	// compute rhs: adjust current by whitened update
	Vector b = unwhitenedError(c) + b_; // remove original b
	this->noiseModel_->whitenInPlace(b);

	return boost::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b - b_, model_));
}

/* ************************************************************************* */
void LinearizedFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
	this->noiseModel_->print(s + std::string(" model"));
	BOOST_FOREACH(const KeyMatrixMap::value_type& p, matrices_) {
		gtsam::print(p.second, keyFormatter(p.first));
	}
	gtsam::print(b_, std::string("b"));
	std::cout << " nonlinear keys: ";
	BOOST_FOREACH(const Key& key, this->keys())
		cout << keyFormatter(key) << "  ";
	lin_points_.print("Linearization Point");
}

/* ************************************************************************* */
bool LinearizedFactor::equals(const NonlinearFactor& other, double tol) const {
  const LinearizedFactor* e = dynamic_cast<const LinearizedFactor*>(&other);
	if (!e || !Base::equals(other, tol)
			|| !lin_points_.equals(e->lin_points_, tol)
			|| !equal_with_abs_tol(b_, e->b_, tol)
			|| !model_->equals(*e->model_, tol)
			|| matrices_.size() != e->matrices_.size())
		return false;

	KeyMatrixMap::const_iterator map1 = matrices_.begin(), map2 = e->matrices_.begin();
	for (; map1 != matrices_.end() && map2 != e->matrices_.end(); ++map1, ++map2)
		if ((map1->first != map2->first) || !equal_with_abs_tol(map1->second, map2->second, tol))
			return false;
	return true;
}

} // \namespace gtsam


// Boost.Geometry Index
//
// R-tree R*-tree split algorithm implementation
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// 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)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP

#include <boost/geometry/index/detail/algorithms/intersection_content.hpp>
#include <boost/geometry/index/detail/algorithms/union_content.hpp>
#include <boost/geometry/index/detail/algorithms/margin.hpp>

#include <boost/geometry/index/detail/rtree/node/node.hpp>
#include <boost/geometry/index/detail/rtree/visitors/insert.hpp>
#include <boost/geometry/index/detail/rtree/visitors/is_leaf.hpp>

namespace boost { namespace geometry { namespace index {

namespace detail { namespace rtree {

namespace rstar {

template <typename Element, typename Translator, typename Tag, size_t Corner, size_t AxisIndex>
class element_axis_corner_less
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_TAG, (Tag));
};

template <typename Element, typename Translator, size_t Corner, size_t AxisIndex>
class element_axis_corner_less<Element, Translator, box_tag, Corner, AxisIndex>
{
public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        return geometry::get<Corner, AxisIndex>(rtree::element_indexable(e1, m_tr))
            < geometry::get<Corner, AxisIndex>(rtree::element_indexable(e2, m_tr));
    }

private:
    Translator const& m_tr;
};

template <typename Element, typename Translator, size_t Corner, size_t AxisIndex>
class element_axis_corner_less<Element, Translator, point_tag, Corner, AxisIndex>
{
public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        return geometry::get<AxisIndex>(rtree::element_indexable(e1, m_tr))
            < geometry::get<AxisIndex>(rtree::element_indexable(e2, m_tr));
    }

private:
    Translator const& m_tr;
};

template <typename Parameters, typename Box, size_t Corner, size_t AxisIndex>
struct choose_split_axis_and_index_for_corner
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename Elements::value_type element_type;
        typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
        typedef typename tag<indexable_type>::type indexable_tag;

        BOOST_GEOMETRY_INDEX_ASSERT(elements.size() == parameters.get_max_elements() + 1, "wrong number of elements");

        // copy elements
        Elements elements_copy(elements);                                                                       // MAY THROW, STRONG (alloc, copy)

        // sort elements
        element_axis_corner_less<element_type, Translator, indexable_tag, Corner, AxisIndex> elements_less(translator);
        std::sort(elements_copy.begin(), elements_copy.end(), elements_less);                                   // MAY THROW, BASIC (copy)

        // init outputs
        choosen_index = parameters.get_min_elements();
        sum_of_margins = 0;
        smallest_overlap = (std::numeric_limits<content_type>::max)();
        smallest_content = (std::numeric_limits<content_type>::max)();

        // calculate sum of margins for all distributions
        size_t index_last = parameters.get_max_elements() - parameters.get_min_elements() + 2;
        for ( size_t i = parameters.get_min_elements() ; i < index_last ; ++i )
        {
            // TODO - awulkiew: may be optimized - box of group 1 may be initialized with
            // box of min_elems number of elements and expanded for each iteration by another element

            Box box1 = rtree::elements_box<Box>(elements_copy.begin(), elements_copy.begin() + i, translator);
            Box box2 = rtree::elements_box<Box>(elements_copy.begin() + i, elements_copy.end(), translator);

            sum_of_margins += index::detail::comparable_margin(box1) + index::detail::comparable_margin(box2);

            content_type ovl = index::detail::intersection_content(box1, box2);
            content_type con = index::detail::content(box1) + index::detail::content(box2);

            // TODO - shouldn't here be < instead of <= ?
            if ( ovl < smallest_overlap || (ovl == smallest_overlap && con <= smallest_content) )
            {
                choosen_index = i;
                smallest_overlap = ovl;
                smallest_content = con;
            }
        }

        ::boost::ignore_unused_variable_warning(parameters);
    }
};

template <typename Parameters, typename Box, size_t AxisIndex, typename ElementIndexableTag>
struct choose_split_axis_and_index_for_axis
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_TAG, (ElementIndexableTag));
};

template <typename Parameters, typename Box, size_t AxisIndex>
struct choose_split_axis_and_index_for_axis<Parameters, Box, AxisIndex, box_tag>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        size_t index1 = 0;
        margin_type som1 = 0;
        content_type ovl1 = (std::numeric_limits<content_type>::max)();
        content_type con1 = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_corner<Parameters, Box, min_corner, AxisIndex>::
            apply(elements, index1,
                  som1, ovl1, con1,
                  parameters, translator);                                                                  // MAY THROW, STRONG

        size_t index2 = 0;
        margin_type som2 = 0;
        content_type ovl2 = (std::numeric_limits<content_type>::max)();
        content_type con2 = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_corner<Parameters, Box, max_corner, AxisIndex>::
            apply(elements, index2,
                  som2, ovl2, con2,
                  parameters, translator);                                                                  // MAY THROW, STRONG

        sum_of_margins = som1 + som2;

        if ( ovl1 < ovl2 || (ovl1 == ovl2 && con1 <= con2) )
        {
            choosen_corner = min_corner;
            choosen_index = index1;
            smallest_overlap = ovl1;
            smallest_content = con1;
        }
        else
        {
            choosen_corner = max_corner;
            choosen_index = index2;
            smallest_overlap = ovl2;
            smallest_content = con2;
        }
    }
};

template <typename Parameters, typename Box, size_t AxisIndex>
struct choose_split_axis_and_index_for_axis<Parameters, Box, AxisIndex, point_tag>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        choose_split_axis_and_index_for_corner<Parameters, Box, min_corner, AxisIndex>::
            apply(elements, choosen_index,
                  sum_of_margins, smallest_overlap, smallest_content,
                  parameters, translator);                                                                  // MAY THROW, STRONG

        choosen_corner = min_corner;
    }
};

template <typename Parameters, typename Box, size_t Dimension>
struct choose_split_axis_and_index
{
    BOOST_STATIC_ASSERT(0 < Dimension);

    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_axis,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & smallest_sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename rtree::element_indexable_type<typename Elements::value_type, Translator>::type element_indexable_type;

        choose_split_axis_and_index<Parameters, Box, Dimension - 1>::
            apply(elements, choosen_axis, choosen_corner, choosen_index,
                  smallest_sum_of_margins, smallest_overlap, smallest_content,
                  parameters, translator);                                                                  // MAY THROW, STRONG

        margin_type sum_of_margins = 0;

        size_t corner = min_corner;
        size_t index = 0;

        content_type overlap_val = (std::numeric_limits<content_type>::max)();
        content_type content_val = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_axis<
            Parameters,
            Box,
            Dimension - 1,
            typename tag<element_indexable_type>::type
        >::apply(elements, corner, index, sum_of_margins, overlap_val, content_val, parameters, translator); // MAY THROW, STRONG

        if ( sum_of_margins < smallest_sum_of_margins )
        {
            choosen_axis = Dimension - 1;
            choosen_corner = corner;
            choosen_index = index;
            smallest_sum_of_margins = sum_of_margins;
            smallest_overlap = overlap_val;
            smallest_content = content_val;
        }
    }
};

template <typename Parameters, typename Box>
struct choose_split_axis_and_index<Parameters, Box, 1>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_axis,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & smallest_sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename rtree::element_indexable_type<typename Elements::value_type, Translator>::type element_indexable_type;

        choosen_axis = 0;

        choose_split_axis_and_index_for_axis<
            Parameters,
            Box,
            0,
            typename tag<element_indexable_type>::type
        >::apply(elements, choosen_corner, choosen_index, smallest_sum_of_margins, smallest_overlap, smallest_content, parameters, translator); // MAY THROW
    }
};

template <size_t Corner, size_t Dimension>
struct partial_sort
{
    BOOST_STATIC_ASSERT(0 < Dimension);

    template <typename Elements, typename Translator>
    static inline void apply(Elements & elements, const size_t axis, const size_t index, Translator const& tr)
    {
        if ( axis < Dimension - 1 )
        {
            partial_sort<Corner, Dimension - 1>::apply(elements, axis, index, tr);                          // MAY THROW, BASIC (copy)
        }
        else
        {
            BOOST_GEOMETRY_INDEX_ASSERT(axis == Dimension - 1, "unexpected axis value");

            typedef typename Elements::value_type element_type;
            typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
            typedef typename tag<indexable_type>::type indexable_tag;

            element_axis_corner_less<element_type, Translator, indexable_tag, Corner, Dimension - 1> less(tr);
            std::partial_sort(elements.begin(), elements.begin() + index, elements.end(), less);            // MAY THROW, BASIC (copy)
        }
    }
};

template <size_t Corner>
struct partial_sort<Corner, 1>
{
    template <typename Elements, typename Translator>
    static inline void apply(Elements & elements,
                             const size_t BOOST_GEOMETRY_INDEX_ASSERT_UNUSED_PARAM(axis),
                             const size_t index,
                             Translator const& tr)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(axis == 0, "unexpected axis value");

        typedef typename Elements::value_type element_type;
        typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
        typedef typename tag<indexable_type>::type indexable_tag;

        element_axis_corner_less<element_type, Translator, indexable_tag, Corner, 0> less(tr);
        std::partial_sort(elements.begin(), elements.begin() + index, elements.end(), less);                // MAY THROW, BASIC (copy)
    }
};

} // namespace rstar

template <typename Value, typename Options, typename Translator, typename Box, typename Allocators>
struct redistribute_elements<Value, Options, Translator, Box, Allocators, rstar_tag>
{
    typedef typename rtree::node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type node;
    typedef typename rtree::internal_node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type internal_node;
    typedef typename rtree::leaf<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type leaf;

    typedef typename Options::parameters_type parameters_type;

    static const size_t dimension = geometry::dimension<Box>::value;

    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Node>
    static inline void apply(
        Node & n,
        Node & second_node,
        Box & box1,
        Box & box2,
        parameters_type const& parameters,
        Translator const& translator,
        Allocators & allocators)
    {
        typedef typename rtree::elements_type<Node>::type elements_type;

        elements_type & elements1 = rtree::elements(n);
        elements_type & elements2 = rtree::elements(second_node);

        size_t split_axis = 0;
        size_t split_corner = 0;
        size_t split_index = parameters.get_min_elements();
        margin_type smallest_sum_of_margins = (std::numeric_limits<margin_type>::max)();
        content_type smallest_overlap = (std::numeric_limits<content_type>::max)();
        content_type smallest_content = (std::numeric_limits<content_type>::max)();

        rstar::choose_split_axis_and_index<
            typename Options::parameters_type,
            Box,
            dimension
        >::apply(elements1,
                 split_axis, split_corner, split_index,
                 smallest_sum_of_margins, smallest_overlap, smallest_content,
                 parameters, translator);                                                               // MAY THROW, STRONG

        // TODO: awulkiew - get rid of following static_casts?

        BOOST_GEOMETRY_INDEX_ASSERT(split_axis < dimension, "unexpected value");
        BOOST_GEOMETRY_INDEX_ASSERT(split_corner == static_cast<size_t>(min_corner) || split_corner == static_cast<size_t>(max_corner), "unexpected value");
        BOOST_GEOMETRY_INDEX_ASSERT(parameters.get_min_elements() <= split_index && split_index <= parameters.get_max_elements() - parameters.get_min_elements() + 1, "unexpected value");

        // copy original elements
        elements_type elements_copy(elements1);                                                         // MAY THROW, STRONG
        elements_type elements_backup(elements1);                                                       // MAY THROW, STRONG

        // TODO: awulkiew - check if std::partial_sort produces the same result as std::sort
        if ( split_corner == static_cast<size_t>(min_corner) )
            rstar::partial_sort<min_corner, dimension>
                ::apply(elements_copy, split_axis, split_index, translator);                            // MAY THROW, BASIC (copy)
        else
            rstar::partial_sort<max_corner, dimension>
                ::apply(elements_copy, split_axis, split_index, translator);                            // MAY THROW, BASIC (copy)

        BOOST_TRY
        {
            // copy elements to nodes
            elements1.assign(elements_copy.begin(), elements_copy.begin() + split_index);               // MAY THROW, BASIC
            elements2.assign(elements_copy.begin() + split_index, elements_copy.end());                 // MAY THROW, BASIC

            // calculate boxes
            box1 = rtree::elements_box<Box>(elements1.begin(), elements1.end(), translator);
            box2 = rtree::elements_box<Box>(elements2.begin(), elements2.end(), translator);
        }
        BOOST_CATCH(...)
        {
            //elements_copy.clear();
            elements1.clear();
            elements2.clear();

            rtree::destroy_elements<Value, Options, Translator, Box, Allocators>::apply(elements_backup, allocators);
            //elements_backup.clear();

            BOOST_RETHROW                                                                                // RETHROW, BASIC
        }
        BOOST_CATCH_END
    }
};

}} // namespace detail::rtree

}}} // namespace boost::geometry::index

#endif // BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP


/*!
@file
Defines `boost::hana::Boolean`.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_BOOLEAN_HPP
#define BOOST_HANA_BOOLEAN_HPP

#include <boost/hana/core/typeclass.hpp>
#include <boost/hana/detail/constexpr.hpp>

namespace boost { namespace hana {
    //! @ingroup group-typeclasses
    //! The `Boolean` type class defines a
    //! [boolean algebra](http://en.wikipedia.org/wiki/Boolean_algebra_(structure)).
    //!
    //! @note
    //! We could define what it means for a Boolean to be compile-time and
    //! what it means to be runtime. Then, it might be possible to specify
    //! some algorithms in term of the runtime-ness of the predicates they
    //! use internally. Shooting from the hip, compile-time ness might be
    //! the capacity to have a `then` and an `else_` branch of different
    //! type, plus the insurance to never try to cast the unused branch
    //! (that could work hand-in-hand with a `lazy` feature).
    //!
    //! ----------------------------------------------------------------------
    //!
    //! ## Laws
    //! ...
    struct Boolean {
        BOOST_HANA_TYPECLASS(Boolean);
        struct mcd { };
    };

    //! ...
    //! @relates Boolean
    BOOST_HANA_CONSTEXPR_LAMBDA auto complement = [](auto x) {
        return Boolean::instance<datatype_t<decltype(x)>>::complement_impl(x);
    };

    //! ...
    //! @relates Boolean
    BOOST_HANA_CONSTEXPR_LAMBDA auto meet = [](auto x, auto y) {
        return Boolean::instance<datatype_t<decltype(x)>>::meet_impl(x, y);
    };

    //! ...
    //! @relates Boolean
    BOOST_HANA_CONSTEXPR_LAMBDA auto join = [](auto x, auto y) {
        return Boolean::instance<datatype_t<decltype(x)>>::join_impl(x, y);
    };

    //! ...
    //! @relates Boolean
    template <typename T>
    BOOST_HANA_CONSTEXPR_LAMBDA auto top = Boolean::instance<T>::top_impl();

    //! ...
    //! @relates Boolean
    template <typename T>
    BOOST_HANA_CONSTEXPR_LAMBDA auto bottom = Boolean::instance<T>::bottom_impl();
}} // end namespace boost::hana

#endif // !BOOST_HANA_BOOLEAN_HPP


#include <boost/numeric/ublas/expression_types.hpp>


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

  Non-trivial node types (class lables) for MRF models.

  Copyright (c) 2012, 2013
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  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.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 THE AUTHOR OR CONTRIBUTORS 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 NODE_TYPES_HPP_6CF0A0E1_8AE9_4951_A785_9339B90FB976
#define NODE_TYPES_HPP_6CF0A0E1_8AE9_4951_A785_9339B90FB976

#include <cmath>
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <boost/operators.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace bo {
namespace mrf {

template <typename P>
class ParametricNodeType: boost::equality_comparable1<ParametricNodeType<P> >
{
public:
    typedef boost::shared_ptr<P> ClassParamsPtr;

    ParametricNodeType(ClassParamsPtr class_params): class_params_(class_params)
    { }

    //  Generated copy c-tor and assignment operator are fine.

    virtual ~ParametricNodeType()
    { }

    // Classes are equal when their parameters are equal.
    virtual bool operator==(const ParametricNodeType<P>& other) const
    { return (class_params_ == other.class_params_); }

protected:
    ClassParamsPtr class_params_;
};

// A class representing a value (class label) with associated Gauss distribution
// parameters. Parameters structure is represented as a tuple with 4 elements:
// class label, mean, standard deviation, and additional item a = ln(sigma * sqrt(2 pi)).
template <typename RealType>
class GaussDistrClasses: public ParametricNodeType<
        boost::tuples::tuple<int, RealType, RealType, RealType> >
{
public:
    typedef GaussDistrClasses<RealType> SelfType;

    typedef boost::tuples::tuple<int, RealType, RealType, RealType> ClassParams;
    typedef ParametricNodeType<ClassParams> BaseType;
    typedef typename BaseType::ClassParamsPtr ClassParamsPtr;

public:
    GaussDistrClasses(ClassParamsPtr class_params): BaseType(class_params)
    { }

    static GaussDistrClasses CreateInstance(int idx, RealType mean, RealType sigma)
    { return (SelfType(boost::make_shared<ClassParams>(idx, mean, sigma, compute_a(sigma)))); }

    // Accessors for class label and class parameters.
    int label() const
    { return this->class_params_->template get<0>(); }

    RealType mean() const
    { return this->class_params_->template get<1>(); }

    RealType sigma() const
    { return this->class_params_->template get<2>(); }

    RealType a() const
    { return this->class_params_->template get<3>(); }

    // 2.5 is precomputed sqrt(2 pi).
    static RealType compute_a(RealType sigma)
    { return (std::log(sigma * RealType(2.5))); }
};

// A class representing a value (class label) with associated Gamma distribution
// parameters. Parameters structure is represented as a tuple with 4 elements:
// class label and a set of Gamma distribution parameters (k, theta, a), where
// a = ln(G(k)) + k ln(theta) and G(t) is the Gamma function.
template <typename RealType>
class GammaDistrClasses: public ParametricNodeType<
        boost::tuples::tuple<int, RealType, RealType, RealType> >
{
public:
    typedef GammaDistrClasses<RealType> SelfType;

    typedef boost::tuples::tuple<int, RealType, RealType, RealType> ClassParams;
    typedef ParametricNodeType<ClassParams> BaseType;
    typedef typename BaseType::ClassParamsPtr ClassParamsPtr;

public:
    GammaDistrClasses(ClassParamsPtr class_params): BaseType(class_params)
    { }

    static GammaDistrClasses CreateInstance(int idx, RealType k, RealType theta)
    { return (SelfType(boost::make_shared<ClassParams>(idx, k, theta, compute_a(k, theta)))); }

    // Accessors for class label and class parameters.
    int label() const
    { return this->class_params_->template get<0>(); }

    RealType k() const
    { return this->class_params_->template get<1>(); }

    RealType theta() const
    { return this->class_params_->template get<2>(); }

    RealType a() const
    { return this->class_params_->template get<3>(); }

    RealType mean() const
    { return (k() * theta()); }

    static RealType compute_a(RealType k, RealType theta)
    { return (boost::math::lgamma(k) + k * std::log(theta)); }
};

template <typename T>
std::ostream& operator<<(std::ostream& os, const GaussDistrClasses<T>& obj)
{
    os << obj.label() << " (mean: " << obj.mean() << ", sigma: " << obj.sigma() << ")";
    return os;
}

template <typename T>
std::ostream& operator<<(std::ostream& os, const GammaDistrClasses<T>& obj)
{
    os << obj.label() << " (k: " << obj.k() << ", theta: " << obj.theta() << ")";
    return os;
}

} // namespace mrf
} // namespace bo

#endif // NODE_TYPES_HPP_6CF0A0E1_8AE9_4951_A785_9339B90FB976


// Copyright (c) 2018-2019, CNRS
// Authors: Sanghyun Kim <ggory15@snu.ac.kr>

#ifndef __timeopt_phase_hpp__
#define __timeopt_phase_hpp__

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <vector>

#include "timeopt/fwd.hpp"

  namespace timeopt
  {
    struct PhaseInfo
    {
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      typedef size_t Index;
      typedef Eigen::DenseIndex DenseIndex;
      typedef double Scalar;
      typedef Eigen::MatrixXd MatrixX;
      typedef Eigen::VectorXd VectorX;
      typedef Eigen::Vector3d Vector3d;
      typedef Eigen::Matrix3d Matrix3d;
      typedef std::vector< VectorX,Eigen::aligned_allocator<VectorX> > VectorXVector;
      typedef Eigen::Matrix<double, 10, 1> FootPrint;

      /// \brief Default constructor
      PhaseInfo();

      /// \brief Copy constructor
      PhaseInfo(const PhaseInfo & other);

      /// \brief constructor
      PhaseInfo(const EndeffectorID & ee, const double & stime, const double & etime, const Vector3d& pos, const Matrix3d& rot);

      /// \brief Copy operator
      PhaseInfo & operator=(const PhaseInfo & other)
      {
        ee_id = other.ee_id;
        start_time = other.start_time;
        end_time = other.end_time;
        pos = other.pos;
        rot = other.rot;

        return *this;
      }

      /// \brief Comparison operator
      bool operator==(const PhaseInfo & other) const
      {
        return ee_id == other.ee_id
        && start_time == other.start_time
        && end_time == other.end_time
        && pos == other.pos
        && rot == other.rot
        ;
      }

      bool operator!=(const PhaseInfo & other) const
      { return !(*this == other); }

      FootPrint toVector(){
        FootPrint print;
        print(0) = start_time;
        print(1) = end_time;
        print.segment(2, 3) = pos;
        Eigen::Quaterniond quat(rot);
        print.segment(5, 4) << quat.w(), quat.x(), quat.y(), quat.z();
        print(9) = 1;

        return print;
      }

      /* Bounds on the duration */
      double start_time, end_time;

      /* Position & rot*/
      Vector3d pos;
      Matrix3d rot;

      /* End_effector id */
      EndeffectorID ee_id;

      /* Integrator */
      PhaseType phase_type;

    }; // struct PhaseInfo
  }

#endif // ifndef __timeopt_phase_hpp__


//---------------------------------------------------------------------------//
//!
//! \file   tstInverseAtomicMomentumUnit.cpp
//! \author Alex Robinson
//! \brief  The inverse atomic momentum unit unit tests
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>

// FRENSIE Includes
#include "Utility_InverseAtomicMomentumUnit.hpp"
#include "Utility_AtomicMomentumUnit.hpp"

using namespace Utility::Units;
using boost::units::quantity;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

BOOST_UNITS_STATIC_CONSTANT(si_momentum,boost::units::si::momentum);
BOOST_UNITS_STATIC_CONSTANT(cgs_momentum,boost::units::cgs::momentum);

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the inverse atomic momentum unit can be initialized
TEUCHOS_UNIT_TEST( InverseAtomicMomentumUnit, initialize )
{
  quantity<InverseAtomicMomentum>
    inverse_momentum( 1.0*inverse_atomic_momentum );

  TEST_EQUALITY_CONST( inverse_momentum.value(), 1.0 );

  inverse_momentum = 2.0*inverse_atomic_momentum;

  TEST_EQUALITY_CONST( inverse_momentum.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the inverse atomic momentum unit can be initialized from a.mom.
TEUCHOS_UNIT_TEST( InverseAtomicMomentumUnit, initialize_from_atomic_momentum )
{
  quantity<InverseAtomicMomentum> inverse_momentum( 1.0/atomic_momentum );

  TEST_EQUALITY_CONST( inverse_momentum.value(), 1.0 );

  inverse_momentum = 2.0/atomic_momentum;

  TEST_EQUALITY_CONST( inverse_momentum.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the inverse atomic momentum unit can be initialized from cgs, si
TEUCHOS_UNIT_TEST( InverseAtomicMomentumUnit, initialize_from_cgs_si )
{
  quantity<InverseAtomicMomentum> inverse_momentum_si( 1.0/si_momentum );

  quantity<InverseAtomicMomentum> inverse_momentum_cgs( 1.0/cgs_momentum );

  TEST_FLOATING_EQUALITY( inverse_momentum_si.value(),
			  1.992851882e-24,
			  1e-15 );
  TEST_FLOATING_EQUALITY( inverse_momentum_cgs.value(),
			  1.992851882e-19,
			  1e-15 );
}

//---------------------------------------------------------------------------//
// end tstInverseAtomicMomentumUnit.cpp
//---------------------------------------------------------------------------//


/**
 * @Author: Julien Vial-Detambel <l3ninj>
 * @Date:   2018-06-12T11:57:649+01:00
 * @Email:  julien.vial-detambel@epitech.eu
 * @Project: CUDA-Based Simulator of Quantum Systems
 * @Filename: main.cpp
 * @Last modified by:   l3ninj
 * @Last modified time: 2018-08-21T22:17:22+02:00
 * @License: MIT License
 */

#include <iostream>
#include <cmath>
#include <boost/program_options.hpp>

#include "Errors.hpp"
#include "Logger.hpp"
#include "ExecutorManager.hpp"

#include "Parser/ASTGenerator.hpp"
#include "Parser/CircuitBuilder.hpp"
#include "Circuit.hpp"
#include "CircuitCompressor.hpp"
#include "TaskScheduling/CircuitToTaskGraphConverter.hpp"
#include "TaskScheduling/BasicTaskScheduler.hpp"
#include "TaskScheduling/BasicStateStore.hpp"
#include "TaskScheduling/BasicMeasurementResultsTree.hpp"
#include "Worker/Simulator.hpp"
#include "Worker/Worker.hpp"

using namespace StateStore;
using namespace MeasurementResultsTree;

bool g_cpu_execution = false;

int main(int ac, char **av) {
  boost::program_options::options_description desc("Allowed options");
  desc.add_options()
    ("help", "help - produce help message")
    ("cpu", "cpu - force linear algebra execution on the cpu")
    ("sample", boost::program_options::value<int>(),
    "sample - set sample level (int)")
    ("input-file", boost::program_options::value<std::string>(),
    "input file - first positional argument (string)");

  boost::program_options::positional_options_description p;
  p.add("input-file", -1);

  boost::program_options::variables_map vm;
  boost::program_options::store(boost::program_options::command_line_parser(ac, av).options(desc).positional(p).run(), vm);
  boost::program_options::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return EXIT_FAILURE;
  }

  if (vm.count("cpu")) {
    LOG(Logger::INFO, "Naive CPU linear algebra execution forced.");
    g_cpu_execution = true;
  }

  int sample_level = 1000;
  if (vm.count("sample")) {
    LOG(Logger::INFO, "Set sample level to " << vm["sample"].as<int>());
    sample_level = vm["sample"].as<int>();
  } else {
    LOG(Logger::INFO, "Sample level is 1000 by default");
  }

  if (vm.count("input-file")) {
    LOG(Logger::INFO, "Set " << vm["input-file"].as<std::string>()
    << " as input file");
  } else {
    LOG(Logger::ERROR, "Need at least the file argument");
    return EXIT_FAILURE;
  }

  // Matrix m1(new Tvcplxd(64*64, 1.0), 64, 64);
  // Matrix m2(new Tvcplxd(64*64, 1.0), 64, 64);
  // Matrix m3(new Tvcplxd(64*64), 64, 64);
  // Matrix v1(new Tvcplxd(1*64, 1.0), 1, 64);
  // std::cout << "NORM " << v1.normalize() << std::endl;
  // Matrix v2(new Tvcplxd(1*64), 1, 64);
  // std::cout << "TRACE " << m1.trace() << std::endl;

  // m3 = m1 * m2;
  // std::cout << "multiplication (matrix)" << m3 << std::endl;
  // v2 = m1 * v1;
  // std::cout << "multiplication (vector)" << v2 << std::endl;

  Parser::AST::t_AST ast;
  /* Reads the file and generate an AST */
  try {
    ast = Parser::ASTGenerator()(av[1]);
  } catch (const std::ios_base::failure &e) {
    LOG(Logger::ERROR, "Cannot open/parse file " << av[1]);
    return EXIT_FAILURE;
  } catch (const OpenQASMError &e) {
    LOG(Logger::ERROR, "The AST Generator encountered ill-formated OpenQASM");
    return EXIT_FAILURE;
  }

  Circuit circuit;
  /* Reads the AST and generate a Circuit */
  try {
    circuit = CircuitBuilder(av[1])(ast);
    circuit = CircuitCompressor(circuit)();
    LOG(Logger::DEBUG, "Optimized circuit:" << std::endl << circuit);
  } catch (const OpenQASMError& e) {
    LOG(Logger::ERROR, "Error while generating the circuit: " << e.what());
    return EXIT_FAILURE;
  }

  std::shared_ptr<IMeasurementResultsTree> measurementTree = std::make_shared<BasicMeasurementResultsTree>(sample_level);

  /* Convert circuit to graph of tasks */
  CircuitToTaskGraphConverter converter(circuit);
  TaskGraph::Graph graph = converter.generateTaskGraph(*measurementTree);
  LOG(Logger::INFO, "Task graph:" << graph);

  /* Creates scheduler and state store*/
  std::shared_ptr<ITaskScheduler> scheduler = std::make_shared<BasicTaskScheduler>(graph);
  std::shared_ptr<IStateStore> stateStore = std::make_shared<BasicStateStore>(graph);

  /* Start a worker */
  Worker worker = Worker(*scheduler, *stateStore, *measurementTree);
  worker();

  /* End of the simulation */
  measurementTree->printResults(circuit.creg);

  return EXIT_SUCCESS;
}


#ifndef STEREO_LIDAR_CALIBRATION_EXTRACTIMAGEFEATURE_HPP
#define STEREO_LIDAR_CALIBRATION_EXTRACTIMAGEFEATURE_HPP
#include <opencv2/core.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/videoio.hpp>
#include "config.hpp"
#include "libcbdetect/config.h"
#include "libcbdetect/find_corners.h"
#include "libcbdetect/boards_from_corners.h"
#include "utils.hpp"
#include <memory>
#include <vector>
#include <fstream>
#include <string>
#include <sstream>
#include <ctime>
#include <cstdio>
#include <iostream>

using namespace std;
using namespace Eigen;

bool fitBoard(vector<vector<int>> &board, vector<cv::Point2d>& corners, cv::Size &size);

void processImage(vector<string>& image_paths, ImageResults& images_features, cv::Mat camera_matrix, cv::Mat dist);

class ImageFeatureDetector{
public:
    ImageFeatureDetector(cv::Mat camera_matrix, cv::Mat dist_coeffs) : m_camera_matrix(camera_matrix),
                                                                       m_dist_coeffs(dist_coeffs),
                                                                       m_square_size(Config::checkerboardSquareSize()),
                                                                       m_board_size(Config::checkerboardGridSize()),
                                                                       m_padding(Config::checkerboardPadding()) {}

    bool detectCheckerboardInImage(cv::Mat& img, std::vector<cv::Point2f>& image_corners, cv::Mat& rvec, cv::Mat& tvec,string method="cbdetect");
    bool detectCheckerboardInImageByCbdetect(cv::Mat& img, std::vector<cv::Point2f>& image_corners, cv::Mat& rvec, cv::Mat& tvec);
    bool detectCheckerboardInImageByOpenCV(cv::Mat& img, std::vector<cv::Point2f>& image_corners, cv::Mat& rvec, cv::Mat& tvec);
    bool detectImageCorner(cv::Mat& input_image, vector<cv::Point2f>& image_corners); // 从图像检测角点
    void undistort_corners(vector<cv::Point2f>& input_corners, cv::Mat &rectified_corners); // 角点畸变校正
    void estimatePose(vector<cv::Point2f>& chessboard_corners, cv::Mat& rvec, cv::Mat&tvec); // 从角点pnp恢复位姿
    void calculate3DCorners(vector<cv::Point3d>& chessboard_3d_corners, cv::Mat& rvec, cv::Mat& tvec); // 计算marker板的3D角点位置，需考虑偏移
    void calculateLines(std::vector<cv::Point3d>& corners, std::vector<Eigen::VectorXd>& lines); // 通过四个角点计算四条边的方程
    void calculatePlane(std::vector<cv::Point3d>& corners, Eigen::VectorXd& plane); // 通过四个角点计算平面的方程
    void calculatePlane1(std::vector<cv::Point3d>& corners, Eigen::VectorXd& plane); // 通过四个角点计算平面的方程
    void transform_to_normalized_plane(vector<cv::Point2f>& corners1, vector<Vector2d>& corners2);
    void calc_chessboard_points_3d(std::vector<Eigen::Vector3d>&chessboard_points_3d, cv::Mat& rvec, cv::Mat& tvec);
    const cv::Mat get_camera_matrix(){
        return m_camera_matrix;
    }

    const cv::Mat get_dist_coeffs(){
        return m_dist_coeffs;
    }

private:
    void calcBoardCornerPositions(); // 计算board corners 在世界坐标系的坐标
    cv::Size m_board_size;  // chessboard 规格大小
    double m_square_size; // 格子边长(m)
    cv::Mat m_camera_matrix = cv::Mat(3, 3, CV_64F);
    cv::Mat m_dist_coeffs = cv::Mat(5,1, CV_64F); // 相机的内参和畸变矩阵
    //std::vector<cv::Point3f> m_3d_corners; // 世界坐标系下的角点坐标
    std::vector<double> m_padding;
};

#endif


/** \file gameengine/Expressions/IntValue.cpp
 *  \ingroup expressions
 */
// IntValue.cpp: implementation of the CIntValue class.
/*
 * Copyright (c) 1996-2000 Erwin Coumans <coockie@acm.org>
 *
 * Permission to use, copy, modify, distribute and sell this software
 * and its documentation for any purpose is hereby granted without fee,
 * provided that the above copyright notice appear in all copies and
 * that both that copyright notice and this permission notice appear
 * in supporting documentation.  Erwin Coumans makes no
 * representations about the suitability of this software for any
 * purpose.  It is provided "as is" without express or implied warranty.
 *
 */

#include "EXP_IntValue.h"

#include <boost/format.hpp>

#include "CM_Message.h"
#include "EXP_BoolValue.h"
#include "EXP_ErrorValue.h"
#include "EXP_FloatValue.h"
#include "EXP_StringValue.h"

CIntValue::CIntValue()
{
}

CIntValue::CIntValue(cInt innie) : m_int(innie)
{
}

CIntValue::CIntValue(cInt innie, const std::string &name) : m_int(innie)
{
  SetName(name);
}

CIntValue::~CIntValue()
{
}

CValue *CIntValue::Calc(VALUE_OPERATOR op, CValue *val)
{
  switch (op) {
    case VALUE_POS_OPERATOR: {
      return new CIntValue(m_int);
      break;
    }
    case VALUE_NEG_OPERATOR: {
      return new CIntValue(-m_int);
      break;
    }
    case VALUE_NOT_OPERATOR: {
      return new CBoolValue(m_int == 0);
      break;
    }
    case VALUE_AND_OPERATOR:
    case VALUE_OR_OPERATOR: {
      return new CErrorValue(val->GetText() + op2str(op) + "only allowed on booleans");
      break;
    }
    default: {
      return val->CalcFinal(VALUE_INT_TYPE, op, this);
      break;
    }
  }
}

CValue *CIntValue::CalcFinal(VALUE_DATA_TYPE dtype, VALUE_OPERATOR op, CValue *val)
{
  CValue *ret;

  switch (dtype) {
    case VALUE_EMPTY_TYPE:
    case VALUE_INT_TYPE: {
      switch (op) {
        case VALUE_MOD_OPERATOR: {
          ret = new CIntValue(((CIntValue *)val)->GetInt() % m_int);
          break;
        }
        case VALUE_ADD_OPERATOR: {
          ret = new CIntValue(((CIntValue *)val)->GetInt() + m_int);
          break;
        }
        case VALUE_SUB_OPERATOR: {
          ret = new CIntValue(((CIntValue *)val)->GetInt() - m_int);
          break;
        }
        case VALUE_MUL_OPERATOR: {
          ret = new CIntValue(((CIntValue *)val)->GetInt() * m_int);
          break;
        }
        case VALUE_DIV_OPERATOR: {
          if (m_int == 0) {
            if (val->GetNumber() == 0) {
              ret = new CErrorValue("Not a Number");
            }
            else {
              ret = new CErrorValue("Division by zero");
            }
          }
          else {
            ret = new CIntValue(((CIntValue *)val)->GetInt() / m_int);
          }
          break;
        }
        case VALUE_EQL_OPERATOR: {
          ret = new CBoolValue(((CIntValue *)val)->GetInt() == m_int);
          break;
        }
        case VALUE_NEQ_OPERATOR: {
          ret = new CBoolValue(((CIntValue *)val)->GetInt() != m_int);
          break;
        }
        case VALUE_GRE_OPERATOR: {
          ret = new CBoolValue(((CIntValue *)val)->GetInt() > m_int);
          break;
        }
        case VALUE_LES_OPERATOR: {
          ret = new CBoolValue(((CIntValue *)val)->GetInt() < m_int);
          break;
        }
        case VALUE_GEQ_OPERATOR: {
          ret = new CBoolValue(((CIntValue *)val)->GetInt() >= m_int);
          break;
        }
        case VALUE_LEQ_OPERATOR: {
          ret = new CBoolValue(((CIntValue *)val)->GetInt() <= m_int);
          break;
        }
        case VALUE_NEG_OPERATOR: {
          ret = new CIntValue(-m_int);
          break;
        }
        case VALUE_POS_OPERATOR: {
          ret = new CIntValue(m_int);
          break;
        }
        case VALUE_NOT_OPERATOR: {
          ret = new CBoolValue(m_int == 0);
          break;
        }
        default: {
          CM_Error("found op: " << op);
          ret = new CErrorValue("illegal operator. please send a bug report.");
          break;
        }
      }
      break;
    }
    case VALUE_FLOAT_TYPE: {
      switch (op) {
        case VALUE_MOD_OPERATOR: {
          ret = new CFloatValue(fmod(((CFloatValue *)val)->GetFloat(), m_int));
          break;
        }
        case VALUE_ADD_OPERATOR: {
          ret = new CFloatValue(((CFloatValue *)val)->GetFloat() + m_int);
          break;
        }
        case VALUE_SUB_OPERATOR: {
          ret = new CFloatValue(((CFloatValue *)val)->GetFloat() - m_int);
          break;
        }
        case VALUE_MUL_OPERATOR: {
          ret = new CFloatValue(((CFloatValue *)val)->GetFloat() * m_int);
          break;
        }
        case VALUE_DIV_OPERATOR: {
          if (m_int == 0) {
            ret = new CErrorValue("Division by zero");
          }
          else {
            ret = new CFloatValue(((CFloatValue *)val)->GetFloat() / m_int);
          }
          break;
        }
        case VALUE_EQL_OPERATOR: {
          ret = new CBoolValue(((CFloatValue *)val)->GetFloat() == m_int);
          break;
        }
        case VALUE_NEQ_OPERATOR: {
          ret = new CBoolValue(((CFloatValue *)val)->GetFloat() != m_int);
          break;
        }
        case VALUE_GRE_OPERATOR: {
          ret = new CBoolValue(((CFloatValue *)val)->GetFloat() > m_int);
          break;
        }
        case VALUE_LES_OPERATOR: {
          ret = new CBoolValue(((CFloatValue *)val)->GetFloat() < m_int);
          break;
        }
        case VALUE_GEQ_OPERATOR: {
          ret = new CBoolValue(((CFloatValue *)val)->GetFloat() >= m_int);
          break;
        }
        case VALUE_LEQ_OPERATOR: {
          ret = new CBoolValue(((CFloatValue *)val)->GetFloat() <= m_int);
          break;
        }
        case VALUE_NOT_OPERATOR: {
          ret = new CBoolValue(m_int == 0);
          break;
        }
        default: {
          ret = new CErrorValue("illegal operator. please send a bug report.");
          break;
        }
      }
      break;
    }
    case VALUE_STRING_TYPE: {
      switch (op) {
        case VALUE_ADD_OPERATOR: {
          ret = new CStringValue(val->GetText() + GetText(), "");
          break;
        }
        case VALUE_EQL_OPERATOR:
        case VALUE_NEQ_OPERATOR:
        case VALUE_GRE_OPERATOR:
        case VALUE_LES_OPERATOR:
        case VALUE_GEQ_OPERATOR:
        case VALUE_LEQ_OPERATOR: {
          ret = new CErrorValue("[Cannot compare string with integer]" + op2str(op) + GetText());
          break;
        }
        default: {
          ret = new CErrorValue("[operator not allowed on strings]" + op2str(op) + GetText());
          break;
        }
      }
      break;
    }
    case VALUE_BOOL_TYPE: {
      ret = new CErrorValue("[operator not valid on boolean and integer]" + op2str(op) +
                            GetText());
      break;
    }
    case VALUE_ERROR_TYPE: {
      ret = new CErrorValue(val->GetText() + op2str(op) + GetText());
      break;
    }
    default: {
      ret = new CErrorValue("illegal type. contact your dealer (if any)");
      break;
    }
  }
  return ret;
}

cInt CIntValue::GetInt()
{
  return m_int;
}

double CIntValue::GetNumber()
{
  return (double)m_int;
}

int CIntValue::GetValueType()
{
  return VALUE_INT_TYPE;
}

std::string CIntValue::GetText()
{
  return (boost::format("%lld") % m_int).str();
}

CValue *CIntValue::GetReplica()
{
  CIntValue *replica = new CIntValue(*this);
  replica->ProcessReplica();

  return replica;
}

void CIntValue::SetValue(CValue *newval)
{
  m_int = (cInt)newval->GetNumber();
}

#ifdef WITH_PYTHON
PyObject *CIntValue::ConvertValueToPython()
{
  return PyLong_FromLongLong(m_int);
}
#endif  // WITH_PYTHON


#include "coordinate_systems.hpp"
#include <boost/python.hpp>
using namespace gd;
using namespace boost::python;

namespace gd {
struct custom_vector_to_python_tuple
{
	static PyObject* convert(Vector3 const& v)
	{
		boost::python::tuple t = make_tuple(v(0), v(1), v(2));
		return boost::python::incref(t.ptr());
	}
};

void py_export_coordinate_systems() {

	boost::python::to_python_converter<
		Vector3,
		custom_vector_to_python_tuple>();

	class_<CoordinateSystem, boost::noncopyable >("CoordinateSystem", no_init)
	                         ;
	class_<Cartesian, bases<CoordinateSystem> >("Cartesian", init<>())
		;
	class_<Cylindrical, bases<CoordinateSystem> >("Cylindrical", init<>())
		;
	class_<SphericalGalactic, bases<CoordinateSystem> >("SphericalGalactic", init<>())
		.def("ex", &SphericalGalactic::ex)
		.def("ey", &SphericalGalactic::ey)
		.def("ez", &SphericalGalactic::ez)
	;

	class_<Coordinate>("Coordinate", init<double,double,double, CoordinateSystem*>())
		.def("to_cartesian", &Coordinate::to_cartesian)
		;
	class_<VelocityCoordinate>("VelocityCoordinate", init<double,double,double, CoordinateSystem*>())
		.def("to_cartesian", &VelocityCoordinate::to_cartesian)
		;
//class_<Position>("Position", init<double,double,double, CoordinateSystem*>())
	//	.def("to_cartesian", &Coordinate::to_cartesian)
	//	;

	class_<ReferenceFrameBase, boost::noncopyable  >("ReferenceFrameBase", no_init)
		;

	class_<Position>("Position", init<Coordinate*, ReferenceFrameBase*>())
		.def("to", &Position::to)
		.def("to_coordinate_system", &Position::to_coordinate_system)
		.def("to_global", &Position::to_global)
		;
	class_<Velocity>("Velocity", init<VelocityCoordinate*, ReferenceFrameBase*, Position*, ReferenceFrameBase*>())
		.def("to", &Velocity::to)
		.def("to_coordinate_system", &Velocity::to_coordinate_system)
		.def("to_global", &Velocity::to_global)
		;

	class_<ZeroReferenceFrame, bases<ReferenceFrameBase> >("ZeroReferenceFrame", init<>())
		;
	class_<ReferenceFrame, bases<ReferenceFrameBase> >("ReferenceFrame", init<Position*, ReferenceFrameBase*>())
		;
	class_<RotatedReferenceFrame, bases<ReferenceFrameBase> >("RotatedReferenceFrame", init<double, ReferenceFrameBase*>())
		;
	class_<MovingReferenceFrame, bases<ReferenceFrameBase> >("MovingReferenceFrame", init<Velocity*, ReferenceFrameBase*>())
		;
	class_<EqReferenceFrame, bases<ReferenceFrameBase> >("EqReferenceFrame", init<double, double, double, ReferenceFrameBase*>())
		;
//class_<TranslatedCoordinateSystem, bases<CoordinateSystem> >("TranslatedCoordinateSystem", init<double, double, double, CoordinateSystem*>())
	//	;
	//class_<Cylindrical , bases<CoordinateSystem> >("Cylindrical", init<Position*>())
	//	;
	//class_<RotatedCoordinateSystem, bases<CoordinateSystem> >("RotatedCoordinateSystem", init<CoordinateSystem*>())
	//;

}

}


//
// Copyright (c) 2019 INRIA
//

#ifndef __pinocchio_math_matrix_block_hpp__
#define __pinocchio_math_matrix_block_hpp__

#include <Eigen/Core>

namespace pinocchio
{
  template<int NV>
  struct SizeDepType
  {
    template<class Mat>
    struct SegmentReturn
    {
      typedef typename Mat::template FixedSegmentReturnType<NV>::Type Type;
      typedef typename Mat::template ConstFixedSegmentReturnType<NV>::Type ConstType;
    };

    template<typename D>
    static typename SegmentReturn<D>::ConstType
    segment(const Eigen::MatrixBase<D> & mat,
            typename Eigen::DenseBase<D>::Index start,
            typename Eigen::DenseBase<D>::Index size = NV)
    {
      PINOCCHIO_UNUSED_VARIABLE(size);
      return mat.template segment<NV>(start);
    }

    template<typename D>
    static typename SegmentReturn<D>::Type
    segment(Eigen::MatrixBase<D> & mat,
            typename Eigen::DenseBase<D>::Index start,
            typename Eigen::DenseBase<D>::Index size = NV)
    {
      PINOCCHIO_UNUSED_VARIABLE(size);
      return mat.template segment<NV>(start);
    }

    template<class Mat>
    struct ColsReturn
    {
      typedef typename Mat::template NColsBlockXpr<NV>::Type Type;
      typedef typename Mat::template ConstNColsBlockXpr<NV>::Type ConstType;
    };

    template<typename D>
    static typename ColsReturn<D>::ConstType
    middleCols(const Eigen::MatrixBase<D> & mat,
               typename Eigen::DenseBase<D>::Index start,
               typename Eigen::DenseBase<D>::Index size = NV)
    {
      PINOCCHIO_UNUSED_VARIABLE(size);
      return mat.template middleCols<NV>(start);
    }

    template<typename D>
    static typename ColsReturn<D>::Type
    middleCols(Eigen::MatrixBase<D> & mat,
               typename Eigen::DenseBase<D>::Index start,
               typename Eigen::DenseBase<D>::Index size = NV)
    {
      PINOCCHIO_UNUSED_VARIABLE(size);
      return mat.template middleCols<NV>(start);
    }

    template<class Mat>
    struct RowsReturn
    {
      typedef typename Mat::template NRowsBlockXpr<NV>::Type Type;
      typedef typename Mat::template ConstNRowsBlockXpr<NV>::Type ConstType;
    };

    template<typename D>
    static typename RowsReturn<D>::ConstType
    middleRows(const Eigen::MatrixBase<D> & mat,
               typename Eigen::DenseBase<D>::Index start,
               typename Eigen::DenseBase<D>::Index size = NV)
    {
      PINOCCHIO_UNUSED_VARIABLE(size);
      return mat.template middleRows<NV>(start);
    }

    template<typename D>
    static typename RowsReturn<D>::Type
    middleRows(Eigen::MatrixBase<D> & mat,
               typename Eigen::DenseBase<D>::Index start,
               typename Eigen::DenseBase<D>::Index size = NV)
    {
      PINOCCHIO_UNUSED_VARIABLE(size);
      return mat.template middleRows<NV>(start);
    }

    template<class Mat>
    struct BlockReturn
    {
      typedef Eigen::Block<Mat, NV, NV> Type;
      typedef const Eigen::Block<const Mat, NV, NV> ConstType;
    };

    template<typename D>
    static typename BlockReturn<D>::ConstType
    block(const Eigen::MatrixBase<D> & mat,
          typename Eigen::DenseBase<D>::Index row_id,
          typename Eigen::DenseBase<D>::Index col_id,
          typename Eigen::DenseBase<D>::Index row_size_block = NV,
          typename Eigen::DenseBase<D>::Index col_size_block = NV)
    {
      PINOCCHIO_UNUSED_VARIABLE(row_size_block);
      PINOCCHIO_UNUSED_VARIABLE(col_size_block);
      return mat.template block<NV,NV>(row_id,col_id);
    }

    template<typename D>
    static typename BlockReturn<D>::Type
    block(Eigen::MatrixBase<D> & mat,
          typename Eigen::DenseBase<D>::Index row_id,
          typename Eigen::DenseBase<D>::Index col_id,
          typename Eigen::DenseBase<D>::Index row_size_block = NV,
          typename Eigen::DenseBase<D>::Index col_size_block = NV)
    {
      PINOCCHIO_UNUSED_VARIABLE(row_size_block);
      PINOCCHIO_UNUSED_VARIABLE(col_size_block);
      return mat.template block<NV,NV>(row_id,col_id);
    }
  };

  template<>
  struct SizeDepType<Eigen::Dynamic>
  {
    template<class Mat>
    struct SegmentReturn
    {
      typedef typename Mat::SegmentReturnType Type;
      typedef typename Mat::ConstSegmentReturnType ConstType;
    };

    template<typename D>
    static typename SegmentReturn<D>::ConstType
    segment(const Eigen::MatrixBase<D> & mat,
            typename Eigen::DenseBase<D>::Index start,
            typename Eigen::DenseBase<D>::Index size)
    {
      return mat.segment(start,size);
    }

    template<typename D>
    static typename SegmentReturn<D>::Type
    segment(Eigen::MatrixBase<D> & mat,
            typename Eigen::DenseBase<D>::Index start,
            typename Eigen::DenseBase<D>::Index size)
    {
      return mat.segment(start,size);
    }

    template<class Mat>
    struct ColsReturn
    {
      typedef typename Mat::ColsBlockXpr Type;
      typedef typename Mat::ConstColsBlockXpr ConstType;
    };

    template<typename D>
    static typename ColsReturn<D>::ConstType
    middleCols(const Eigen::MatrixBase<D> & mat,
               typename Eigen::DenseBase<D>::Index start,
               typename Eigen::DenseBase<D>::Index size)
    {
      return mat.middleCols(start,size);
    }

    template<typename D>
    static typename ColsReturn<D>::Type
    middleCols(Eigen::MatrixBase<D> & mat,
               typename Eigen::DenseBase<D>::Index start,
               typename Eigen::DenseBase<D>::Index size)
    {
      return mat.middleCols(start,size);
    }

    template<class Mat>
    struct RowsReturn
    {
      typedef typename Mat::RowsBlockXpr Type;
      typedef typename Mat::ConstRowsBlockXpr ConstType;
    };

    template<typename D>
    static typename RowsReturn<D>::ConstType
    middleRows(const Eigen::MatrixBase<D> & mat,
               typename Eigen::DenseBase<D>::Index start,
               typename Eigen::DenseBase<D>::Index size)
    {
      return mat.middleRows(start,size);
    }

    template<typename D>
    static typename RowsReturn<D>::Type
    middleRows(Eigen::MatrixBase<D> & mat,
               typename Eigen::DenseBase<D>::Index start,
               typename Eigen::DenseBase<D>::Index size)
    {
      return mat.middleRows(start,size);
    }

    template<class Mat>
    struct BlockReturn
    {
      typedef Eigen::Block<Mat> Type;
      typedef const Eigen::Block<const Mat> ConstType;
    };

    template<typename D>
    static typename BlockReturn<D>::ConstType
    block(const Eigen::MatrixBase<D> & mat,
          typename Eigen::DenseBase<D>::Index row_id,
          typename Eigen::DenseBase<D>::Index col_id,
          typename Eigen::DenseBase<D>::Index row_size_block,
          typename Eigen::DenseBase<D>::Index col_size_block)
    {
      return mat.block(row_id,col_id,row_size_block,col_size_block);
    }

    template<typename D>
    static typename BlockReturn<D>::Type
    block(Eigen::MatrixBase<D> & mat,
          typename Eigen::DenseBase<D>::Index row_id,
          typename Eigen::DenseBase<D>::Index col_id,
          typename Eigen::DenseBase<D>::Index row_size_block,
          typename Eigen::DenseBase<D>::Index col_size_block)
    {
      return mat.block(row_id,col_id,row_size_block,col_size_block);
    }
  };
}

#endif // ifndef __pinocchio_math_matrix_block_hpp__


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// 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 "bempp/common/config_trilinos.hpp"

#include "abstract_boundary_operator_composition.hpp"

#include "abstract_boundary_operator_pseudoinverse.hpp"
#include "boundary_operator.hpp"
#include "context.hpp"
#include "discrete_boundary_operator_composition.hpp"
#include "identity_operator.hpp"

#include "../common/boost_make_shared_fwd.hpp"
#include "../common/shared_ptr.hpp"
#include "../fiber/explicit_instantiation.hpp"

#ifdef WITH_TRILINOS

#include <boost/make_shared.hpp>
#endif

namespace Bempp {

template <typename BasisFunctionType, typename ResultType>
AbstractBoundaryOperatorComposition<BasisFunctionType, ResultType>::
    AbstractBoundaryOperatorComposition(
        const BoundaryOperator<BasisFunctionType, ResultType> &outer,
        const BoundaryOperator<BasisFunctionType, ResultType> &inner,
        int symmetry)
    : Base(inner.domain(), outer.range(), outer.dualToRange(),
           "(" + outer.label() + ") * (" + inner.label() + ")", symmetry),
      m_outer(outer), m_inner(inner) {
  assert(m_outer.abstractOperator());
  assert(m_inner.abstractOperator());

  if (!(m_outer.domain()->spaceIsCompatible(*m_inner.range())))
    throw std::invalid_argument(
        "AbstractBoundaryOperatorComposition::"
        "AbstractBoundaryOperatorComposition(" +
        m_outer.label() + ", " + m_inner.label() +
        "): Domain of the outer term must be compatible to the range of "
        "the inner term");
}

template <typename BasisFunctionType, typename ResultType>
bool
AbstractBoundaryOperatorComposition<BasisFunctionType, ResultType>::isLocal()
    const {
  return (m_outer.abstractOperator()->isLocal() &&
          m_inner.abstractOperator()->isLocal());
}

template <typename BasisFunctionType, typename ResultType>
shared_ptr<DiscreteBoundaryOperator<ResultType>>
AbstractBoundaryOperatorComposition<BasisFunctionType, ResultType>::
    assembleWeakFormImpl(const Context<BasisFunctionType, ResultType> &context)
    const {
  typedef BoundaryOperator<BasisFunctionType, ResultType> BoundaryOp;
  typedef DiscreteBoundaryOperator<ResultType> DiscreteLinOp;

  shared_ptr<const DiscreteLinOp> discreteOuter = m_outer.weakForm();
  shared_ptr<const DiscreteLinOp> discreteInner = m_inner.weakForm();

  // Calculate the (pseudo)inverse mass matrix
  BoundaryOp id = identityOperator(
      // We don't need a persistent shared_ptr since identityOperator
      // will go out of scope at the end of this function anyway.
      // All we need is a weak form.
      make_shared_from_ref(context), m_inner.range(), m_inner.range(),
      m_inner.dualToRange());
  BoundaryOp pinvId = pseudoinverse(id, m_inner.dualToRange());
  // Dual space not important here. Could be anything.

  shared_ptr<const DiscreteLinOp> temp =
      boost::make_shared<DiscreteBoundaryOperatorComposition<ResultType>>(
          pinvId.weakForm(), discreteInner);
  return boost::make_shared<DiscreteBoundaryOperatorComposition<ResultType>>(
      discreteOuter, temp);
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS_AND_RESULT(
    AbstractBoundaryOperatorComposition);

} // namespace Bempp


#ifndef BSPLINEFITTERIMPL_HPP_
#define BSPLINEFITTERIMPL_HPP_

#include "DiffManifoldBSplineTools.hpp"
#include "../DynamicOrTemplateInt.hpp"
#include "../DiffManifoldBSpline.hpp"

#include <memory>
#include <map>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>
#include <sparse_block_matrix/linear_solver_cholmod.h>

namespace bsplines{
#define _TEMPLATE template <typename TSpline>
#define _CLASS BSplineFitter <TSpline>

//TODO improve : support different containers of times and points
_TEMPLATE
inline void _CLASS::initUniformSpline(TSpline & spline, const std::vector<time_t> & times, const std::vector<point_t> & points, int numSegments, double lambda, FittingBackend backend){
	SM_ASSERT_GE(Exception, times.size(), 2, "There must be at least two time point pairs");
	initUniformSpline(spline, times.front(), times.back(), times, points, numSegments, lambda, backend);
}

_TEMPLATE
inline void _CLASS::initUniformSpline(TSpline & spline, time_t minT, time_t maxT, const std::vector<time_t> & times, const std::vector<point_t> & points, int numSegments, double lambda, FittingBackend backend){
	spline.assertConstructing();
	SM_ASSERT_GE(Exception, times.front(), minT, "Times must all be >= minT");
	SM_ASSERT_LE(Exception, times.back(), maxT, "Times must all be <= maxT");
	IntervalUniformKnotGenerator<TimePolicy> knotGenerator(spline.getSplineOrder(), minT, maxT, numSegments);
	initSpline(spline, knotGenerator, times, points, lambda, backend);
}

_TEMPLATE
inline DeltaUniformKnotGenerator<typename _CLASS::TimePolicy> _CLASS::initUniformSplineWithKnotDelta(TSpline & spline, const std::vector<time_t> & times, const std::vector<point_t> & points, const duration_t knotDelta, double lambda, FittingBackend backend){
	spline.assertConstructing();
	SM_ASSERT_GE(Exception, times.size(), 2, "There must be at least two time point pairs");
	DeltaUniformKnotGenerator<TimePolicy> knotGenerator(spline.getSplineOrder(), *times.begin(), *--times.end(), knotDelta);
	initSpline(spline, knotGenerator, times, points, lambda, backend);
	return knotGenerator;
}

namespace internal{
	template<typename T>
	class PointerGuard {
		bool _deleteIt;
		T * _ptr;
	 public:
		PointerGuard() : _deleteIt(false), _ptr(nullptr) {}
		inline void init(T* ptr, bool deleteIt = true) {
			_deleteIt = deleteIt;
			_ptr = ptr;
		}
		inline ~PointerGuard(){
			if(_deleteIt) delete _ptr;
		}
		inline T& operator *(){
			return *_ptr;
		}
	};

	template<typename TTime>
	struct MapKnotIndexResolver : public KnotIndexResolver<TTime>{
		std::map<TTime, int> knots;
		inline int getKnotIndexAtTime(TTime t) const { return (--knots.upper_bound(t))->second; };
	};

	template <typename TException, typename TTime>
	inline void checkTimesBounds(const std::vector<TTime>& times)
	{
		if(times.size() > 1){
			TTime min = *times.begin(), max=*--times.end();
			for(TTime t : times){
				SM_ASSERT_LE(TException, min, t, "The time sequence must be bounded by its bounding elements. But " << t << " is smaller than the first element :" << min);
				SM_ASSERT_LE(TException, t, max, "The time sequence must be bounded by its bounding elements. But " << t << " is bigger than the last element :" << max);
			}
		}
	}
}

	_TEMPLATE
	void _CLASS::initSpline(TSpline & spline, KnotGenerator<time_t> & knotGenerator, const std::vector<time_t> & times, const std::vector<point_t> & points, double lambda, FittingBackend fittingBackend){
		spline.assertConstructing();
		const size_t numPoints = points.size();

		SM_ASSERT_EQ(Exception, times.size(), numPoints, "The number of times and the number of points must be equal");
		SM_ASSERT_GE(Exception, numPoints, 2, "There must be at least two time point pairs");
		internal::checkTimesBounds<Exception>(times);

		internal::PointerGuard<const KnotIndexResolver<time_t> > resolverPtr;

		if(knotGenerator.hasKnotIndexResolver()){
			for(size_t i = 0; knotGenerator.hasNextKnot(); i++){
				spline.addKnot(knotGenerator.getNextKnot());
			}
			resolverPtr.init(&knotGenerator.getKnotIndexResolver(), false);
		}else{
			auto mapResolver = new internal::MapKnotIndexResolver<time_t>();
			resolverPtr.init(mapResolver, true);
			for(size_t i = 0; knotGenerator.hasNextKnot(); i++){
				time_t nextKnot = knotGenerator.getNextKnot();
				spline.addKnot(nextKnot);
				mapResolver->knots.insert(std::make_pair(nextKnot, i));
			}
		}

		spline.init();

		calcFittedControlVertices(spline, *resolverPtr, times, points, std::function<scalar_t(int) > (), lambda, 0, fittingBackend);
	}

	_TEMPLATE
	void _CLASS::fitSpline(TSpline & spline, const std::vector<time_t> & times, const std::vector<point_t> & points, double lambda, int fixNFirstRelevantControlVertices, std::function<scalar_t(int i) > weights, FittingBackend fittingBackend, const bool calculateControlVertexOffsets){
		spline.assertEvaluable();
		const size_t numPoints = points.size();
		SM_ASSERT_GE(Exception, numPoints, 1, "The must be at least one time point pair!");
		SM_ASSERT_EQ(Exception, times.size(), numPoints, "The number of times and the number of points must be equal");
		internal::checkTimesBounds<Exception>(times);

		auto lastTime = times[numPoints - 1];
		SM_ASSERT_GE(Exception, times[0], spline.getMinTime(), "The values in times may not exceed the time domain of the spline.");
		SM_ASSERT_LE(Exception, lastTime, spline.getMaxTime(), "The values in times may not exceed the time domain of the spline.");

		internal::MapKnotIndexResolver<time_t> mapResolver;
		int i = 0;
		for(auto it = spline.getFirstRelevantSegmentByLast(spline.getSegmentIterator(times[0])), end = spline.getAbsoluteEnd(); it != end; ++it){
			time_t nextKnot = it->getKnot();
			if(nextKnot > lastTime) break;
			spline.addKnot(nextKnot);
			mapResolver.knots.insert(std::make_pair(nextKnot, i++));
		}

		calcFittedControlVertices(spline, mapResolver, times, points, weights, lambda, fixNFirstRelevantControlVertices, fittingBackend, calculateControlVertexOffsets);
	}

namespace internal{
	template<typename TSpline>
	unsigned int getNumberOfRelevantControlVertices(TSpline& spline, typename TSpline::time_t startTime, const typename TSpline::time_t& upToTime, std::function<void(typename TSpline::SegmentIterator it)> apply = std::function<void(typename TSpline::SegmentIterator it)>())
	{
		unsigned int fixFirstVertices = 0;
		for (auto it = spline.getFirstRelevantSegmentByLast(spline.getSegmentIterator(startTime));it->getKnot() < upToTime;fixFirstVertices++, it++) {
			if(apply) apply(it);
		}
		return fixFirstVertices;
	}
}

	_TEMPLATE
	void _CLASS::extendAndFitSpline(TSpline & spline, KnotGenerator<time_t> & knotGenerator, const std::vector<time_t> & times, const std::vector<point_t> & points, double lambda, unsigned char honorCurrentValuePercentage, FittingBackend fittingBackend, const bool calculateControlVertexOffsets){
		const size_t numPoints = points.size();
		SM_ASSERT_TRUE(Exception, knotGenerator.supportsAppending(), "The knot generator must support appending!");
		SM_ASSERT_GE(Exception, numPoints, 1, "The must be at least one time point pair!");
		SM_ASSERT_EQ(Exception, times.size(), numPoints, "The number of times and the number of points must be equal");
		SM_ASSERT_LE(Exception, honorCurrentValuePercentage, 100, "honorCurrentValuePercentage must be in [0, 100]");

		// first we get the time to which to extend the spline.
		const time_t lastTime = times[times.size() - 1];

		const time_t oldMaxTime = spline.getMaxTime();
		if(lastTime > oldMaxTime){
			knotGenerator.extendBeyondTime(lastTime);
			spline.appendSegments(knotGenerator, -1, nullptr);
		}

		if(honorCurrentValuePercentage == 0 || honorCurrentValuePercentage == 100){
			unsigned int fixFirstVertices;
			if(honorCurrentValuePercentage) {
				if(lastTime > oldMaxTime){
					fixFirstVertices = internal::getNumberOfRelevantControlVertices(spline, times[0], oldMaxTime);
				} else {
					// no new vertices but old should not be touched
					return;
				}
			}
			else{// ignore former values
				fixFirstVertices = 0;
			}
			fitSpline(spline, times, points, lambda, (int)fixFirstVertices, std::function<scalar_t(int i) >(), fittingBackend, calculateControlVertexOffsets);
		}else{
			std::vector<scalar_t> weights;
			scalar_t baseWeight = scalar_t(honorCurrentValuePercentage) / scalar_t(100 - honorCurrentValuePercentage);

			std::vector<time_t> newTimes;
			std::vector<point_t> newPoints;
			const time_t minTime = spline.getMinTime();

			const unsigned int relevantOldVCs = internal::getNumberOfRelevantControlVertices<TSpline>(spline, times[0], oldMaxTime,
				[&](typename TSpline::SegmentIterator it)
				{
					it++;
					time_t t(it->getKnot());
					if(t >= minTime){
						newTimes.push_back(t);
						newPoints.push_back(spline.template getEvaluatorAt<0>(t).eval());
						weights.push_back(baseWeight); //TODO improve: normalize the weight base on the amount of points given per segment.
					}
				}
			);
			//TODO optimize: use better ways to concatenate times and points.
			SM_ASSERT_EQ_DBG(std::runtime_error, newTimes.size(), relevantOldVCs, "BUG in "  << __FILE__ << ":" << __LINE__);
			SM_ASSERT_EQ_DBG(std::runtime_error, newPoints.size(), relevantOldVCs, "BUG in "  << __FILE__ << ":" << __LINE__);

			newTimes.resize(times.size() + relevantOldVCs);
			copy(times.begin(), times.end(), newTimes.begin() + relevantOldVCs);
			newPoints.resize(points.size() + relevantOldVCs);
			copy(points.begin(), points.end(), newPoints.begin() + relevantOldVCs);

			fitSpline(spline, newTimes, newPoints, lambda, 0, [relevantOldVCs, &weights](int i){ return i < (int)relevantOldVCs ? weights[i]: scalar_t(1.0);}, fittingBackend, calculateControlVertexOffsets);
		}
	}

	_TEMPLATE
	void _CLASS::calcFittedControlVertices(TSpline & spline, const KnotIndexResolver<time_t> & knotIndexResolver, const std::vector<time_t> & times, const std::vector<point_t> & points, std::function<scalar_t(int i) > weights, double lambda, int fixNFirstRelevantControlVertices, FittingBackend fittingBackend, const bool calculateControlVertexOffsets){
		switch(fittingBackend){
			case FittingBackend::DENSE:
				calcFittedControlVertices<FittingBackend::DENSE>(spline, knotIndexResolver, times, points, weights, lambda, fixNFirstRelevantControlVertices, calculateControlVertexOffsets);
				break;
			case FittingBackend::SPARSE:
				calcFittedControlVertices<FittingBackend::SPARSE>(spline, knotIndexResolver, times, points, weights, lambda, fixNFirstRelevantControlVertices, calculateControlVertexOffsets);
				break;
		}
	}

	namespace internal {
		template< enum FittingBackend FittingBackend_> struct FittingBackendFunctions {
		};

		template<> struct FittingBackendFunctions<FittingBackend::DENSE> {
			typedef typename FittingBackendTraits<FittingBackend::DENSE>::Matrix Matrix;
			typedef typename FittingBackendTraits<FittingBackend::DENSE>::Vector Vector;

			inline static Matrix createA(unsigned int constraintSize, unsigned int coefficientDim, unsigned int /* D */) {
				return Matrix::Zero(constraintSize, coefficientDim);
			}
			inline static Vector createB(int constraintSize) {
				return Vector::Zero(constraintSize);
			}

			inline static auto blockA(Matrix & A, int row, int col, int D, int size = 1) -> decltype(A.block(0, 0, 0, 0)){
				return A.block(row * D, col * D, size * D, size * D);
			}
			inline static typename Vector::SegmentReturnType segmentB(Vector & b, int row, int D){
				return b.segment(row * D, D);
			}
			template <typename DERIVED, typename DERIVED2>
			inline static void addBlockToBlock(Eigen::MatrixBase<DERIVED> & A, int aRow, int aCol, const Eigen::MatrixBase<DERIVED2> & Q, int qRow, int qCol, int rows, int cols, int D, bool segmentInRowVector){
				rows *= D;
				if(segmentInRowVector){
					cols = 1;
				}
				else{
					cols *= D;
				}
				A.block(aRow * D, aCol * D, rows, cols) += Q.block(qRow * D, qCol * D, rows, cols);
			}

			inline static Eigen::VectorXd solve(Matrix & A, Vector & b){
				return A.ldlt().solve(b);
			}
		};

		template<> struct FittingBackendFunctions<FittingBackend::SPARSE> {
			typedef typename FittingBackendTraits<FittingBackend::SPARSE>::Matrix Matrix;
			typedef typename FittingBackendTraits<FittingBackend::SPARSE>::Vector Vector;

			std::vector<int> rows;
			std::vector<int> cols;

			const static bool allocateBlock = true;

			inline Matrix createA(unsigned int constraintSize, unsigned int coefficientDim, unsigned int D) {
				for(unsigned int i = D; i <= constraintSize; i+=D) rows.push_back(i);
				for(unsigned int i = D; i <= coefficientDim; i+=D) cols.push_back(i);
				return Matrix(rows,cols, true);
			}

			inline Vector createB(int /* constraintSize */) {
				std::vector<int> bcols(1);
				bcols[0] = 1;
				return Vector(rows,bcols, true);
			}

			inline static typename Matrix::SparseMatrixBlock & blockA(Matrix & A, int row, int col, int /* D */, int size = 1){
				SM_ASSERT_EQ(std::runtime_error, 1, size, "This function should does not support size != 1");
				return *A.block(row, col, allocateBlock);
			}
			inline static typename Vector::SparseMatrixBlock & segmentB(Vector & b, int row, int /* D */){
				return *b.block(row, 0, allocateBlock);
			}
			template <typename DERIVED>
			inline static void addBlockToBlock(Matrix & A, int aRow, int aCol, const Eigen::MatrixBase<DERIVED> & Q, int qRow, int qCol, int rows, int cols, int D, bool segmentInRowVector){
				for(int i = 0; i < rows; ++i){
					for(int j = 0; j < cols; ++j){
						*A.block(aRow + i, aCol + j, true) += Q.block((qRow + i) * D, (qCol + j) * D, D, segmentInRowVector ? 1 : D);
					}
				}
			}

			inline static Eigen::VectorXd solve(Matrix & A, Vector & b){
				// solve:
				sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd> solver;
				solver.init();

				Eigen::VectorXd c(A.rows());
				c.setZero();
				Eigen::VectorXd b_dense = b.toDense();
				bool result = solver.solve(A,&c[0],&b_dense[0]);
				if(!result) {
					c.setZero();
					// fallback => use nonsparse solver:
					std::cout << "Fallback to Dense Solver" << std::endl;
					Eigen::MatrixXd Adense = A.toDense();
					c = Adense.ldlt().solve(b_dense);
				}
				return c;
			}
		};
	}

	_TEMPLATE
	template <enum FittingBackend FittingBackend_>
	void _CLASS::calcFittedControlVertices(TSpline & spline, const KnotIndexResolver<time_t> & knotIndexResolver, const std::vector<time_t> & times, const std::vector<point_t> & points, std::function<scalar_t(int i) > weights, double lambda, int fixNFirstRelevantControlVertices, const bool calculateControlVertexOffsets)
	{
		if(calculateControlVertexOffsets){
			//TODO implement : support calculateControlVertexOffsets in addCurveQuadraticIntegralDiagTo and remove this check!
			SM_ASSERT_EQ(Exception, 0.0, lambda, "control vertex offsets aren't supported together with lambda != 0, yet!");
		}

		SM_ASSERT_GE_DBG(Exception, fixNFirstRelevantControlVertices, 0, "fixNFirstRelevantControlVertices must be nonnegative.");
		const time_t splineMaxTime = spline.getMaxTime(), lastTime = times[times.size() - 1];
		const int splineOrder = spline.getSplineOrder();
		const int minKnotIndex = knotIndexResolver.getKnotIndexAtTime(times[0]) - (splineOrder - 1) - (times[0] == splineMaxTime ? 1 : 0); // get minimal relevant knot's index //TODO improve: remove this irregularity
		const int controlVertexIndexOffset = minKnotIndex + fixNFirstRelevantControlVertices;
		const int numToFitControlVertices = knotIndexResolver.getKnotIndexAtTime(lastTime) - controlVertexIndexOffset + 1 - (lastTime == splineMaxTime ? 1 : 0);//TODO improve: remove this irregularity

		if(numToFitControlVertices <= 0){
			SM_THROW_DBG(Exception, "No control vertices are left over to be optimized.")
			return;
		}

		const int capturedCurrentControlVertices = calculateControlVertexOffsets ? fixNFirstRelevantControlVertices + numToFitControlVertices : fixNFirstRelevantControlVertices;
		const typename TSpline::point_t* currentControlVertices[capturedCurrentControlVertices];
		if(capturedCurrentControlVertices){
			auto it = spline.getFirstRelevantSegmentByLast(spline.getSegmentIterator(times[0]));
			for(int i = 0; i < capturedCurrentControlVertices; ++i){
				currentControlVertices[i] = &it->getControlVertex();
				++it;
			}
		}

		const size_t numPoints = points.size();

		// What is the vector coefficient dimension
		const size_t D = spline.getPointSize();

		// Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
		size_t coefficientDim = (size_t) numToFitControlVertices * D;

		const int constraintSize = numPoints * D;

		internal::FittingBackendFunctions<FittingBackend_> backend;

		auto A = backend.createA(constraintSize, coefficientDim, D);
		auto b = backend.createB(constraintSize);

		int brow = 0;

		// Add the position constraints.
		for(size_t i = 0; i < numPoints; i++)
		{
			time_t time = times[i];
			scalar_t weight = weights? weights(i) : scalar_t(1.0);
			int knotIndex = knotIndexResolver.getKnotIndexAtTime(time) - controlVertexIndexOffset;
			//TODO optimize : a uniform time spline evaluator could be much faster here!
			typename TSpline::SplineOrderVector bi = spline.template getEvaluatorAt<1>(times[i]).getLocalBi();

			if(weight != scalar_t(1.0)){
				bi *= weight;
			}

			knotIndex -= splineOrder - 1; // get first relevant knot's index
			if(time == splineMaxTime){ //TODO improve: remove this irregularity
				knotIndex --;
			}

			for(int j = 0; j < splineOrder; j ++){
				const int col = knotIndex + j;
				if(col >= 0){ // this control vertex is not fixed
					if(bi[j] != 0.0)
						backend.blockA(A, brow, col, D).diagonal().setConstant(bi[j]);
				}
				if(col < 0 || calculateControlVertexOffsets){
					const int vertexIndex = fixNFirstRelevantControlVertices + col;
					SM_ASSERT_GE_DBG(std::runtime_error, vertexIndex, 0, "BUG in BSplineFitter");
					SM_ASSERT_LT_DBG(std::runtime_error, vertexIndex, capturedCurrentControlVertices, "BUG in BSplineFitter");
					backend.segmentB(b, brow, D) -= (*currentControlVertices[vertexIndex]) * bi[j];
				}
			}

			if(knotIndex >= 0 && ! calculateControlVertexOffsets)
				backend.segmentB(b, brow, D) = points[i] * weight;
			else{
				backend.segmentB(b, brow, D) += points[i] * weight;
			}
			++brow;
		}

		b = A.transpose() * b;
		A = A.transpose() * A;

		if(lambda != 0.0){
			// Add the motion constraint.
			point_t W = point_t::Constant(D, lambda);
			auto start = spline.getSegmentIterator(times[0]);
			auto end = spline.getSegmentIterator(lastTime);
			const int moveCount = fixNFirstRelevantControlVertices - (splineOrder - 1);
			const int movedCount = moveIterator(start, moveCount < 0 ? spline.begin() : end, moveCount);
			const int moveEndCount = splineOrder;
			moveIterator(end, spline.end(), moveEndCount);
			if(moveCount - movedCount <= 0){ // did we not reach end?
				addCurveQuadraticIntegralDiagTo<FittingBackend_>(spline, start, end, movedCount, W, 2, A, b);
			}
		}

		// Solve for the coefficient vector.
		Eigen::VectorXd c = backend.solve(A, b);

		if(calculateControlVertexOffsets){
			const auto & manifold = spline.getManifold();
			spline.manipulateControlVertices([&c, D, &manifold](int i, point_t & v) {
					v += c.block(i * D, 0, D, 1);
					manifold.projectIntoManifold(v);
				}, numToFitControlVertices, times[0], fixNFirstRelevantControlVertices);
		}
		else{
			spline.setControlVertices(c, times[0], fixNFirstRelevantControlVertices);
		}
	}

	_TEMPLATE
	template <enum FittingBackend FittingBackend_>
	void _CLASS::addCurveQuadraticIntegralDiagTo(const TSpline & spline, typename TSpline::SegmentConstIterator start, typename TSpline::SegmentConstIterator end, int startIndex, const point_t & Wdiag, int derivativeOrder, typename internal::FittingBackendTraits<FittingBackend_>::Matrix & toMatrix, typename internal::FittingBackendTraits<FittingBackend_>::Vector & toB)
	{
		SM_ASSERT_EQ(Exception,Wdiag.rows(), (int)spline.getPointSize(), "Wdiag must be of control point size");
		SM_ASSERT_EQ(Exception, toMatrix.rows(), toB.rows(), "Wdiag must be of control point size");

		typedef internal::FittingBackendFunctions<FittingBackend_> Backend;

		const int D = spline.getPointSize();
		const int blocksInQ = spline.getSplineOrder();
		SM_ASSERT_EQ(Exception,Wdiag.rows(), D, "Wdiag must be of control point size");

		const auto qiSize = spline.getSplineOrder() * spline.getPointSize();
		typedef decltype(qiSize) QiSize;
		typedef Eigen::Matrix<double, QiSize::VALUE, QiSize::VALUE> Q_T;
		Q_T Q((int)qiSize, (int)qiSize);

		int blockRow = startIndex;
		const int blockRows = toMatrix.rows() / D;
		for(typename TSpline::SegmentConstIterator sIt = start; sIt != end; sIt++)
		{
			if(FittingBackend_ == FittingBackend::DENSE && blockRow >= 0 && blockRow <= blockRows - blocksInQ){ // all splineOrder many diagonal blocks are contained in the toMatrix
				addOrSetSegmentQuadraticIntegralDiag(spline, Wdiag, sIt, derivativeOrder, Backend::blockA(toMatrix, blockRow, blockRow, D, blocksInQ), true);
			} else { // we have to assign sub blocks individually
				addOrSetSegmentQuadraticIntegralDiag<Q_T &>(spline, Wdiag, sIt, derivativeOrder, Q, false);
				/*
				 * Q and toMatrix (=:M) (both square) have shifted block index space. i_Q + brow = i_M.
				 * Lets x denote the coefficients of all non fixed control vertices
				 * first we need the overlapping region:
				 * its left most index in M is : max(0, 0 + brow).
				 * its right most index in M is : min(cols(M) - 1, cols(Q) - 1 + brow).
				 */
				const int overlappingBlocksLeftIndex = std::max(0, blockRow);
				const int overlappingBlocksRightIndex = std::min(blockRows, blocksInQ + blockRow) - 1;
				const int numOverlappingBlocks = overlappingBlocksRightIndex - overlappingBlocksLeftIndex + 1;
				if(numOverlappingBlocks > 0){
					Backend::addBlockToBlock(toMatrix, overlappingBlocksLeftIndex, overlappingBlocksLeftIndex, Q, overlappingBlocksLeftIndex - blockRow, overlappingBlocksLeftIndex - blockRow, numOverlappingBlocks, numOverlappingBlocks, D, false);
					if(numOverlappingBlocks != blocksInQ){
						/*
						 * Now we have to subtract the effect of the fixed control vertices on the acceleration term.
						 *
						 * [Q_l  Q_x  Q_r]  * [ c_l; x; c_r] = b_x
						 * <=> Q_x * x  = b_x - Q_l * c_l - Q_r * c_r
						 *
						 * We start with c_r because sIt corresponds to its bottom at the moment.
						 */
						auto constantVertexIt = sIt;
						const int Q_r_LeftColumnIndex = overlappingBlocksRightIndex - blockRow + 1;
						const int Q_l_RightColumnIndex = - blockRow - 1;
						for(int i = blocksInQ-1; i >= 0; i--) {
							if(i >= Q_r_LeftColumnIndex || i <= Q_l_RightColumnIndex){ // then vertex is fixed and we have to subtract its effect on the right hand side
								Backend::addBlockToBlock(toB, overlappingBlocksLeftIndex, 0, (-Q.block((overlappingBlocksLeftIndex - blockRow) * D, i * D, numOverlappingBlocks * D, D) * constantVertexIt->getControlVertex()).eval(), 0, 0, numOverlappingBlocks, 1, D, true);
							}
							constantVertexIt--;
						}
					}
				}
			}
			blockRow ++;
		}
	}

	_TEMPLATE
	template<typename M_T>
	inline void _CLASS::addOrSetSegmentQuadraticIntegralDiag(const TSpline & spline, const point_t & Wdiag, typename TSpline::SegmentConstIterator segmentIt, int derivativeOrder, M_T toMatrix, bool add)
	{
		const int D = spline.getPointSize();
		const int splineOrder = spline.getSplineOrder();
		SM_ASSERT_GE_LT(Exception, segmentIt.getKnot(), spline.getMinTime(), spline.getMaxTime(), "Out of range");
		SM_ASSERT_EQ(Exception, Wdiag.rows(), D, "Wdiag must be of control point size");

		typename TSpline::SplineOrderSquareMatrix Dm(splineOrder, splineOrder);
		Dm.setZero(splineOrder, splineOrder);
		spline.computeDiiInto(segmentIt, Dm);
		typename TSpline::SplineOrderSquareMatrix V(splineOrder, splineOrder);
		V.setZero(splineOrder, splineOrder);
		spline.computeViInto(segmentIt, V);

		// Calculate the appropriate derivative version of V
		// using the matrix multiplication version of the derivative.
		for(int i = 0; i < derivativeOrder; i++)
		{
			V = (Dm.transpose() * V * Dm).eval();
		}

		const auto splineOrderTimesPointSize = spline.getSplineOrder() * spline.getPointSize();
		typedef decltype(splineOrderTimesPointSize) SplineOrderTimesPointSize;
		typedef Eigen::Matrix<double, SplineOrderTimesPointSize::VALUE, SplineOrderTimesPointSize::VALUE> MType;

		MType WV((int)splineOrderTimesPointSize, (int)splineOrderTimesPointSize);

		WV.setZero(splineOrderTimesPointSize, splineOrderTimesPointSize);

		for(int d = 0; d < D; d++)
		{
			WV.block(splineOrder*d, splineOrder*d, splineOrder, splineOrder) = Wdiag(d) * V;
		}

		MType M((int)splineOrderTimesPointSize, (int)splineOrderTimesPointSize);
		computeMiInto(spline, segmentIt, M);

		if(add) toMatrix += M.transpose() * WV * M;
		else toMatrix = M.transpose() * WV * M;
	}

	_TEMPLATE
	template <typename M_T>
	void _CLASS::computeBijInto(const TSpline & spline, const typename TSpline::SegmentConstIterator & segmentIndex, int columnIndex, M_T B)
	{
		const int D = spline.getPointSize();
		const int splineOrder = spline.getSplineOrder();

		for(int i = 0; i < D; i++)
		{
			B.block(i*splineOrder,i,splineOrder,1) = segmentIndex->getBasisMatrix().col(columnIndex);
		}
	}

	_TEMPLATE
	template <typename M_T>
	void _CLASS::computeMiInto(const TSpline & spline, const typename TSpline::SegmentConstIterator & segmentIndex, M_T & M)
	{
		const int D = spline.getPointSize();
		const int splineOrder = spline.getSplineOrder();
		const int splineOrderTimesPointSize = splineOrder * D;
		M.setZero();

		for(int j = 0; j < splineOrder; j++)
		{
			computeBijInto(spline, segmentIndex, j, M.block(0, j*D, splineOrderTimesPointSize, D));
		}
	}
}

#undef _TEMPLATE
#undef _CLASS

#endif /* BSPLINEFITTERIMPL_HPP_ */