// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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_STRATEGIES_AGNOSTIC_CONVEX_GRAHAM_ANDREW_HPP
#define BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_CONVEX_GRAHAM_ANDREW_HPP

#include <cstddef>
#include <algorithm>
#include <vector>

#include <boost/range.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/strategies/convex_hull.hpp>

#include <boost/geometry/views/detail/range_type.hpp>

#include <boost/geometry/policies/compare.hpp>

#include <boost/geometry/algorithms/detail/for_each_range.hpp>
#include <boost/geometry/views/reversible_view.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace convex_hull
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template
<
    typename InputRange,
    typename RangeIterator,
    typename StrategyLess,
    typename StrategyGreater
>
struct get_extremes
{
    typedef typename point_type<InputRange>::type point_type;

    point_type left, right;

    bool first;

    StrategyLess less;
    StrategyGreater greater;

    inline get_extremes()
        : first(true)
    {}

    inline void apply(InputRange const& range)
    {
        if (boost::size(range) == 0)
        {
            return;
        }

        // First iterate through this range
        // (this two-stage approach avoids many point copies,
        //  because iterators are kept in memory. Because iterators are
        //  not persistent (in MSVC) this approach is not applicable
        //  for more ranges together)

        RangeIterator left_it = boost::begin(range);
        RangeIterator right_it = boost::begin(range);

        for (RangeIterator it = boost::begin(range) + 1;
            it != boost::end(range);
            ++it)
        {
            if (less(*it, *left_it))
            {
                left_it = it;
            }

            if (greater(*it, *right_it))
            {
                right_it = it;
            }
        }

        // Then compare with earlier
        if (first)
        {
            // First time, assign left/right
            left = *left_it;
            right = *right_it;
            first = false;
        }
        else
        {
            // Next time, check if this range was left/right from
            // the extremes already collected
            if (less(*left_it, left))
            {
                left = *left_it;
            }

            if (greater(*right_it, right))
            {
                right = *right_it;
            }
        }
    }
};

template
<
    typename InputRange,
    typename RangeIterator,
    typename Container,
    typename SideStrategy
>
struct assign_range
{
    Container lower_points, upper_points;

    typedef typename point_type<InputRange>::type point_type;

    point_type const& most_left;
    point_type const& most_right;

    inline assign_range(point_type const& left, point_type const& right)
        : most_left(left)
        , most_right(right)
    {}

    inline void apply(InputRange const& range)
    {
        typedef SideStrategy side;

        // Put points in one of the two output sequences
        for (RangeIterator it = boost::begin(range);
            it != boost::end(range);
            ++it)
        {
            // check if it is lying most_left or most_right from the line

            int dir = side::apply(most_left, most_right, *it);
            switch(dir)
            {
                case 1 : // left side
                    upper_points.push_back(*it);
                    break;
                case -1 : // right side
                    lower_points.push_back(*it);
                    break;

                // 0: on line most_left-most_right,
                //    or most_left, or most_right,
                //    -> all never part of hull
            }
        }
    }
};

template <typename Range>
static inline void sort(Range& range)
{
    typedef typename boost::range_value<Range>::type point_type;
    typedef geometry::less<point_type> comparator;

    std::sort(boost::begin(range), boost::end(range), comparator());
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Graham scan strategy to calculate convex hull
\ingroup strategies
\note Completely reworked version inspired on the sources listed below
\see http://www.ddj.com/architect/201806315
\see http://marknelson.us/2007/08/22/convex
 */
template <typename InputGeometry, typename OutputPoint>
class graham_andrew
{
public :
    typedef OutputPoint point_type;
    typedef InputGeometry geometry_type;

private:

    typedef typename cs_tag<point_type>::type cs_tag;

    typedef typename std::vector<point_type> container_type;
    typedef typename std::vector<point_type>::const_iterator iterator;
    typedef typename std::vector<point_type>::const_reverse_iterator rev_iterator;

    class partitions
    {
        friend class graham_andrew;

        container_type m_lower_hull;
        container_type m_upper_hull;
        container_type m_copied_input;
    };

public:
    typedef partitions state_type;

    inline void apply(InputGeometry const& geometry, partitions& state) const
    {
        // First pass.
        // Get min/max (in most cases left / right) points
        // This makes use of the geometry::less/greater predicates

        // For the left boundary it is important that multiple points
        // are sorted from bottom to top. Therefore the less predicate
        // does not take the x-only template parameter (this fixes ticket #6019.
        // For the right boundary it is not necessary (though also not harmful),
        // because points are sorted from bottom to top in a later stage.
        // For symmetry and to get often more balanced lower/upper halves
        // we keep it.

        typedef typename geometry::detail::range_type<InputGeometry>::type range_type;

        typedef typename boost::range_iterator
            <
                range_type const
            >::type range_iterator;

        detail::get_extremes
            <
                range_type,
                range_iterator,
                geometry::less<point_type>,
                geometry::greater<point_type>
            > extremes;
        geometry::detail::for_each_range(geometry, extremes);

        // Bounding left/right points
        // Second pass, now that extremes are found, assign all points
        // in either lower, either upper
        detail::assign_range
            <
                range_type,
                range_iterator,
                container_type,
                typename strategy::side::services::default_strategy<cs_tag>::type
            > assigner(extremes.left, extremes.right);

        geometry::detail::for_each_range(geometry, assigner);

        // Sort both collections, first on x(, then on y)
        detail::sort(assigner.lower_points);
        detail::sort(assigner.upper_points);

        //std::cout << boost::size(assigner.lower_points) << std::endl;
        //std::cout << boost::size(assigner.upper_points) << std::endl;

        // And decide which point should be in the final hull
        build_half_hull<-1>(assigner.lower_points, state.m_lower_hull,
                extremes.left, extremes.right);
        build_half_hull<1>(assigner.upper_points, state.m_upper_hull,
                extremes.left, extremes.right);
    }

    template <typename OutputIterator>
    inline void result(partitions const& state,
                    OutputIterator out, bool clockwise)  const
    {
        if (clockwise)
        {
            output_range<iterate_forward>(state.m_upper_hull, out, false);
            output_range<iterate_reverse>(state.m_lower_hull, out, true);
        }
        else
        {
            output_range<iterate_forward>(state.m_lower_hull, out, false);
            output_range<iterate_reverse>(state.m_upper_hull, out, true);
        }
    }

private:

    template <int Factor>
    static inline void build_half_hull(container_type const& input,
            container_type& output,
            point_type const& left, point_type const& right)
    {
        output.push_back(left);
        for(iterator it = input.begin(); it != input.end(); ++it)
        {
            add_to_hull<Factor>(*it, output);
        }
        add_to_hull<Factor>(right, output);
    }

    template <int Factor>
    static inline void add_to_hull(point_type const& p, container_type& output)
    {
        typedef typename strategy::side::services::default_strategy<cs_tag>::type side;

        output.push_back(p);
        std::size_t output_size = output.size();
        while (output_size >= 3)
        {
            rev_iterator rit = output.rbegin();
            point_type const last = *rit++;
            point_type const& last2 = *rit++;

            if (Factor * side::apply(*rit, last, last2) <= 0)
            {
                // Remove last two points from stack, and add last again
                // This is much faster then erasing the one but last.
                output.pop_back();
                output.pop_back();
                output.push_back(last);
                output_size--;
            }
            else
            {
                return;
            }
        }
    }

    template <iterate_direction Direction, typename OutputIterator>
    static inline void output_range(container_type const& range,
        OutputIterator out, bool skip_first)
    {
        typedef typename reversible_view<container_type const, Direction>::type view_type;
        view_type view(range);
        bool first = true;
        for (typename boost::range_iterator<view_type const>::type it = boost::begin(view);
            it != boost::end(view); ++it)
        {
            if (first && skip_first)
            {
                first = false;
            }
            else
            {
                *out = *it;
                ++out;
            }
        }
    }

};

}} // namespace strategy::convex_hull

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
template <typename InputGeometry, typename OutputPoint>
struct strategy_convex_hull<InputGeometry, OutputPoint, cartesian_tag>
{
    typedef strategy::convex_hull::graham_andrew<InputGeometry, OutputPoint> type;
};
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_CONVEX_GRAHAM_ANDREW_HPP


/*
 * KameleonInterpolator_compute_gradient.cpp
 *
 *  Created on: Sep 28, 2009
 *      Author: David Berrios
 */

#include "KameleonInterpolator.h"
#include <vector>
#include <boost/algorithm/string.hpp>

namespace ccmc
{

	/**
	 * @param variable
	 * @param positionComponent1 The first component of the position
	 * @param positionComponent2 The second component of the position
	 * @param positionComponent3 The third component of the position
	 * @return
	 */
	float KameleonInterpolator::compute_gradient(const std::string& variable, const float& positionComponent1,
			const float& positionComponent2, const float& positionComponent3)
	{
		float dComponent1, dComponent2, dComponent3;
		return compute_gradient(variable, positionComponent1, positionComponent2, positionComponent3, dComponent1,
				dComponent2, dComponent3);
	}

	/**
	 * @param variable
	 * @param c0 The first component of the position
	 * @param c1 The second component of the position
	 * @param c2 The third component of the position
	 * @param dComponent1 The delta of the block size for component1 at the position specified.
	 * @param dComponent2 The delta of the block size for component2 at the position specified.
	 * @param dComponent3 The delta of the block size for component3 at the position specified.
	 * @return
	 */
	float KameleonInterpolator::compute_gradient(const std::string& variable, const float& c0,
			const float& c1, const float& c2, float& dc0, float& dc1,
			float& dc2)
	{
		//parse the string, and attempt to calculate the gradient of the parsed variable

		std::vector<std::string> tokens;
		boost::split(tokens, variable, boost::is_any_of("."), boost::token_compress_on);

		/*t = interpolate("t", positionComponent1, positionComponent2, positionComponent3, dComponent1, dComponent2,
				dComponent3);
		if (t == missingValue)
			return missingValue;
		n = interpolate("n", positionComponent1, positionComponent2, positionComponent3, dComponent1, dComponent2,
				dComponent3);
		if (n == missingValue)
			return missingValue;
		float s = t / pow(n, 2.0f / 3.0f);*/
		//cout << "gradient variable: '" << tokens[1] << "'" << endl;

		//super expensive to calculation this. Try to store the components of the gradient, and speed up the calculations
		//if the components are requested in sequential order
		float pointValue = interpolate(tokens[1],c0,c1,c2,dc0,dc1,dc2);
		float direction;
		float dc0_scaled = dc0*.5f;
		float dc1_scaled = dc1*.5f;
		float dc2_scaled = dc2*.5f;
		float positions_c0[8];
		float positions_c1[8];
		float positions_c2[8];

		positions_c0[0] = c0 + dc0_scaled;
		positions_c0[1] = c0 + dc0_scaled;
		positions_c0[2] = c0 - dc0_scaled;
		positions_c0[3] = c0 - dc0_scaled;
		positions_c0[4] = c0 + dc0_scaled;
		positions_c0[5] = c0 + dc0_scaled;
		positions_c0[6] = c0 - dc0_scaled;
		positions_c0[7] = c0 - dc0_scaled;

		positions_c1[0] = c1 - dc1_scaled;
		positions_c1[1] = c1 - dc1_scaled;
		positions_c1[2] = c1 - dc1_scaled;
		positions_c1[3] = c1 - dc1_scaled;
		positions_c1[4] = c1 + dc1_scaled;
		positions_c1[5] = c1 + dc1_scaled;
		positions_c1[6] = c1 + dc1_scaled;
		positions_c1[7] = c1 + dc1_scaled;

		positions_c2[0] = c2 - dc2_scaled;
		positions_c2[1] = c2 + dc2_scaled;
		positions_c2[2] = c2 + dc2_scaled;
		positions_c2[3] = c2 - dc2_scaled;
		positions_c2[4] = c2 - dc2_scaled;
		positions_c2[5] = c2 + dc2_scaled;
		positions_c2[6] = c2 + dc2_scaled;
		positions_c2[7] = c2 - dc2_scaled;

		//direction and magnitude of greatest change
		float p0 = c0;
		float p1 = c1;
		float p2 = c2;
		float current_max = -999999999999.f;
		for (int i = 0; i < 8; i++)
		{

			direction = interpolate(tokens[1],positions_c0[i],positions_c1[i],positions_c2[i]);
			//cout << "*i: " << i << endl;
			float current_diff = direction - pointValue;
			if (current_diff > current_max )
			{
				current_max = current_diff;
				p0 = positions_c0[i];
				p1 = positions_c1[i];
				p2 = positions_c2[i];
			}
		}

		return current_max;

	}
}


//=============================================================================================================
/**
 * @file     spectrogram.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>
 * @version  dev
 * @date     September, 2015
 *
 * @section  LICENSE
 *
 * Copyright (C) 2015, Lorenz Esch. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 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.
 *
 *
 * @brief    Definition of spectrogram class.
 */

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "spectrogram.h"

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/SparseCore>
#include <unsupported/Eigen/FFT>

//*************************************************************************************************************
//=============================================================================================================
// Qt INCLUDES
//=============================================================================================================

#include <QDebug>
#include <QElapsedTimer>
#include <QThread>
#include <QtConcurrent>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;
using namespace Eigen;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

MatrixXd Spectrogram::makeSpectrogram(VectorXd signal, qint32 windowSize = 0)
{
    //QElapsedTimer timer;
    //timer.start();

    signal.array() -= signal.mean();
    QList<SpectogramInputData> lData;
    int iThreadSize = QThread::idealThreadCount()*2;
    int iStepsSize = signal.rows()/iThreadSize;
    int iResidual = signal.rows()%iThreadSize;

    SpectogramInputData dataTemp;
    dataTemp.vecInputData = signal;
    dataTemp.window_size = windowSize;
    if(dataTemp.window_size == 0) {
        dataTemp.window_size = signal.rows()/15;
    }

    for (int i = 0; i < iThreadSize; ++i) {
        dataTemp.iRangeLow = i*iStepsSize;
        dataTemp.iRangeHigh = i*iStepsSize+iStepsSize;
        lData.append(dataTemp);
    }

    dataTemp.iRangeLow = iThreadSize*iStepsSize;
    dataTemp.iRangeHigh = iThreadSize*iStepsSize+iResidual;
    lData.append(dataTemp);

    QFuture<MatrixXd> resultMat = QtConcurrent::mappedReduced(lData,
                                                              compute,
                                                              reduce);
    resultMat.waitForFinished();

    //qDebug() << "Spectrogram::make_spectrogram - timer.elapsed()" << timer.elapsed();
    return resultMat.result();
}

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

VectorXd Spectrogram::gaussWindow(qint32 sample_count, qreal scale, quint32 translation)
{
    VectorXd gauss = VectorXd::Zero(sample_count);

    for(qint32 n = 0; n < sample_count; n++)
    {
        qreal t = (qreal(n) - translation) / scale;
        gauss[n] = exp(-3.14 * pow(t, 2))*pow(sqrt(scale),(-1))*pow(qreal(2),(0.25));
    }

    return gauss;
}

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

MatrixXd Spectrogram::compute(const SpectogramInputData& inputData)
{
    #ifdef EIGEN_FFTW_DEFAULT
        fftw_make_planner_thread_safe();
    #endif

    Eigen::FFT<double> fft;
    MatrixXd tf_matrix = MatrixXd::Zero(inputData.vecInputData.rows()/2, inputData.vecInputData.rows());
    VectorXd envelope, windowed_sig, real_coeffs;
    VectorXcd fft_win_sig;
    qint32 window_size = inputData.window_size;

    for(quint32 translate = inputData.iRangeLow; translate < inputData.iRangeHigh; translate++) {
        envelope = gaussWindow(inputData.vecInputData.rows(), window_size, translate);

        windowed_sig = VectorXd::Zero(inputData.vecInputData.rows());
        fft_win_sig = VectorXcd::Zero(inputData.vecInputData.rows());

        windowed_sig = inputData.vecInputData.array() * envelope.array();\

        fft.fwd(fft_win_sig, windowed_sig);

        real_coeffs = fft_win_sig.segment(0,inputData.vecInputData.rows()/2).array().abs2();

        tf_matrix.col(translate) = real_coeffs;
    }

    return tf_matrix;
}

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

void Spectrogram::reduce(MatrixXd &resultData,
                         const MatrixXd &data)
{
    if(resultData.size() == 0) {
        resultData = data;
    } else {
        resultData += data;
    }
}


//=======================================================================
// Copyright (c) 2013 Attila Bernath, Piotr Wygocki
//               2014 Piotr Godlewski, Piotr Smulewicz
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file tree_augmentation.hpp
 * @brief
 * @author Attila Bernath, Piotr Smulewicz, Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-06-20
 */
#ifndef PAAL_TREE_AUGMENTATION_HPP
#define PAAL_TREE_AUGMENTATION_HPP

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

#include <boost/bimap.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/range/as_array.hpp>
#include <boost/range/distance.hpp>

#include <algorithm>
#include <utility>

namespace paal {
namespace ir {

namespace detail {

/// This function returns the number of edges in a graph. The
/// reason this function was written is that BGL's num_edges()
/// function does not work properly for filtered_graph.  See
/// http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/filtered_graph.html#2
template <class EdgeListGraph>
int filtered_num_edges(const EdgeListGraph & g) {
    return boost::distance(edges(g));
}

/// A class that translates bool map on the edges to a filter, which can be used
/// with
/// boost::filtered_graph. That is, we translate operator[] to
/// operator().  We do a little more: we will also need the negated
/// version of the map (for the graph of the non-tree edges).
template <typename EdgeBoolMap> struct bool_map_to_tree_filter {
    bool_map_to_tree_filter() {}

    bool_map_to_tree_filter(EdgeBoolMap m) : ebmap(m) {}

    template <typename Edge> bool operator()(const Edge &e) const {
        return get(ebmap, e);
    }

    EdgeBoolMap ebmap;
};

template <typename EdgeBoolMap> struct bool_map_to_non_tree_filter {
    bool_map_to_non_tree_filter() {}

    bool_map_to_non_tree_filter(EdgeBoolMap m) : ebmap(m) {}

    template <typename Edge> bool operator()(const Edge &e) const {
        return !get(ebmap, e);
    }

    EdgeBoolMap ebmap;
};

/// A boost graph map that returns a constant integer value.
template <typename KeyType, int num>
class const_int_map
    : public boost::put_get_helper<int, const_int_map<KeyType, num>> {
  public:
    using category = boost::readable_property_map_tag;
    using value_type = int;
    using reference = int;
    using key_type = KeyType;
    reference operator[](KeyType e) const { return num; }
};

} // namespace detail

namespace {
struct ta_compare_traits {
    static const double EPSILON;
};

const double ta_compare_traits::EPSILON = 1e-10;
}

/**
 * Round Condition of the IR Tree Augmentation algorithm.
 */
struct ta_round_condition {
    /**
     * Constructor. Takes epsilon used in double comparison.
     */
    ta_round_condition(double epsilon = ta_compare_traits::EPSILON)
        : m_round_half(epsilon) {}

    /**
     * Rounds a column up if it is at least half in the
     * current solution. If the column is rounded, the
     * corresponding edge is added to the result.
     */
    template <typename Problem, typename LP>
    boost::optional<double> operator()(Problem &problem, LP &lp,
                                       lp::col_id col) {
        auto res = m_round_half(problem, lp, col);
        if (res) {
            problem.add_to_solution(col);
        }
        return res;
    }

  private:
    round_condition_greater_than_half m_round_half;
};

/**
 * Set Solution component of the IR Tree Augmentation algorithm.
 */
struct ta_set_solution {
    /**
     * Constructor. Takes epsilon used in double comparison.
     */
    ta_set_solution(double epsilon = ta_compare_traits::EPSILON)
        : m_compare(epsilon) {}

    /**
     * Creates the result form the LP (all edges corresponding to columns with
     * value 1).
     */
    template <typename Problem, typename GetSolution>
    void operator()(Problem &problem, const GetSolution &solution) {
        for (auto e :
             boost::as_array(edges(problem.get_links_graph()))) {
            if (!problem.is_in_solution(e)) {
                auto col = problem.edge_to_col(e);
                if (m_compare.e(solution(col), 1)) {
                    problem.add_to_solution(col);
                }
            }
        }
    }

private:
    const utils::compare<double> m_compare;
};

/**
 * Initialization of the IR Tree Augmentation algorithm.
 */
class ta_init {
  public:
    /**
     * Initialize the cut LP.
     */
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        problem.init();
        lp.set_lp_name("Tree augmentation");
        lp.set_optimization_type(lp::MINIMIZE);

        add_variables(problem, lp);
        add_cut_constraints(problem, lp);
    }

  private:
    /**
     * Adds a variable to the LP for each link in the input graph.
     * Binds the LP columns to graph edges.
     */
    template <typename Problem, typename LP>
    void add_variables(Problem & problem, LP & lp) {
        for (auto e : boost::as_array(edges(problem.get_links_graph()))) {
            lp::col_id col_idx = lp.add_column(problem.get_cost(e), 0);
            problem.bind_edge_to_col(e, col_idx);
        }
    }

    /**
     * Adds a cut constraint to the LP for each edge in the input graph
     * and binds edges to LP rows.
     */
    template <typename Problem, typename LP>
    void add_cut_constraints(Problem &problem, LP &lp) {
        for (auto e :
             boost::as_array(edges(problem.get_tree_graph()))) {
            lp::linear_expression expr;
            for (auto pe : problem.get_covered_by(e)) {
                expr += problem.edge_to_col(pe);
            }

            lp::row_id row_idx = lp.add_row(std::move(expr) >= 1);
            problem.bind_edge_to_row(e, row_idx);
        }
    }
};

template <
    typename Init = ta_init,
    typename RoundCondition = ta_round_condition,
    typename RelaxContition = utils::always_false,
    typename SetSolution = ta_set_solution>
        using tree_augmentation_ir_components = IRcomponents<Init, RoundCondition,
                        RelaxContition, SetSolution>;

/**
 * @brief This is Jain's iterative rounding
 * 2-approximation algorithm for the Generalised Steiner Network
 * Problem, specialized for the Tree Augmentation Problem.
 *
 * The Tree Augmentation Problem is the following. Given a
 * 2-edge connected graph, in which a spanning tree is
 * designated. The non-tree edges are also called links. The
 * links have non-negative costs. The problem is to find a
 * minimum cost subset of links which, together with the
 * tree-edges give a 2-edge-connected graph.
 *
 * @tparam Graph the graph type used
 * @tparam TreeMap it is assumed to be a bool map on the edges of a graph of type Graph.
 *  It is used for designating a spanning tree in the graph.
 * @tparam CostMap type for the costs of the links.
 * @tparam VertexIndex type for the vertex index map.
 * @tparam EdgeSetOutputIterator type for the result edge set.
 */
template <typename Graph, typename TreeMap, typename CostMap,
          typename VertexIndex, typename EdgeSetOutputIterator>
class tree_aug {
  public:
    using Edge = typename boost::graph_traits<Graph>::edge_descriptor;
    using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
    using CostValue = double;

    using TreeGraph =
        boost::filtered_graph<Graph, detail::bool_map_to_tree_filter<TreeMap>>;
    using NonTreeGraph = boost::filtered_graph<
        Graph, detail::bool_map_to_non_tree_filter<TreeMap>>;

    using EdgeList = std::vector<Edge>;
    using CoverMap = std::unordered_map<Edge, EdgeList, edge_hash<Graph>>;

    // cross reference between links and columns
    using EdgeToColId = boost::bimap<Edge, lp::col_id>;
    using RowIdToEdge = std::unordered_map<lp::row_id, Edge>;

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

    /**
     * Constructor.
     *
     * @param g  the graph to work with
     * @param tree_map designate a spanning tree in \c g
     * @param cost_map costs of the links (=non-tree edges). The costs assigned to tree edges are not used.
     * @param vertex_index vertex index map
     * @param solution result set of edges output iterator
     */
    tree_aug(const Graph & g, TreeMap tree_map, CostMap cost_map,
            VertexIndex vertex_index, EdgeSetOutputIterator solution) :
        m_g(g), m_tree_map(tree_map), m_cost_map(cost_map), m_index(vertex_index),
        m_solution(solution),
        m_tree(m_g, detail::bool_map_to_tree_filter<TreeMap>(m_tree_map)),
        m_ntree(m_g, detail::bool_map_to_non_tree_filter<TreeMap>(m_tree_map)),
        m_sol_cost(0)
    {}

    /// Checks validity of the input
    ErrorMessage check_input_validity() {
        // Num of edges == num of nodes-1 in the tree?
        int n_v = num_vertices(m_g);
        int n_e = filtered_num_edges(m_tree);

        if (n_e != n_v - 1) {
            return "Incorrect number of edges in the spanning tree. "
                        + std::string("Should be ") + std::to_string(n_v - 1)
                        + ", but it is " + std::to_string(n_e) + ".";
        }

        // Is the tree connected?
        std::vector<int> component(num_vertices(m_g));
        int num = boost::connected_components(m_tree, &component[0]);
        if (num > 1) {
            return ErrorMessage{ "The spanning tree is not connected." };
        }

        // Is the graph 2-edge-connected?
        detail::const_int_map<Edge, 1> const_1_edge_map;
        // TODO This stoer-wagner algorithm is unnecessarily slow for some reason
        int min_cut = boost::stoer_wagner_min_cut(m_g, const_1_edge_map);
        if (min_cut < 2) {
            return ErrorMessage{"The graph is not 2-edge-connected."};
        }

        return ErrorMessage{};
    }

    /**
     * Returns the non-tree graph (set of links).
     */
    const NonTreeGraph &get_links_graph() const { return m_ntree; }

    /**
     * Returns the spanning tree.
     */
    const TreeGraph &get_tree_graph() const { return m_tree; }

    /**
     * Returns the cost of an edge.
     */
    auto get_cost(Edge e)->decltype(get(std::declval<CostMap>(), e)) {
        return get(m_cost_map, e);
    }

    /**
     * Adds an edge corresponding to the given LP column to the result set.
     */
    void add_to_solution(lp::col_id col) {
        *m_solution = m_edge_to_col_id.right.at(col);
        ++m_solution;
        m_sol_cost += m_cost_map[m_edge_to_col_id.right.at(col)];
        m_edge_to_col_id.right.erase(col);
    }

    /**
     * Binds a graph edge to a LP column.
     */
    void bind_edge_to_col(Edge e, lp::col_id col) {
        auto tmp =
            m_edge_to_col_id.insert(typename EdgeToColId::value_type(e, col));
        assert(tmp.second);
    }

    /**
     * Binds a graph edge to a LP row.
     */
    void bind_edge_to_row(Edge e, lp::row_id row) {
        auto tmp =
            m_row_id_to_edge.insert(typename RowIdToEdge::value_type(row, e));
        assert(tmp.second);
    }

    /**
     * Initializes the necessary data structures.
     */
    void init() {
        // We need to fill very useful auxiliary data structures:
        //\c m_covered_by - containing lists of
        // edges. For a tree edge \c t the list \c m_covered_by[t]
        // contains the list of links covering \c t.

        std::vector<Edge> pred(num_vertices(m_g));
        auto pred_map =
            boost::make_iterator_property_map(pred.begin(), m_index);
        std::set<Vertex> seen;
        for (auto u : boost::as_array(vertices(m_g))) {
            auto tmp = seen.insert(u);
            assert(tmp.second);
            boost::breadth_first_search(
                m_tree, u, boost::visitor(boost::make_bfs_visitor(
                               boost::record_edge_predecessors(
                                   pred_map, boost::on_tree_edge()))));

            for (auto e : boost::as_array(out_edges(u, m_ntree))) {
                auto node = target(e, m_ntree);
                if (!seen.count(node)) {
                    while (node != u) {
                        m_covered_by[get(pred_map, node)].push_back(e);
                        node = source(get(pred_map, node), m_tree);
                    }
                }
            }
        }
    }

    /**
     * Returns the edge corresponding to an LP row.
     */
    Edge row_to_edge(lp::row_id row) const { return m_row_id_to_edge.at(row); }

    /**
     * Returns the LP columnn corresponding to a graph edge.
     */
    lp::col_id edge_to_col(Edge e) const { return m_edge_to_col_id.left.at(e); }

    /**
     * Returns the list of links covering a given edge.
     */
    EdgeList &get_covered_by(Edge e) { return m_covered_by[e]; }

    /**
     * Checks if an edge belongs to the solution.
     */
    bool is_in_solution(Edge e) const {
        return m_edge_to_col_id.left.find(e) == m_edge_to_col_id.left.end();
    }

    /**
     * Returns cost of the found solution.
     */
    CostValue get_solution_cost() const { return m_sol_cost; }

  private:

    /// Input graph
    const Graph &m_g;
    /// Input tree edges map
    TreeMap m_tree_map;
    /// Input edge cost map
    CostMap m_cost_map;
    /// Vertex index map
    VertexIndex m_index;

    /// Which links are chosen in the solution
    EdgeSetOutputIterator m_solution;

    /// Auxiliary data structures
    EdgeToColId m_edge_to_col_id;

    /// The spanning tree
    TreeGraph m_tree;
    /// The non-tree (=set of links)
    NonTreeGraph m_ntree;
    /// Cost of the solution found
    CostValue m_sol_cost;
    /// Structures for the "m_covered_by" relations
    CoverMap m_covered_by;
    /// Reference between tree edges and row ids
    RowIdToEdge m_row_id_to_edge;
};

namespace detail {
/**
 * @brief Creates a tree_aug object. Non-named parameters.
 *
 * @tparam Graph
 * @tparam TreeMap
 * @tparam CostMap
 * @tparam VertexIndex
 * @tparam EdgeSetOutputIterator
 * @param g
 * @param tree_map
 * @param cost_map
 * @param vertex_index
 * @param solution
 *
 * @return tree_aug object
 */
template <typename Graph, typename TreeMap, typename CostMap,
          typename VertexIndex, typename EdgeSetOutputIterator>
tree_aug<Graph, TreeMap, CostMap, VertexIndex, EdgeSetOutputIterator>
make_tree_aug(const Graph & g, TreeMap tree_map, CostMap cost_map,
                VertexIndex vertex_index, EdgeSetOutputIterator solution) {
    return paal::ir::tree_aug<Graph, TreeMap, CostMap,
            VertexIndex, EdgeSetOutputIterator>(g, tree_map, cost_map, vertex_index, solution);
}

/**
 * @brief Solves the Tree Augmentation problem using Iterative Rounding.
* Non-named parameters.
 *
 * @tparam Graph
 * @tparam TreeMap
 * @tparam CostMap
 * @tparam VertexIndex
 * @tparam EdgeSetOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @param g
 * @param tree_map
 * @param cost_map
 * @param vertex_index
 * @param solution
 * @param components
 * @param visitor
 *
 * @return solution status
 */
template <typename Graph, typename TreeMap, typename CostMap,
          typename VertexIndex, typename EdgeSetOutputIterator,
          typename IRcomponents = tree_augmentation_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult tree_augmentation_iterative_rounding(
        const Graph & g,
        TreeMap tree_map,
        CostMap cost_map,
        VertexIndex vertex_index,
        EdgeSetOutputIterator solution,
        IRcomponents components = IRcomponents(),
        Visitor visitor = Visitor()) {
    auto treeaug = make_tree_aug(g, tree_map, cost_map, vertex_index, solution);
    return solve_iterative_rounding(treeaug, std::move(components), std::move(visitor));
}
} // detail

/**
 * Creates a tree_aug object. Named parameters.
 * The returned object can be used to check input validity or to get a lower
* bound on the
 * optimal solution cost.
 *
 * @tparam Graph
 * @tparam EdgeSetOutputIterator
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g
 * @param params
 * @param solution
 *
 * @return tree_aug object
 */
template <typename Graph, typename EdgeSetOutputIterator, typename P,
          typename T, typename R>
auto make_tree_aug(const Graph &g,
                   const boost::bgl_named_params<P, T, R> &params,
                   EdgeSetOutputIterator solution)
    ->tree_aug<
          Graph,
          decltype(choose_const_pmap(get_param(params, boost::edge_color), g,
                                     boost::edge_color)),
          decltype(choose_const_pmap(get_param(params, boost::edge_weight), g,
                                     boost::edge_weight)),
          decltype(choose_const_pmap(get_param(params, boost::vertex_index), g,
                                     boost::vertex_index)),
          EdgeSetOutputIterator> {
    return detail::make_tree_aug(
        g, choose_const_pmap(get_param(params, boost::edge_color), g,
                             boost::edge_color),
        choose_const_pmap(get_param(params, boost::edge_weight), g,
                          boost::edge_weight),
        choose_const_pmap(get_param(params, boost::vertex_index), g,
                          boost::vertex_index),
        solution);
}

/**
 * Creates a tree_aug object. All default parameters.
 * The returned object can be used to check input validity or to get a lower
* bound on the
 * optimal solution cost.
 *
 * @tparam Graph
 * @tparam EdgeSetOutputIterator
 * @param g
 * @param solution
 *
 * @return tree_aug object
 */
template <typename Graph, typename EdgeSetOutputIterator>
auto make_tree_aug(const Graph &g, EdgeSetOutputIterator solution)
    ->decltype(make_tree_aug(g, boost::no_named_parameters(), solution)) {
    return make_tree_aug(g, boost::no_named_parameters(), solution);
}

/**
 * @brief Solves the Tree Augmentation problem using Iterative Rounding. Named
* parameters.
 *
 * @tparam Graph
 * @tparam EdgeSetOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g
 * @param params
 * @param solution
 * @param components
 * @param visitor
 *
 * @return solution status
 */
template <typename Graph, typename EdgeSetOutputIterator,
          typename IRcomponents = tree_augmentation_ir_components<>,
          typename Visitor = trivial_visitor, typename P, typename T,
          typename R>
IRResult tree_augmentation_iterative_rounding(
    const Graph &g, const boost::bgl_named_params<P, T, R> &params,
    EdgeSetOutputIterator solution, IRcomponents components = IRcomponents(),
    Visitor visitor = Visitor()) {
    return detail::tree_augmentation_iterative_rounding(
        g, choose_const_pmap(get_param(params, boost::edge_color), g,
                             boost::edge_color),
        choose_const_pmap(get_param(params, boost::edge_weight), g,
                          boost::edge_weight),
        choose_const_pmap(get_param(params, boost::vertex_index), g,
                          boost::vertex_index),
        std::move(solution), std::move(components), std::move(visitor));
}

/**
 * @brief Solves the Tree Augmentation problem using Iterative Rounding. All
* default parameters.
 *
 * @tparam Graph
 * @tparam EdgeSetOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @param g
 * @param solution
 * @param components
 * @param visitor
 *
 * @return solution status
 */
template <typename Graph, typename EdgeSetOutputIterator,
          typename IRcomponents = tree_augmentation_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult tree_augmentation_iterative_rounding(const Graph &g,
                                              EdgeSetOutputIterator solution,
                                              IRcomponents components =
                                                  IRcomponents(),
                                              Visitor visitor = Visitor()) {
    return tree_augmentation_iterative_rounding(
        g, boost::no_named_parameters(), std::move(solution),
        std::move(components), std::move(visitor));
}

} // ir
} // paal

#endif // PAAL_TREE_AUGMENTATION_HPP


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef SplitMSSM_FullMSSM_THRESHOLDS_H
#define SplitMSSM_FullMSSM_THRESHOLDS_H

#include <Eigen/Core>

/**
 * @file splitmssm_thresholds.hpp
 *
 * Contains function declarations for threshold corrections of the
 * MSSM to the Standard Model or the Split-MSSM from arXiv:1407.4081.
 *
 * Example for matching of the MSSM to the Standard Model:
@code
   Parameters pars;
   // fill parameters ...

   double lambda = lambda_tree_level(pars);

   if (loopLevel > 0) {
      lambda +=
         + delta_lambda_1loop_reg(pars)
         + delta_lambda_1loop_phi(pars)
         + delta_lambda_1loop_chi_1(pars)
         + delta_lambda_1loop_chi_2(pars);
   }

   if (loopLevel > 1)
      lambda += delta_lambda_2loop_phi_HSS(pars);
@endcode
 *
 * Example for matching of the MSSM to the Split-MSSM:
@code
   Parameters pars;
   // fill parameters ...
   // Note: g1 and g2 are defined in the Split-MSSM

   double lambda_split = lambda_tree_level(pars);
   double gYu = gYu_tree_level(pars);
   double gYd = gYd_tree_level(pars);
   double g2u = g2u_tree_level(pars);
   double g2d = g2d_tree_level(pars);

   if (loopLevel > 0) {
      lambda_split +=
         + delta_lambda_1loop_reg(pars)
         + delta_lambda_1loop_phi(pars);
      gYu += delta_gYu_1loop(pars);
      gYd += delta_gYd_1loop(pars);
      g2u += delta_g2u_1loop(pars);
      g2d += delta_g2d_1loop(pars);
   }

   if (loopLevel > 1)
      lambda_split += delta_lambda_2loop_phi(pars);
@endcode
 *
 * Example for matching of the Split-MSSM to the Standard Model:
@code
   Parameters pars;
   // fill parameters ...
   // Note: parameters are defined in the Split-MSSM

   double lambda_SM = lambda_split;

   if (loopLevel > 0) {
      lambda_SM += delta_lambda_1loop_chi_1(
         scale, mu, lambda_split, gYu, gYd, g2u, g2d, m1, m2);
   }
@endcode
 */

namespace flexiblesusy {
namespace splitmssm_thresholds {

/**
 * @class Parameters
 * @brief Parameters for MSSM threshold corrections to the SM or Split-MSSM
 *
 * Contains the running MS-bar parameters of the EFT (either the SM or
 * the Split-MSSM) and the runnign DR-bar parameters of the MSSM.  See
 * arXiv:1407.4081 for the parameter definition.
 */
struct Parameters {
   double g1{0.}, g2{0.}, g3{0.}; ///< MS-bar gauge couplings in the EFT (GUT normalized)
   double gt{0.};          ///< MS-bar top Yukawa coupling of the SM or Split-MSSM
   double At{0.};          ///< DR-bar trilinear coupling for the stops in the MSSM
   double mu{0.};          ///< bilinear Higgsino coupling
   double mA{0.};          ///< mass of the heavy Higgs doublett
   double m1{0.};          ///< bino mass parameter
   double m2{0.};          ///< wino mass parameter
   double tan_beta{0.};    ///< mixing angle of the heavy Higgs doublett in the MSSM
   double scale{0.};       ///< renormalization scale
   Eigen::Matrix<double,3,3> mq2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking left-handed squark mass parameters in the MSSM
   Eigen::Matrix<double,3,3> mu2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking right-handed up-squark mass parameters in the MSSM
   Eigen::Matrix<double,3,3> md2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking right-handed down-squark mass parameters in the MSSM
   Eigen::Matrix<double,3,3> ml2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking left-handed selectron mass parameters in the MSSM
   Eigen::Matrix<double,3,3> me2{Eigen::Matrix<double,3,3>::Zero()}; ///< DR-bar squared soft-breaking right-handed selectron mass parameters in the MSSM
};

std::ostream& operator<<(std::ostream&, const Parameters&);

double lambda_tree_level(const Parameters&);
double gYu_tree_level(const Parameters&);
double gYd_tree_level(const Parameters&);
double g2u_tree_level(const Parameters&);
double g2d_tree_level(const Parameters&);

double delta_lambda_1loop_reg(const Parameters&);
double delta_lambda_1loop_phi(const Parameters&);
double delta_lambda_1loop_chi_1(const Parameters&);
double delta_lambda_1loop_chi_1(
   double scale, double mu, double lambda, double gYu, double gYd,
   double g2u, double g2d, double m1, double m2);
double delta_lambda_1loop_chi_2(const Parameters&);
double delta_lambda_1loop_chi_2(
   double scale, double mu, double m2, double g1, double g2, double tan_beta);
double delta_lambda_2loop_phi(const Parameters&);
double delta_lambda_2loop_phi_HSS(const Parameters&);
double delta_gYu_1loop(const Parameters&);
double delta_gYd_1loop(const Parameters&);
double delta_g2u_1loop(const Parameters&);
double delta_g2d_1loop(const Parameters&);
double delta_gt_1loop_chi(
   double scale, double mu, double gYu, double gYd,
   double g2u, double g2d, double m1, double m2);
double delta_m2_1loop_chi(const Parameters&);

} // namespace splitmssm_thresholds
} // namespace flexiblesusy

#endif


//  Copyright John Maddock 2006.
//  Use, modification and distribution are 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)

// Test real concept.

// real_concept is an archetype for User defined Real types.

// This file defines the features, constructors, operators, functions...
// that are essential to use mathematical and statistical functions.
// The template typename "RealType" is used where this type
// (as well as the normal built-in types, float, double & long double)
// can be used.
// That this is the minimum set is confirmed by use as a type
// in tests of all functions & distributions, for example:
//   test_spots(0.F); & test_spots(0.);  for float and double, but also
//   test_spots(boost::math::concepts::real_concept(0.));
// NTL quad_float type is an example of a type meeting the requirements,
// but note minor additions are needed - see ntl.diff and documentation
// "Using With NTL - a High-Precision Floating-Point Library".

#ifndef BOOST_MATH_REAL_CONCEPT_HPP
#define BOOST_MATH_REAL_CONCEPT_HPP

#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/special_functions/modf.hpp>
#include <boost/math/tools/big_constant.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/policies/policy.hpp>
#include <boost/math/special_functions/asinh.hpp>
#include <boost/math/special_functions/atanh.hpp>
#if defined(__SGI_STL_PORT)
#  include <boost/math/tools/real_cast.hpp>
#endif
#include <ostream>
#include <istream>
#include <boost/config/no_tr1/cmath.hpp>
#include <math.h> // fmodl

#if defined(__SGI_STL_PORT) || defined(_RWSTD_VER) || defined(__LIBCOMO__)
#  include <cstdio>
#endif

namespace boost{ namespace math{

namespace concepts
{

#ifdef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   typedef double real_concept_base_type;
#else
   typedef long double real_concept_base_type;
#endif

class real_concept
{
public:
   // Constructors:
   real_concept() : m_value(0){}
   real_concept(char c) : m_value(c){}
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   real_concept(wchar_t c) : m_value(c){}
#endif
   real_concept(unsigned char c) : m_value(c){}
   real_concept(signed char c) : m_value(c){}
   real_concept(unsigned short c) : m_value(c){}
   real_concept(short c) : m_value(c){}
   real_concept(unsigned int c) : m_value(c){}
   real_concept(int c) : m_value(c){}
   real_concept(unsigned long c) : m_value(c){}
   real_concept(long c) : m_value(c){}
#if defined(__DECCXX) || defined(__SUNPRO_CC)
   real_concept(unsigned long long c) : m_value(static_cast<real_concept_base_type>(c)){}
   real_concept(long long c) : m_value(static_cast<real_concept_base_type>(c)){}
#elif defined(BOOST_HAS_LONG_LONG)
   real_concept(boost::ulong_long_type c) : m_value(static_cast<real_concept_base_type>(c)){}
   real_concept(boost::long_long_type c) : m_value(static_cast<real_concept_base_type>(c)){}
#elif defined(BOOST_HAS_MS_INT64)
   real_concept(unsigned __int64 c) : m_value(static_cast<real_concept_base_type>(c)){}
   real_concept(__int64 c) : m_value(static_cast<real_concept_base_type>(c)){}
#endif
   real_concept(float c) : m_value(c){}
   real_concept(double c) : m_value(c){}
   real_concept(long double c) : m_value(c){}
#ifdef BOOST_MATH_USE_FLOAT128
   real_concept(BOOST_MATH_FLOAT128_TYPE c) : m_value(c){}
#endif

   // Assignment:
   real_concept& operator=(char c) { m_value = c; return *this; }
   real_concept& operator=(unsigned char c) { m_value = c; return *this; }
   real_concept& operator=(signed char c) { m_value = c; return *this; }
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   real_concept& operator=(wchar_t c) { m_value = c; return *this; }
#endif
   real_concept& operator=(short c) { m_value = c; return *this; }
   real_concept& operator=(unsigned short c) { m_value = c; return *this; }
   real_concept& operator=(int c) { m_value = c; return *this; }
   real_concept& operator=(unsigned int c) { m_value = c; return *this; }
   real_concept& operator=(long c) { m_value = c; return *this; }
   real_concept& operator=(unsigned long c) { m_value = c; return *this; }
#ifdef BOOST_HAS_LONG_LONG
   real_concept& operator=(boost::long_long_type c) { m_value = static_cast<real_concept_base_type>(c); return *this; }
   real_concept& operator=(boost::ulong_long_type c) { m_value = static_cast<real_concept_base_type>(c); return *this; }
#endif
   real_concept& operator=(float c) { m_value = c; return *this; }
   real_concept& operator=(double c) { m_value = c; return *this; }
   real_concept& operator=(long double c) { m_value = c; return *this; }

   // Access:
   real_concept_base_type value()const{ return m_value; }

   // Member arithmetic:
   real_concept& operator+=(const real_concept& other)
   { m_value += other.value(); return *this; }
   real_concept& operator-=(const real_concept& other)
   { m_value -= other.value(); return *this; }
   real_concept& operator*=(const real_concept& other)
   { m_value *= other.value(); return *this; }
   real_concept& operator/=(const real_concept& other)
   { m_value /= other.value(); return *this; }
   real_concept operator-()const
   { return -m_value; }
   real_concept const& operator+()const
   { return *this; }
   real_concept& operator++()
   { ++m_value;  return *this; }
   real_concept& operator--()
   { --m_value;  return *this; }

private:
   real_concept_base_type m_value;
};

// Non-member arithmetic:
inline real_concept operator+(const real_concept& a, const real_concept& b)
{
   real_concept result(a);
   result += b;
   return result;
}
inline real_concept operator-(const real_concept& a, const real_concept& b)
{
   real_concept result(a);
   result -= b;
   return result;
}
inline real_concept operator*(const real_concept& a, const real_concept& b)
{
   real_concept result(a);
   result *= b;
   return result;
}
inline real_concept operator/(const real_concept& a, const real_concept& b)
{
   real_concept result(a);
   result /= b;
   return result;
}

// Comparison:
inline bool operator == (const real_concept& a, const real_concept& b)
{ return a.value() == b.value(); }
inline bool operator != (const real_concept& a, const real_concept& b)
{ return a.value() != b.value();}
inline bool operator < (const real_concept& a, const real_concept& b)
{ return a.value() < b.value(); }
inline bool operator <= (const real_concept& a, const real_concept& b)
{ return a.value() <= b.value(); }
inline bool operator > (const real_concept& a, const real_concept& b)
{ return a.value() > b.value(); }
inline bool operator >= (const real_concept& a, const real_concept& b)
{ return a.value() >= b.value(); }

// Non-member functions:
inline real_concept acos(real_concept a)
{ return std::acos(a.value()); }
inline real_concept cos(real_concept a)
{ return std::cos(a.value()); }
inline real_concept asin(real_concept a)
{ return std::asin(a.value()); }
inline real_concept atan(real_concept a)
{ return std::atan(a.value()); }
inline real_concept atan2(real_concept a, real_concept b)
{ return std::atan2(a.value(), b.value()); }
inline real_concept ceil(real_concept a)
{ return std::ceil(a.value()); }
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
// I've seen std::fmod(long double) crash on some platforms
// so use fmodl instead:
#ifdef _WIN32_WCE
//
// Ugly workaround for macro fmodl:
//
inline long double call_fmodl(long double a, long double b)
{  return fmodl(a, b); }
inline real_concept fmod(real_concept a, real_concept b)
{ return call_fmodl(a.value(), b.value()); }
#else
inline real_concept fmod(real_concept a, real_concept b)
{ return fmodl(a.value(), b.value()); }
#endif
#endif
inline real_concept cosh(real_concept a)
{ return std::cosh(a.value()); }
inline real_concept exp(real_concept a)
{ return std::exp(a.value()); }
inline real_concept fabs(real_concept a)
{ return std::fabs(a.value()); }
inline real_concept abs(real_concept a)
{ return std::abs(a.value()); }
inline real_concept floor(real_concept a)
{ return std::floor(a.value()); }
inline real_concept modf(real_concept a, real_concept* ipart)
{
#ifdef __MINGW32__
   real_concept_base_type ip;
   real_concept_base_type result = boost::math::modf(a.value(), &ip);
   *ipart = ip;
   return result;
#else
   real_concept_base_type ip;
   real_concept_base_type result = std::modf(a.value(), &ip);
   *ipart = ip;
   return result;
#endif
}
inline real_concept frexp(real_concept a, int* expon)
{ return std::frexp(a.value(), expon); }
inline real_concept ldexp(real_concept a, int expon)
{ return std::ldexp(a.value(), expon); }
inline real_concept log(real_concept a)
{ return std::log(a.value()); }
inline real_concept log10(real_concept a)
{ return std::log10(a.value()); }
inline real_concept tan(real_concept a)
{ return std::tan(a.value()); }
inline real_concept pow(real_concept a, real_concept b)
{ return std::pow(a.value(), b.value()); }
#if !defined(__SUNPRO_CC)
inline real_concept pow(real_concept a, int b)
{ return std::pow(a.value(), b); }
#else
inline real_concept pow(real_concept a, int b)
{ return std::pow(a.value(), static_cast<real_concept_base_type>(b)); }
#endif
inline real_concept sin(real_concept a)
{ return std::sin(a.value()); }
inline real_concept sinh(real_concept a)
{ return std::sinh(a.value()); }
inline real_concept sqrt(real_concept a)
{ return std::sqrt(a.value()); }
inline real_concept tanh(real_concept a)
{ return std::tanh(a.value()); }

//
// C++11 ism's
// Note that these must not actually call the std:: versions as that precludes using this
// header to test in C++03 mode, call the Boost versions instead:
//
inline boost::math::concepts::real_concept asinh(boost::math::concepts::real_concept a)
{
   return boost::math::asinh(a.value(), boost::math::policies::make_policy(boost::math::policies::overflow_error<boost::math::policies::ignore_error>()));
}
inline boost::math::concepts::real_concept acosh(boost::math::concepts::real_concept a)
{
   return boost::math::acosh(a.value(), boost::math::policies::make_policy(boost::math::policies::overflow_error<boost::math::policies::ignore_error>()));
}
inline boost::math::concepts::real_concept atanh(boost::math::concepts::real_concept a)
{
   return boost::math::atanh(a.value(), boost::math::policies::make_policy(boost::math::policies::overflow_error<boost::math::policies::ignore_error>()));
}

//
// Conversion and truncation routines:
//
template <class Policy>
inline int iround(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::iround(v.value(), pol); }
inline int iround(const concepts::real_concept& v)
{ return boost::math::iround(v.value(), policies::policy<>()); }
template <class Policy>
inline long lround(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::lround(v.value(), pol); }
inline long lround(const concepts::real_concept& v)
{ return boost::math::lround(v.value(), policies::policy<>()); }

#ifdef BOOST_HAS_LONG_LONG
template <class Policy>
inline boost::long_long_type llround(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::llround(v.value(), pol); }
inline boost::long_long_type llround(const concepts::real_concept& v)
{ return boost::math::llround(v.value(), policies::policy<>()); }
#endif

template <class Policy>
inline int itrunc(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::itrunc(v.value(), pol); }
inline int itrunc(const concepts::real_concept& v)
{ return boost::math::itrunc(v.value(), policies::policy<>()); }
template <class Policy>
inline long ltrunc(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::ltrunc(v.value(), pol); }
inline long ltrunc(const concepts::real_concept& v)
{ return boost::math::ltrunc(v.value(), policies::policy<>()); }

#ifdef BOOST_HAS_LONG_LONG
template <class Policy>
inline boost::long_long_type lltrunc(const concepts::real_concept& v, const Policy& pol)
{ return boost::math::lltrunc(v.value(), pol); }
inline boost::long_long_type lltrunc(const concepts::real_concept& v)
{ return boost::math::lltrunc(v.value(), policies::policy<>()); }
#endif

// Streaming:
template <class charT, class traits>
inline std::basic_ostream<charT, traits>& operator<<(std::basic_ostream<charT, traits>& os, const real_concept& a)
{
   return os << a.value();
}
template <class charT, class traits>
inline std::basic_istream<charT, traits>& operator>>(std::basic_istream<charT, traits>& is, real_concept& a)
{
   real_concept_base_type v;
   is >> v;
   a = v;
   return is;
}

} // namespace concepts

namespace tools
{

template <>
inline concepts::real_concept make_big_value<concepts::real_concept>(boost::math::tools::largest_float val, const char* , std::false_type const&, std::false_type const&)
{
   return val;  // Can't use lexical_cast here, sometimes it fails....
}

template <>
inline concepts::real_concept max_value<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
   return max_value<concepts::real_concept_base_type>();
}

template <>
inline concepts::real_concept min_value<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
   return min_value<concepts::real_concept_base_type>();
}

template <>
inline concepts::real_concept log_max_value<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
   return log_max_value<concepts::real_concept_base_type>();
}

template <>
inline concepts::real_concept log_min_value<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
   return log_min_value<concepts::real_concept_base_type>();
}

template <>
inline concepts::real_concept epsilon<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept))
{
#ifdef __SUNPRO_CC
   return std::numeric_limits<concepts::real_concept_base_type>::epsilon();
#else
   return tools::epsilon<concepts::real_concept_base_type>();
#endif
}

template <>
inline BOOST_MATH_CONSTEXPR int digits<concepts::real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::real_concept)) BOOST_NOEXCEPT
{
   // Assume number of significand bits is same as real_concept_base_type,
   // unless std::numeric_limits<T>::is_specialized to provide digits.
   return tools::digits<concepts::real_concept_base_type>();
   // Note that if numeric_limits real concept is NOT specialized to provide digits10
   // (or max_digits10) then the default precision of 6 decimal digits will be used
   // by Boost test (giving misleading error messages like
   // "difference between {9.79796} and {9.79796} exceeds 5.42101e-19%"
   // and by Boost lexical cast and serialization causing loss of accuracy.
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_REAL_CONCEPT_HPP


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

#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/vectors/VectorBase.hpp"
#include "kindr/phys_quant/PhysicalType.hpp"

namespace kindr {

/*! \class Vector
 * \brief Vector in n-dimensional-space.
 *
 * This class implements a vector in n-dimensional-space.
 * More precisely an interface to store and access the coordinates of a vector of a point in n-dimensional-space is provided.
 * \tparam PhysicalType_    Physical type of the vector.
 * \tparam PrimType_        Primitive type of the coordinates.
 * \tparam Dimension_       Dimension of the vector.
 * \ingroup vectors
 */
template<enum PhysicalType PhysicalType_, typename PrimType_, int Dimension_>
class Vector : public VectorBase<Vector<PhysicalType_, PrimType_, Dimension_> >, private Eigen::Matrix<PrimType_, Dimension_, 1> {
 private:
  /*! \brief The base type.
   */
  typedef VectorBase<Vector<PhysicalType_, PrimType_, Dimension_> > Base;

  /*! \brief The size if the vector has dynamic dimension (must be equal to Eigen::Dynamic).
   */
  static constexpr int DynamicDimension = -1;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /*! \brief The implementation type.
   *
   *  The implementation type is always an Eigen object.
   */
  typedef Eigen::Matrix<PrimType_, Dimension_, 1> Implementation;

  /*! \brief The primitive type of the coordinates.
   */
  typedef PrimType_ Scalar;

  /*! \brief The dimension of the vector.
   */
  static constexpr int Dimension = Dimension_;

  /*! \brief Default constructor for static sized vectors which initializes all components with zero.
   */
  template<int DimensionCopy_ = Dimension_>
  Vector(typename std::enable_if<DimensionCopy_ != DynamicDimension>::type* = nullptr)
    : Implementation(Implementation::Zero()) {
  }

  /*! \brief Default constructor for dynamic sized vectors.
   */
  template<int DimensionCopy_ = Dimension_>
  Vector(typename std::enable_if<DimensionCopy_ == DynamicDimension>::type* = nullptr)
    : Implementation() {
  }

  /*! \brief Constructor using other vector with generic type.
   *  \param other   Vector<OtherPhysicalType_, OtherPrimType_, Dimension_>
   */
  template<enum PhysicalType OtherPhysicalType_, typename OtherPrimType_>
  explicit Vector(const Vector<OtherPhysicalType_, OtherPrimType_, Dimension_>& other)
    : Implementation(other.toImplementation().template cast<PrimType_>()) {
  }

  /*! \brief Constructor of a dynamic vector using a static vector.
   *  \param other   Vector<OtherPhysicalType_, OtherPrimType_, Dimension_>
   */
  template<int DimensionOther_, int DimensionCopy_ = Dimension_>
  Vector(const Vector<PhysicalType_, PrimType_, DimensionOther_>& other, typename std::enable_if<DimensionCopy_ == DynamicDimension>::type* = nullptr)
    : Implementation(other.toImplementation()) {
  }

  /*! \brief Constructor using Eigen::Matrix.
   *  \param other   Eigen::Matrix<PrimType_,Dimension_,1>
   */
  explicit Vector(const Implementation& other)
    : Implementation(other) {
  }

  /*! \brief Constructor using three scalars.
   *  \param x x-Component
   *  \param y y-Component
   *  \param z z-Component
   */
  template<int DimensionCopy_ = Dimension_>
  Vector(Scalar x, Scalar y, Scalar z, typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr)
    : Implementation(x,y,z) {
  }

  /*! \brief Get zero element.
   * \returns zero element
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> Zero() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::Zero());
  }

  /*! \brief Sets all components of the vector to zero.
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& setZero() {
    Implementation::setZero();
    return *this;
  }

  /*! \brief Get random element.
   * \returns random element
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> Random() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::Random());
  }

  /*! \brief Sets all components of the vector to random.
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& setRandom() {
    Implementation::setRandom();
    return *this;
  }

  /*! \brief Get the unity vector in x.
   * \returns the unity vector in x
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> UnitX() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::UnitX());
  }

  /*! \brief Get the unity vector in y.
   * \returns the unity vector in y
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> UnitY() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::UnitY());
  }

  /*! \brief Get the unity vector in z.
   * \returns the unity vector in z
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> UnitZ() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::UnitZ());
  }

  /*! \brief Set values.
   */
  using Implementation::operator<<;

  /*! \brief Get/set values.
   */
  using Implementation::operator();

  /*!\brief Get the head of the vector (copy)
   * \returns the head of the vector (copy)
   */
  template<int DimensionOutput_>
  Vector<PhysicalType_, PrimType_, DimensionOutput_> getHead() const {
    return Vector<PhysicalType_, PrimType_, DimensionOutput_>(this->toImplementation().template head<DimensionOutput_>());
  }

  /*!\brief Get the head of the vector (copy)
   * \returns the head of the vector (copy)
   */
  Vector<PhysicalType_, PrimType_, DynamicDimension> getHead(int length) const {
    return Vector<PhysicalType_, PrimType_, DynamicDimension>(this->toImplementation().head(length));
  }

  /*!\brief Get the tail of the vector (copy)
   * \returns the tail of the vector (copy)
   */
  template<int DimensionOutput_>
  Vector<PhysicalType_, PrimType_, DimensionOutput_> getTail() const {
    return Vector<PhysicalType_, PrimType_, DimensionOutput_>(this->toImplementation().template tail<DimensionOutput_>());
  }

  /*!\brief Get the tail of the vector (copy)
   * \returns the tail of the vector (copy)
   */
  Vector<PhysicalType_, PrimType_, DynamicDimension> getTail(int length) const {
    return Vector<PhysicalType_, PrimType_, DynamicDimension>(this->toImplementation().tail(length));
  }

  /*!\brief Get a segment of the vector (copy)
   * \returns a segment of the vector (copy)
   */
  template<int DimensionOutput_>
  Vector<PhysicalType_, PrimType_, DimensionOutput_> getSegment(int start) const {
    return Vector<PhysicalType_, PrimType_, DimensionOutput_>(this->toImplementation().template segment<DimensionOutput_>(start));
  }

  /*!\brief Get a segment of the vector (copy)
   * \returns a segment of the vector (copy)
   */
  Vector<PhysicalType_, PrimType_, DynamicDimension> getSegment(int start, int length) const {
    return Vector<PhysicalType_, PrimType_, DynamicDimension>(this->toImplementation().segment(start, length));
  }

  /*!\brief Set the head of the vector
   */
  template<int DimensionInput_>
  void setHead(const Vector<PhysicalType_, PrimType_, DimensionInput_> & input) {
    this->toImplementation().template head<DimensionInput_>() = input.toImplementation();
  }

  /*!\brief Set the tail of the vector
   */
  template<int DimensionInput_>
  void setTail(const Vector<PhysicalType_, PrimType_, DimensionInput_> & input) {
    this->toImplementation().template tail<DimensionInput_>() = input.toImplementation();
  }

  /*!\brief Set a segment of the vector
   */
  template<int DimensionInput_>
  void setSegment(int start, const Vector<PhysicalType_, PrimType_, DimensionInput_> & input) {
    this->toImplementation().template segment<DimensionInput_>(start) = input.toImplementation();
  }

  /*!\brief Get x-coordinate of the vector (copy)
   * \returns the x-coordinate of the vector (copy)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar x(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) const {
    return this->toImplementation().x();
  }

  /*!\brief Get x-coordinate of the vector (reference)
   * \returns the x-coordinate of the vector (reference)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar& x(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) {
    return this->toImplementation().x();
  }

  /*!\brief Get y-coordinate of the vector (copy)
   * \returns the y-coordinate of the vector (copy)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar y(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) const {
    return this->toImplementation().y();
  }

  /*!\brief Get y-coordinate of the vector (reference)
   * \returns the y-coordinate of the vector (reference)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar& y(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) {
    return this->toImplementation().y();
  }

  /*!\brief Get z-coordinate of the vector (copy)
   * \returns the z-coordinate of the vector (copy)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar z(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) const {
    return this->toImplementation().z();
  }

  /*!\brief Get z-coordinate of the vector (reference)
   * \returns the z-coordinate of the vector (reference)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar& z(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) {
    return this->toImplementation().z();
  }

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

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

  /*! \brief Cast to Eigen::Matrix<PrimType_, Dimension_, 1>.
   *  \returns Eigen::Matrix<PrimType_, Dimension_, 1>
   */
  inline Implementation& vector() {
    return static_cast<Implementation&>(*this);
  }

  /*! \brief Cast to Eigen::Matrix<PrimType_, Dimension_, 1>.
   *  \returns Eigen::Matrix<PrimType_, Dimension_, 1>
   */
  inline const Implementation& vector() const {
    return static_cast<const Implementation&>(*this);
  }

  /*! \brief Assignment operator.
   * \param other   other vector
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_> & operator=(const Vector<PhysicalType_, PrimType_, Dimension_>& other) { // (The assignment of a static to a dynamic vector does not work because the amount of parameters must be one and SFINAE leads to two parameters. Workaround: cast the static vector into a dynamic one, then assign.)
    this->toImplementation() = other.toImplementation();
    return *this;
  }

  /*! \brief Addition of two vectors.
   * \param other   other vector
   * \returns sum
   */
  Vector<PhysicalType_, PrimType_, Dimension_> operator+(const Vector<PhysicalType_, PrimType_, Dimension_>& other) const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation() + other.toImplementation());
  }

  /*! \brief Subtraction of two vectors.
   * \param other   other vector
   * \returns difference
   */
  Vector<PhysicalType_, PrimType_, Dimension_> operator-(const Vector<PhysicalType_, PrimType_, Dimension_>& other) const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation() - other.toImplementation());
  }

  /*! \brief Multiplies vector with a scalar.
   * \param factor   factor
   * \returns product
   */
  template<typename PrimTypeFactor_>
  Vector<PhysicalType_, PrimType_, Dimension_> operator*(PrimTypeFactor_ factor) const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation()*(PrimType_)factor);
  }

  /*! \brief Divides vector by a scalar.
   * \param divisor   divisor
   * \returns quotient
   */
  template<typename PrimTypeDivisor_>
  Vector<PhysicalType_, PrimType_, Dimension_> operator/(PrimTypeDivisor_ divisor) const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation()/(PrimType_)divisor);
  }

  /*! \brief Addition and assignment of two vectors.
   * \param other   other vector
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& operator+=(const Vector<PhysicalType_, PrimType_, Dimension_>& other) {
    this->toImplementation() += other.toImplementation();
    return *this;
  }

  /*! \brief Subtraction and assignment of two vectors.
   * \param other   other vector
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& operator-=(const Vector<PhysicalType_, PrimType_, Dimension_>& other) {
    this->toImplementation() -= other.toImplementation();
    return *this;
  }

  /*! \brief Multiplication with a scalar and assignment.
   * \param factor   factor
   * \returns reference
   */
  template<typename PrimTypeFactor_>
  Vector<PhysicalType_, PrimType_, Dimension_>& operator*=(PrimTypeFactor_ factor) {
    this->toImplementation() *= (PrimType_)factor;
    return *this;
  }

  /*! \brief Division by a scalar and assignment.
   * \param divisor   divisor
   * \returns reference
   */
  template<typename PrimTypeDivisor_>
  Vector<PhysicalType_, PrimType_, Dimension_>& operator/=(PrimTypeDivisor_ divisor) {
    this->toImplementation() /= (PrimType_)divisor;
    return *this;
  }

  /*! \brief Negation of a vector.
   * \returns negative vector
   */
  Vector<PhysicalType_, PrimType_, Dimension_> operator-() const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(-this->toImplementation());
  }

  /*! \brief Comparison operator.
   * \param other   other vector
   * \returns true if equal
   */
  bool operator==(const Vector<PhysicalType_, PrimType_, Dimension_>& other) const {
    return this->toImplementation() == other.toImplementation();
  }

  /*! \brief Comparison operator.
   * \param other   other vector
   * \returns true if unequal
   */
  bool operator!=(const Vector<PhysicalType_, PrimType_, Dimension_>& other) const {
    return this->toImplementation() != other.toImplementation();
  }

  /*! \brief Comparison function.
   * \param other   other vector
   * \param tol   tolerance
   * \returns true if similar within tolerance
   */
  bool isSimilarTo(const Vector<PhysicalType_, PrimType_, Dimension_>& other, Scalar tol) const {
    if((*this - other).abs().max() < tol) {
      return true;
    } else {
      return false;
    }
  }

  /*! \brief Norm of the vector.
   *  \returns norm.
   */
  Scalar norm() const {
    return this->toImplementation().norm();
  }

  /*! \brief Squared norm of the vector.
   *  \returns norm.
   */
  Scalar squaredNorm() const {
    return this->toImplementation().squaredNorm();
  }

  /*! \brief Normalizes the vector.
   *  \returns reference.
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& normalize() {
    this->toImplementation().normalize();
    return *this;
  }

  /*! \brief Get a normalized version of the vector.
   *  \returns normalized vector.
   */
  Vector<PhysicalType_, PrimType_, Dimension_> normalized() const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation().normalized());
  }

  /*! \brief Dot product with other vector.
   *  \param other   other vector
   *  \returns dot product.
   */
  template<enum PhysicalType PhysicalTypeOther_>
  Scalar dot(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other) const {
    return this->toImplementation().dot(other.toImplementation());
  }

  /*! \brief Cross product with other vector.
   *  \param other   other vector
   *  \returns cross product.
   */
  template<enum PhysicalType PhysicalTypeOther_, int DimensionCopy_ = Dimension_>
  typename internal::MultiplicationReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType
  cross(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other, typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) const {
    return typename internal::MultiplicationReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType(this->toImplementation().cross(other.toImplementation()));
  }

  /*! \brief Projects this vector (a) on the other vector (b).
   *  The result  is
   *    proj = b/|b| * |a| * cos(angle) = b/|b| * |a| * a.b / |a| / |b|,
   *  which is computed by
   *           a.b
   *  proj = ------- * a
   *          |a|*|a|
   *  \param other   other vector
   *  \returns projected vector.
   */
  template<enum PhysicalType PhysicalTypeOther_>
  Vector<PhysicalType_, PrimType_, Dimension_> projectOn(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other) const {
    return other * (this->dot(other)/other.squaredNorm());
  }

  /*! \brief Elementwise product with other vector.
   *  \param other   other vector
   *  \returns elementwise product.
   */
  template<enum PhysicalType PhysicalTypeOther_>
  typename internal::MultiplicationReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType
  elementwiseMultiplication(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other) const {
    return typename internal::MultiplicationReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType(this->toImplementation().cwiseProduct(other.toImplementation()));
  }

  /*! \brief Elementwise division by other vector.
   *  \param other   other vector
   *  \returns elementwise quotient.
   */
  template<enum PhysicalType PhysicalTypeOther_>
  typename internal::DivisionReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType
  elementwiseDivision(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other) const {
    return typename internal::DivisionReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType(this->toImplementation().cwiseQuotient(other.toImplementation()));
  }

  /*! \brief Absolute components.
   *  \returns absolute components.
   */
  Vector<PhysicalType_, PrimType_, Dimension_> abs() const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation().cwiseAbs());
  }

  /*! \brief Maximum of the components.
   *  \returns maximum.
   */
  Scalar max() const {
    return this->toImplementation().maxCoeff();
  }

  /*! \brief Minimum of the components.
   *  \returns minimum.
   */
  Scalar min() const {
    return this->toImplementation().minCoeff();
  }

  /*! \brief Sum of the components.
   *  \returns sum.
   */
  Scalar sum() const {
    return this->toImplementation().sum();
  }

  /*! \brief Mean of the components.
   *  \returns mean.
   */
  Scalar mean() const {
    return this->toImplementation().mean();
  }

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const Vector<PhysicalType_, PrimType_, Dimension_>& vector) {
    out << vector.transpose();
    return out;
  }
};

/*! \brief Multiplies a vector with a scalar.
 * \param factor   factor
 * \returns product
 */
template<enum PhysicalType PhysicalType_, typename PrimTypeFactor_, typename PrimType_, int Dimension_>
Vector<PhysicalType_, PrimType_, Dimension_> operator*(PrimTypeFactor_ factor, const Vector<PhysicalType_, PrimType_, Dimension_>& vector) {
  return vector*(PrimType_)factor;
}

namespace internal {

/*! \brief Gets the primitive type of the vector
 */
template<enum PhysicalType PhysicalType_, typename PrimType_, int Dimension_>
class get_scalar<Vector<PhysicalType_, PrimType_, Dimension_>> {
 public:
  typedef PrimType_ Scalar;
};

/*! \brief Gets the dimension of the vector
 */
template<enum PhysicalType PhysicalType_, typename PrimType_, int Dimension_>
class get_dimension<Vector<PhysicalType_, PrimType_, Dimension_>> {
 public:
  static constexpr int Dimension = Dimension_;
};

/*! \brief Gets the return type of a multiplication
 */
template<enum PhysicalType PhysicalType1_, enum PhysicalType PhysicalType2_, typename PrimType_, int Dimension_>
class MultiplicationReturnTypeTrait<Vector<PhysicalType1_, PrimType_, Dimension_>, Vector<PhysicalType2_, PrimType_, Dimension_>>
{
 public:
  typedef Vector<PhysicalType::Typeless, PrimType_, Dimension_> ReturnType;
};

/*! \brief Gets the return type of a multiplication
 */
template<enum PhysicalType PhysicalType1_, enum PhysicalType PhysicalType2_, typename PrimType_, int Dimension_>
class DivisionReturnTypeTrait<Vector<PhysicalType1_, PrimType_, Dimension_>, Vector<PhysicalType2_, PrimType_, Dimension_>>
{
 public:
  typedef Vector<PhysicalType::Typeless, PrimType_, Dimension_> ReturnType;
};

/*! \brief Specializes multiplication and division traits for the triple (factor1 != factor2)
 */
#define KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(FACTOR1, FACTOR2, PRODUCT) \
    template<typename PrimType_, int Dimension_> \
    class MultiplicationReturnTypeTrait<Vector<PhysicalType::FACTOR1, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR2, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::PRODUCT, PrimType_, Dimension_> ReturnType; \
    }; \
    template<typename PrimType_, int Dimension_> \
    class MultiplicationReturnTypeTrait<Vector<PhysicalType::FACTOR2, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR1, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::PRODUCT, PrimType_, Dimension_> ReturnType; \
    }; \
    template<typename PrimType_, int Dimension_> \
    class DivisionReturnTypeTrait<Vector<PhysicalType::PRODUCT, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR1, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::FACTOR2, PrimType_, Dimension_> ReturnType; \
    }; \
    template<typename PrimType_, int Dimension_> \
    class DivisionReturnTypeTrait<Vector<PhysicalType::PRODUCT, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR2, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::FACTOR1, PrimType_, Dimension_> ReturnType; \
    };

/*! \brief Specializes multiplication and division traits for the triple (factor1 == factor2)
 */
#define KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_B(FACTOR1AND2, PRODUCT) \
    template<typename PrimType_, int Dimension_> \
    class MultiplicationReturnTypeTrait<Vector<PhysicalType::FACTOR1AND2, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR1AND2, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::PRODUCT, PrimType_, Dimension_> ReturnType; \
    }; \
    template<typename PrimType_, int Dimension_> \
    class DivisionReturnTypeTrait<Vector<PhysicalType::PRODUCT, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR1AND2, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::FACTOR1AND2, PrimType_, Dimension_> ReturnType; \
    };

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_B(Typeless, Typeless)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Time, Time)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Jerk, Jerk)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Acceleration, Acceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Velocity, Velocity)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Position, Position) // Position/Position = Typeless
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Force, Force)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Momentum, Momentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, AngularJerk, AngularJerk)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, AngularAcceleration, AngularAcceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, AngularVelocity, AngularVelocity)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Angle, Angle)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Torque, Torque)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, AngularMomentum, AngularMomentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, AngularJerk, Jerk)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, AngularAcceleration, Acceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, AngularVelocity, Velocity)
//KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, Angle, Position) // Position/Position = Angle -> ambiguous, explicit cast to Angle if needed
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, Force, Torque)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, Momentum, AngularMomentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Jerk, Acceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Acceleration, Velocity)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Velocity, Position)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Force, Momentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, AngularJerk, AngularAcceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, AngularAcceleration, AngularVelocity)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, AngularVelocity, Angle)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Torque, AngularMomentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Mass, Acceleration, Force)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Mass, Velocity, Momentum)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Inertia, AngularAcceleration, Torque)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Inertia, AngularVelocity, AngularMomentum)

} // namespace internal
} // namespace kindr