// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// Mvec.hpp
// This file is part of the Garamon for project_namespace.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file Mvec.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Class to define a multivector and its basic operators in the Geometric algebra of project_namespace.

// Anti-doublon
#ifndef project_inclusion_guard
#define project_inclusion_guard
#pragma once

// External Includes
#include <Eigen/Core>
#include <list>
#include <iostream>
#include <cmath>
#include <limits>

// Internal Includes
#include "project_namespace/Utility.hpp"
#include "project_namespace/Constants.hpp"

#include "project_namespace/Outer.hpp"
#include "project_namespace/Inner.hpp"
#include "project_namespace/Geometric.hpp"

#include "project_namespace/OuterExplicit.hpp"
#include "project_namespace/InnerExplicit.hpp"
#include "project_namespace/GeometricExplicit.hpp"

/*!
 * @namespace project_namespace
 */
namespace project_namespace{

    /// \class Kvec
    /// \brief class defining a single grade component of a multivector.
    template<class T>
    struct Kvec{

        Eigen::Matrix<T, Eigen::Dynamic, 1> vec;  /*!< dynamic vector of Eigen Library */

        unsigned int grade; /*!< grade k of the k-vector */

        /// \brief operator == to test the equality between 2 k-vectors.
        bool operator == (const Kvec<T>& other) const {
            return vec == other.vec;
        }
    };

    /// \class Mvec
    /// \brief class defining multivectors.
    template<typename T = double>
    class Mvec {

    protected:
        std::list<Kvec<T>> mvData;  /*!< set of k-vectors, mapped by grade */
        unsigned int gradeBitmap;   /*!< ith bit to 1 if grade i is contained in the multivector */

    public:

        /// \brief Default constructor, generate an empty multivector equivalent to the scalar 0.
        Mvec();

        /// \brief Copy constructor
        /// \param mv - the multivector to be copied
        Mvec(const Mvec& mv);

        /// \brief Copy constructor of Mvec
        /// \param mv - the multivector which has to be copied
        Mvec(Mvec&& mv); // move constructor

        template <typename U>
        friend class Mvec;

        /// \brief Copy constructor of Mvec from different types
        /// \param mv - the multivector with another template type
        template<typename U>
        Mvec<T>(const Mvec<U> &mv);

        /// \brief Constructor of Mvec from a scalar
        /// \param val - scalar value
        template<typename S>
        Mvec(const S val);

        /// Destructor
        ~Mvec();

        /// \brief Overload the assignment operator. No need any copy when the argument is an R-value, just need to move.
        /// \param mv - Mvec used as a Rvalue
        /// \return assign other to this
        Mvec& operator=(Mvec&& mv);

        /// \brief Overload the assignment operator
        /// \param mv - Mvec
        /// \return assign mv to this object
        Mvec& operator=(const Mvec& mv);

        /// \brief defines the addition between two Mvec
        /// \param mv2 - second operand of type Mvec
        /// \return this + mv2
        Mvec operator+(const Mvec &mv2) const;

        /// \brief defines the addition between a Mvec and a scalar
        /// \param value - second operand (scalar)
        /// \return this + scalar
        template<typename S>
        Mvec operator+(const S &value) const;

        /// \brief defines the addition between a scalar and a Mvec
        /// \param value a scalar
        /// \param mv - second operand of type Mvec
        /// \return scalar + mv
        template<typename U, typename S>
        friend Mvec<U> operator+(const S &value, const Mvec<U> &mv);

        /// \brief Overload the += operator, corresponds to this += mv
        /// \param mv - Mvec to be added to this object
        /// \return this += mv
        Mvec& operator+=(const Mvec& mv);

        /// \brief defines the opposite of a multivector
        /// \param mv: operand of type Mvec
        /// \return -mv
        template<typename U>
        friend Mvec<U> operator-(const Mvec<U> &mv); // unary operator -mv1

        /// \brief defines the difference between two Mvec
        /// \param mv2 - second operand of type Mvec
        /// \return this - mv2
        Mvec<T> operator-(const Mvec<T> &mv2) const;

        /// \brief defines the difference between a Mvec and a scalar
        /// \param value - second operand (scalar)
        /// \return this - scalar
        template<typename S>
        Mvec operator-(const S &value) const;

        /// \brief defines the difference between a scalar and a Mvec
        /// \param value a scalar
        /// \param mv - second operand of type Mvec
        /// \return scalar - mvX
        template<typename U, typename S>
        friend Mvec<U> operator-(const S &value, const Mvec<U> &mv);

        /// \brief Overload the -= operator, corresponds to this -= mv
        /// \param mv - Mvec to be added to this object
        /// \return this -= mv
        Mvec& operator-=(const Mvec& mv);

        /// \brief defines the outer product between two multivectors
        /// \param mv2 - a multivector
        /// \return this^mv2
        Mvec operator^(const Mvec &mv2) const;

        /// \brief defines the outer product a multivector and a scalar
        /// \param value - a scalar
        /// \return this^value
        template<typename S>
        Mvec operator^(const S &value) const;

        /// \brief defines the outer product between a scalar and a multivector
        /// \param value - a scalar
        /// \param mv - a multivector
        /// \return value ^ mv
        template<typename U, typename S>
        friend Mvec<U> operator^(const S &value, const Mvec<U> &mv);

        /// \brief Overload the outer product with operator =, corresponds to this ^= mv
        /// \param mv - Mvec to be wedged to this object
        /// \return this ^= mv
        Mvec& operator^=(const Mvec& mv);

        /// \brief defines the inner product between two multivectors
        /// \param mv2 - a multivector
        /// \return this.mv2
        Mvec operator|(const Mvec &mv2) const;

        /// \brief defines the inner product between a multivector and a scalar
        /// \param value - a scalar
        /// \return this.value = 0 (empty multivector)
        template<typename S>
        Mvec operator|(const S &value) const;

        /// \brief defines the inner product between a scalar and a multivector
        /// \param value - a scalar
        /// \param mv - a multivector
        /// \return mv.value = 0 (empty multivector)
        template<typename U, typename S>
        friend Mvec<U> operator|(const S &value, const Mvec<U> &mv);

        /// \brief Overload the inner product with operator =, corresponds to this |= mv
        /// \param mv - Mvec to be computed in the inner product with this object
        /// \return this |= mv
        Mvec& operator|=(const Mvec& mv);

        /// \brief defines the right contraction between two multivectors
        /// \param mv2 - a multivector
        /// \return the right contraction : $this \\lfloor mv2$
        Mvec operator>(const Mvec &mv2) const;

        /// \brief defines the right contraction between a multivector and a scalar
        /// \param value - a scalar
        /// \return $this \\lfloor value$
        template<typename S>
        Mvec operator>(const S &value) const;

        /// \brief defines the right contraction between a scalar and a multivector
        /// \param value - a scalar
        /// \param mv - a multivector
        /// \return $value \\lfloor this = 0$ (empty multivector)
        template<typename U, typename S>
        friend Mvec<U> operator>(const S &value, const Mvec<U> &mv);

        /// \brief defines the left contraction between two multivectors
        /// \param mv2 - a multivector
        /// \return the left contraction $this \\rfloor mv2$
        Mvec operator<(const Mvec &mv2) const;

        /// \brief defines the left contraction between a multivector and a scalar
        /// \param value - a scalar
        /// \return $value \\rfloor this$ = 0 (empty multivector)
        template<typename S>
        Mvec operator<(const S &value) const;

        /// \brief defines the left contraction between a scalar and a multivector
        /// \param value - a scalar
        /// \param mv - a multivector
        /// \return $this \\rfloor value$
        template<typename U, typename S>
        friend Mvec<U> operator<(const S &value, const Mvec<U> &mv);

        /// \brief defines the geometric product between two multivectors
        /// \param mv2 - a multivector
        /// \return mv1*mv2
        Mvec operator*(const Mvec &mv2) const;

    project_singular_metric_comment_begin
        /// \brief defines the outer product between two multivectors, where the second multivector is dualized during the product computation.
        /// \param mv2 - a multivector that will be dualized during the wedge with the calling multivector.
        /// \return a multivector.
        Mvec<T> outerPrimalDual(const Mvec<T> &mv2) const;

        /// \brief defines the outer product between two multivectors, where the first multivector is dualized during the product computation.
        /// \param mv2 - a primal form of a multivector; the object multivector will be dualized during the wedge with the calling multivector.
        /// \return a multivector.
        Mvec<T> outerDualPrimal(const Mvec<T> &mv2) const;

        /// \brief defines the outer product between two multivectors, where both multivectors are dualized during the product computation.
        /// \param mv2 - a primal form of a multivector; the object multivector will be dualized during the wedge with the calling multivector.
        /// \return a multivector.
        Mvec<T> outerDualDual(const Mvec<T> &mv2) const;
    project_singular_metric_comment_end

        /// \brief defines the scalar product between two multivectors (sum of the inner products between same grade pairs from the 2 multivectors)
        /// \param mv2 - a multivector
        /// \return a scalar
        Mvec<T> scalarProduct(const Mvec<T> &mv2) const;

        /// \brief defines the Hestenes product between two multivectors (Inner product - scalar product)
        /// \param mv2 - a multivector
        /// \return a multivector.
        Mvec<T> hestenesProduct(const Mvec<T> &mv2) const;

        /// \brief defines the geometric product between a multivector and a scalar
        /// \param value - a scalar
        /// \return mv2*value
        template<typename S>
        Mvec operator*(const S &value) const;

        /// \brief defines the geometric product between a scalar and a multivector
        /// \param value - a scalar
        /// \param mv - a multivector
        /// \return value*mv2
        template<typename U, typename S>
        friend Mvec<U> operator*(const S &value, const Mvec<U> &mv);

        /// \brief Overload the geometric product with operator =, corresponds to this *= mv
        /// \param mv - Mvec to be multiplied to this object
        /// \return this *= mv
        Mvec& operator*=(const Mvec& mv);

        /// \brief defines the geometric product with a multivector and the inverse of a second multivector
        /// \param mv2 - a multivector
        /// \return this / mv2
        Mvec operator/(const Mvec &mv2) const;

        /// \brief defines the scalar inverse between a multivector and a scalar
        /// \param value - a scalar
        /// \return this / value
        template<typename S>
        Mvec operator/(const S &value) const;

        /// \brief defines the inverse product between a scalar and a multivector
        /// \param value - a scalar
        /// \param mv - a multivector
        /// \return value / mv
        template<typename U, typename S>
        friend Mvec<U> operator/(const S &value, const Mvec<U> &mv);

        /// \brief Overload the inverse with operator =, corresponds to this /= mv
        /// \param mv - Mvec to be inversed to this object
        /// \return this /= mv
        Mvec& operator/=(const Mvec& mv);

        /// \brief the reverse of a multivector, i.e. if mv = a1^a2^...^an, then reverse(mv) = an^...^a2^a1
        /// \param mv - a multivector
        /// \return reverse of mv
        template<typename U>
        friend Mvec<U> operator~(const Mvec<U> &mv);

        /// \brief the dual of a k-vector is defined as $A_k^* = A_k \\lcont I_n^{-1}$, for a multivector, we just dualize all its components. If the metric is degenerated, this function computes the right complement (mv ^ !mv = I).
        /// \param mv - a multivector
        /// \return the dual of mv
        template<typename U>
        friend Mvec<U> operator!(const Mvec<U> &mv);

        /// \brief boolean operator that tests the equality between two Mvec
        /// \param mv2 - second operand of type Mvec
        /// \return whether two Mvec have the same coefficients
        inline bool operator==(const Mvec& mv2){
            if(gradeBitmap != mv2.gradeBitmap)
                return false;

            return mvData == mv2.mvData;   //// list : ca marche que si les listes sont ordonnees
        }

        /// \brief compute the inverse of a multivector
        /// \return - the inverse of the current multivector
        Mvec<T> inv() const;

            /// \brief operator to test whether two Mvec have not the same coefficients
        /// \param mv2 - second operand of type Mvec
        /// \return boolean that specify the non-equality between two Mvec
        inline bool operator!=(const Mvec& mv2){
            return !(*this == mv2); // issue #4 fixed by replacing Not !(mvData == mv2.mvData) with !(*this == mv2)
        }

        /// \brief Display all the non-null basis blades of this objects
        /// \param stream - destination stream
        /// \param mvec - the multivector to be outputed
        /// \return a stream that contains the list of the non-zero element of the multivector
        template<typename U>
        friend std::ostream& operator<< (std::ostream& stream, const Mvec<U> &mvec);

        /// \brief overload the casting operator, using this function, it is now possible to compute : float a = float(mv);
        /// \return the scalar part of the multivector
        operator T () {
            // if no scalar part, return

            if( (gradeBitmap & 1) == 0 )
                return 0;

            // assuming now that the scalar part exists, return it
            return findGrade(0)->vec[0];
        }

        /// \brief Overload the [] operator to assign a basis blade to a multivector. As an example, float a = mv[E12] = 42.
        /// \param idx - the basis vector index related to the query.
        /// \return the coefficient of the multivector corresponding to the "idx" component.
        /// \todo handle the cases when the number of indices is higher than the dimension, and when the index is too high
        T& operator[](const int idx ){

            const unsigned int grade = xorIndexToGrade[idx];
            const unsigned int idxHomogeneous = xorIndexToHomogeneousIndex[idx];

            auto it = mvData.begin();
            while(it != mvData.end()){

                // if grade not reach yet, continue
                if(it->grade < grade)
                    ++it;
                else{

                    // if grade found
                    if(it->grade == grade)
                        return it->vec[idxHomogeneous];

                    // if grade exceed, create it and inster it before the current element
                    Kvec<T> kvec = {Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(binomialArray[grade]),grade};
                    auto it2 = mvData.insert(it,kvec);
                    gradeBitmap |= 1 << (grade);
                    return it2->vec[idxHomogeneous];
                }
            }

            // if the searched element should be added at the end, add it
            Kvec<T> kvec = {Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(binomialArray[grade]),grade};

            auto it2 = mvData.insert(mvData.end(),kvec);
            gradeBitmap |= 1 << (grade);
            return it2->vec[idxHomogeneous];
        }

        /// \brief Overload the [] operator to copy a basis blade of this multivector. As an example, float a = mv[E12].
        /// \param idx - the basis vector index related to the query.
        /// \return the coefficient of the multivector corresponding to the "idx" component.
        const T& operator[](const int idx) const{

            const unsigned int grade = xorIndexToGrade[idx];
            const unsigned int idxHomogeneous = xorIndexToHomogeneousIndex[idx];

            auto it = mvData.begin();
            while(it != mvData.end()){

                // if grade not reach yet, continue
                if(it->grade < grade)
                    ++it;
                else{

                    // if grade found
                    if(it->grade == grade)
                        return it->vec[idxHomogeneous];

                    // if grade exceed, return a reference on zero
                    return zero<T>;
                }
            }

            // searched element not found
            return zero<T>;
        }

/*
        /// \cond DEV
        // Variadic operators
        /// \brief Overload the () operator to assign a basis blade to a multivector. As an example, consider we have a Mvec a and we want to set its e23 component to 4.7, using operator(), it will consists in writing a[2,3]=4.8.
        /// \tparam Arguments - denotes a variadic list of index
        /// \param listIndices - the list of indices
        /// \return the right index in the homogeneous vectorXd
        /// \todo future work + handle the cases when the number of indices is higher than the dimension, and when the index is too high
        template<typename... List>
        T& operator()(List ...listIndices){
            // operation on these arguments
            // First determine the grade, simply use the variadic function sizeof...()
            // This function is correct in terms of indexing
            const int gd = sizeof...(listIndices); //

            // Second, from these parameters, compute the index in the corresponding VectorXd
            const int idx = (Binomial<algebraDimension,gd>::value-1) - computeIdxFromList(algebraDimension,gd,listIndices...);//Binomial<algebraDimension,binomCoef>::value;//binomCoef - sumB(first,listIndices...);//computeOrderingIndex<>(listIndices...);//(Binomial<algebraDimension,gd>::value-1);//-computeOrderingIndex<int,List>(4,2,listIndices...);//computeOrderingIndex<algebraDimension,gd>(listIndices...);//idxVariadic<algebraDimension,gd,first,listIndices...>::value; //computeOrderingIndex<algebraDimension,gd,listIndices...>();//idxVariadic<algebraDimension,gd,listIndices...>::value; // (Binomial<algebraDimension,gd>::value-1)- idxVariadic<algebraDimension,gd,listIndices...>::value

            if(mvData.count(gd)>0)
                return (mvData.at(gd))(idx);

            mvData[gd] = Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(Binomial<algebraDimension,gd>::value);

            return mvData[gd](idx);
        }
        /// \endcond
*/

        /// \cond DEV
        /// \brief returns a multivector with one component to 1
        /// \param grade : grade of the component to enable
        /// \param index : index of the parameter in the k-vector (k = grade)
        inline Mvec componentToOne(const unsigned int grade, const int index){
            Kvec<T> kvec;
			kvec.vec=Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(binomialArray[grade]);
			kvec.grade=grade;
            kvec.vec[index] = T(1);
            Mvec mv1;
            mv1.mvData.push_back(kvec);
            mv1.gradeBitmap = 1 << (grade);

            return mv1;
        }
        /// \endcond // do not comment this functions

        /// \cond DEV
        /// \brief create a VectorXd if it has not yet been created
        /// \param grade - grade of the considered kvector
        /// \return nothing
        inline typename std::list<Kvec<T>>::iterator createVectorXdIfDoesNotExist(const unsigned int grade){

            auto it = mvData.begin();
            while(it != mvData.end()){

                // if grade not reach yet, continue
                if(it->grade < grade)
                    ++it;
                else{

                    // if grade found
                    if(it->grade == grade)
                        return it;

                    // if grade exceed, create it and inster it before the current element
                    Kvec<T> kvec;
					kvec.vec=Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(binomialArray[grade]);
					kvec.grade=grade;
                    auto it2 = mvData.insert(it,kvec);
                    gradeBitmap |= 1 << (grade);
                    return it2;
                }
            }

            // if the searched element should be added at the end, add it
            Kvec<T> kvec;
			kvec.vec=Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(binomialArray[grade]);
			kvec.grade=grade;
            auto it2 = mvData.insert(mvData.end(),kvec);
            gradeBitmap |= 1 << (grade);
            return it2;
        }

        /// \cond DEV
        /// \brief modify the element of the multivector whose grade is "grade"
        /// \param grade : the grade to access
        /// \param indexVectorXd : index of the k-vector (with k = "grade")
        /// \return the element of the Mv whose grade is grade and index in the VectorXd is indexVectorXd
        inline T& at(const int grade, const int indexVectorXd){
            auto it = createVectorXdIfDoesNotExist(grade);
            return it->vec.coeffRef(indexVectorXd);
        }
        /// \endcond // do not comment this functions

        /// \cond DEV
        /// \brief return the element of the multivector whose grade is "grade"
        /// \param grade : the grade to access
        /// \param indexVectorXd : index of the k-vector (with k = "grade")
        /// \return the element of the Mv whose grade is grade and index in the VectorXd is indexVectorXd
        inline T at(const int grade, const int indexVectorXd) const{

            if((gradeBitmap & (1<<(grade))) == 0 )
                return T(0);

            auto it = findGrade(grade);
            return it->vec.coeff(indexVectorXd);
        }
        /// \endcond // do not comment this functions

        /// \brief the L2-norm of the mv is sqrt( abs( mv.mv ) )
        /// \return the L2-norm of the multivector (as a double)
        T inline norm() const {
            return sqrt( fabs( (*this).scalarProduct( this->reverse() ) ));
        };

        /// \brief the L2-norm over 2 of the mv is mv.mv
        /// \return the L2-norm of the multivector (as a double)
        T inline quadraticNorm() const {
            return ( this->reverse() | (*this) );
        };

        /// \brief compute the dual of a multivector (i.e mv* = reverse(mv) * Iinv). If the metric is degenerated, this function computes the right complement (mv ^ !mv = I).
        /// \return - the dual of the multivector
        Mvec<T> dual() const;

        /// \brief compute the reverse of a multivector
        /// \return - the reverse of the multivector
        Mvec<T> reverse() const;

        /// \brief search in the multivector for a Kvec of grade "grade"
        /// \return return a const iterator on the Kvec if exist, else return mvData.end()
        inline typename std::list<Kvec<T>>::const_iterator findGrade(const unsigned int & gradeToFind) const {
            for(auto it = mvData.begin();it != mvData.end();++it){
                if(it->grade==gradeToFind){
                    return it;
                }
            }
            return mvData.end();
        }

        /// \brief search in the multivector for a Kvec of grade "grade"
        /// \return return an iterator on the Kvec if exist, else return mvData.end()
        inline typename std::list<Kvec<T>>::iterator findGrade(const unsigned int & gradeToFind) {
            for(auto it = mvData.begin();it != mvData.end();++it){
                if(it->grade==gradeToFind){
                    return it;
                }
            }
            return mvData.end();
        }

        /// \brief return the (highest) grade of the multivector
        /// \return the highest grade of the multivector
        inline int grade() const {
            return (mvData.begin() == mvData.end()) ? 0 : mvData.rbegin()->grade;
        }

        /// \brief return the all non-zero grades of the multivector (several grades for non-homogeneous multivectors)
        /// \return a list of the grades encountered in the multivector
        std::vector<unsigned int> grades() const;

        /// \brief returns a multivector that contains all the components of this multivector whose grade is i
        /// \return an empty Mvec if the requested element is not part of the multivector, or the multivector that contains only this element if present in the current multivector.
        Mvec grade(const int i) const;

        /// \brief tell whether the multivector has grade component
        /// \param grade - grade of the considered kvector
        /// \return true if multivector has grade component, false else
        inline bool isGrade(const unsigned int grade) const{
            if((grade == 0) && isEmpty()) return true;
            return ( (gradeBitmap & (1<<grade)) != 0 );
        }

        /// \brief partially or completely erase the content of a multivector
        /// \param grade : if < 0, erase all the multivector, else just erase the part of grade "grade".
        void clear(const int grade = -1);

        /// \brief check is a mutivector is empty, i.e. corresponds to 0.
        /// \return True if the multivector is empty, else False.
        inline bool isEmpty() const {
            return mvData.empty();
        }

        /// \brief A multivector is homogeneous if all its components have the same grade.
        /// \return True if the multivector is homogeneous, else False.
        inline bool isHomogeneous() const {
            return mvData.size() < 2; // only one element, or zero element on the list
        }

        /// \brief inplace simplify the multivector such that all the values with a magnitude lower than a epsilon in the Mv are set to 0.
        /// \param epsilon - threshold, with default value the epsilon of the float/double/long double type from numeric_limits.
        void roundZero(const T epsilon = std::numeric_limits<T>::epsilon());

        /// \brief Specify if two multivectors have the same grade.
        /// \param mv - multivector to compare with.
        /// \return true if the two multivectors have the same grade, else return false.
        inline bool sameGrade(const Mvec<T>& mv) const{
            return grade() == mv.grade();
        }

        /// \brief Display the multivector data (per grade value)
        void display() const;

        /// \cond DEV
        /// \brief a function to output all the element of a k-vector in the multivector indexing order.
        /// \tparam T - type of the multivector
        /// \param stream - stream that will contain the result
        /// \param mvec - multivector to be processed
        /// \param gradeMV - the considered grade
        /// \param moreThanOne - true if it the first element to display (should we put a '+' before)
        template<typename U>
        friend void traverseKVector(std::ostream &stream, const Eigen::Matrix<U, Eigen::Dynamic, 1>, unsigned int gradeMV, bool& moreThanOne);
        /// \endcond // do not comment this functions

/*
        /// \cond DEV
        /// \brief functions that enables to  to be able to extract a component of a multivector as a multivector. As an example, mv1.e(1,3) will create a multivector whose 1,3 component will be the component of mv1.
        /// \tparam Arguments - denotes a variadic list of index
        /// \param listIndices - the list of indices
        /// \return a homogeneous multivector filled with zero except the listIndices index
        /// \todo future work + handle the cases when the number of indices is higher than the dimension, and when the index is too high
        template<typename... List>
        Mvec e(List ...listIndices){
            // operation on these arguments
            // First determine the grade, simply use the variadic function sizeof...()
            const int gd = sizeof...(listIndices); //

            // Second, from these parameters, compute the index in the corresponding VectorXd
            const int idx = (Binomial<algebraDimension,gd>::value-1) - computeIdxFromList(algebraDimension,gd,listIndices...);

            Mvec res;
            res.mvData[gd] = Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(Binomial<algebraDimension,gd>::value);
            res.mvData[gd](idx) = mvData[gd](idx);

            return res;
        }
        /// \endcond // do not comment this functions
*/

        /// \brief functions that enables to extract one component of this multivector and initialize another mv with this component
        /// \tparam Arguments - denotes a variadic list of index
        /// \param grade : the considered grade
        /// \param sizeOfKVector: C(dimension, grade)
        /// \param indexInKvector: the considerd index
        /// \return a new mv with the right component at the right place
        Mvec extractOneComponent(const int grade, const int sizeOfKVector, const int indexInKvector) const {
            Mvec mv;
            auto itThisMV = this->findGrade(grade);
            if(itThisMV==this->mvData.end()){
                return mv;
            }
            mv = mv.componentToOne(grade,indexInKvector);
            mv.mvData.begin()->vec.coeffRef(indexInKvector) = itThisMV->vec.coeff(indexInKvector);
            return mv;
        }

        /// \brief set of functions that return multivectors containing only the value '1' for the specified component.
project_multivector_one_component

    };  // end of the class definition

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

    template<typename T>
    Mvec<T>::Mvec():gradeBitmap(0)
    {}

    template<typename T>
    Mvec<T>::Mvec(const Mvec& mv) : mvData(mv.mvData), gradeBitmap(mv.gradeBitmap)
    {
        //std::cout << "copy constructor " << std::endl;
    }

    template<typename T>
    Mvec<T>::Mvec(Mvec<T>&& multivector) : mvData(std::move(multivector.mvData)), gradeBitmap(multivector.gradeBitmap)
    {
        // the move constructor
        //std::cout << "move constructor" << std::endl;
    }

    template<typename T>
    template<typename U>
    Mvec<T>::Mvec(const Mvec<U> &mv) : gradeBitmap(mv.gradeBitmap)
    {
        for(auto it=mv.mvData.begin(); it != mv.mvData.end(); ++it){
            Kvec<T> kvec;
            kvec.grade = it->grade;
            kvec.vec = Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(binomialArray[it->grade]);
            for(unsigned int i=0; i<it->vec.size(); ++i)
                kvec.vec.coeffRef(i) = T(it->vec.coeff(i));
            mvData.push_back(kvec);
        }
    }

    template<typename T>
    template<typename U>
    Mvec<T>::Mvec(const U val) {
        if(val != U(0)) {
            gradeBitmap = 1;
            Kvec<T> kvec;
            kvec.vec =Eigen::Matrix<T, Eigen::Dynamic, 1>(1);
            kvec.grade =0;
            mvData.push_back(kvec);
            mvData.begin()->vec.coeffRef(0) = val;
        }
    }

    template<typename T>
    Mvec<T>::~Mvec()
    {}

    template<typename T>
    Mvec<T>& Mvec<T>::operator=(const Mvec& mv){
        if(&mv == this) return *this;
        gradeBitmap = mv.gradeBitmap;
        mvData = mv.mvData;
        return *this;
    }

    template<typename T>
    Mvec<T>& Mvec<T>::operator=(Mvec&& mv){
        mvData = std::move(mv.mvData);
        gradeBitmap = mv.gradeBitmap;
        return *this;
    }

    template<typename T>
    Mvec<T> Mvec<T>::operator+(const Mvec<T> &mv2) const {
        Mvec<T> mv3(*this);
        for(auto & itMv : mv2.mvData) {
            auto it = mv3.createVectorXdIfDoesNotExist(itMv.grade);
            it->vec += itMv.vec;
        }
        return mv3;
    }

    template<typename U, typename S>
    Mvec<U> operator+(const S &value, const Mvec<U> &mv){
        return mv + value;
    }

    template<typename T>
    template<typename S>
    Mvec<T> Mvec<T>::operator+(const S &value) const {
        Mvec<T> mv(*this);
        if(value != T(0)) {
            auto it = mv.createVectorXdIfDoesNotExist(0);
            it->vec.coeffRef(0) += value;
        }
        return mv;
    }

    template<typename T>
    Mvec<T>& Mvec<T>::operator+=(const Mvec& mv){
        *this = *this + mv;
        return *this;
    }

    template<typename T>
    Mvec<T> operator-(const Mvec<T> &mv) { // unary -
        Mvec<T> mv2(mv);
        for(auto & itMv : mv2.mvData)
            itMv.vec = -itMv.vec;
        return mv2;
    }

    template<typename T>
    Mvec<T> Mvec<T>::operator-(const Mvec<T> &mv2) const {
        Mvec<T> mv3(*this);
        for(auto & itMv : mv2.mvData) {
            auto it = mv3.createVectorXdIfDoesNotExist(itMv.grade);
            it->vec -= itMv.vec;
        }
        return mv3;
    }

    template<typename T>
    Mvec<T>& Mvec<T>::operator-=(const Mvec& mv){
        *this = *this - mv;
        return *this;
    }

    template<typename U, typename S>
    Mvec<U> operator-(const S &value, const Mvec<U> &mv){
        return (-mv) + value;
    }

    template<typename T>
    template<typename S>
    Mvec<T> Mvec<T>::operator-(const S &value) const {
        Mvec<T> mv(*this);
        if(value != T(0)) {
            auto it = mv.createVectorXdIfDoesNotExist(0);
            it->vec.coeffRef(0) -= value;
        }
        return mv;
    }

    template<typename T>
    Mvec<T> Mvec<T>::operator^(const Mvec<T> &mv2) const {
#if 0 // only recursive version
        // Loop over non-empty grade of mv1 and mv2
        // This version (with recursive call) is only faster than the standard recursive call if
        // the multivector mv1 and mv2 are homogeneous or near from homogeneous.
        Mvec<T> mv3;
        for(const auto & itMv1 : this->mvData)    // all per-grade component of mv1
            for(const auto & itMv2 : mv2.mvData)  // all per-grade component of mv2
            {
                unsigned int grade_mv3 = itMv1.grade + itMv2.grade;
                if(grade_mv3 <= algebraDimension) {
                    auto itMv3 = mv3.createVectorXdIfDoesNotExist(grade_mv3);
                    outerProductHomogeneous(itMv1.vec, itMv2.vec, itMv3->vec,
                                            itMv1.grade, itMv2.grade, grade_mv3);
                }
            }
        return mv3;
#else // use the adaptative pointer function array
        // Loop over non-empty grade of mv1 and mv2
        // call the right explicit unrolled outer function using the functions pointer called outerFunctionsContainer
        Mvec<T> mv3;
        for(const auto & itMv1 : this->mvData)
            for(const auto & itMv2 : mv2.mvData){
                if(itMv1.grade + itMv2.grade <= (int) algebraDimension ){
                    auto itMv3 = mv3.createVectorXdIfDoesNotExist(itMv1.grade + itMv2.grade);
                    outerFunctionsContainer<T>[itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);
                }
            }
        return mv3;
#endif
    }

    template<typename U, typename S>
    Mvec<U> operator^(const S &value, const Mvec<U> &mv) {
        return  mv^value;
    }

    template<typename T>
    template<typename S>
    Mvec<T> Mvec<T>::operator^(const S &value) const {
        Mvec<T> mv2(*this);
        for(auto & itMv : mv2.mvData)
            itMv.vec *= T(value);
        return mv2;
    }

    template<typename T>
    Mvec<T> &Mvec<T>::operator^=(const Mvec &mv) {
        *this = *this ^ mv;
        return *this;
    }

    template<typename T>
    Mvec<T> Mvec<T>::operator|(const Mvec<T> &mv2) const{
        // Loop over non-empty grade of mv1 and mv2
        // call the right explicit unrolled inner function using the functions pointer called innerFunctionsContainer
        Mvec<T> mv3;
        for(const auto & itMv1 : this->mvData)
            for(const auto & itMv2 : mv2.mvData){

                // perform the inner product
                int absGradeMv3 = std::abs((int)(itMv1.grade - itMv2.grade));
                auto itMv3 = mv3.createVectorXdIfDoesNotExist(absGradeMv3);
                innerFunctionsContainer<T>[itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);

                // check if the result is non-zero
                if(!((itMv3->vec.array() != 0.0).any())){
                    mv3.mvData.erase(itMv3);
                    mv3.gradeBitmap &= ~(1<<absGradeMv3);
                }
            }
        return mv3;
    }

    template<typename U, typename S>
    Mvec<U> operator|(const S &value, const Mvec<U> &mv) {
        return mv | value;
    }

    template<typename T>
    template<typename S>
    Mvec<T> Mvec<T>::operator|(const S &value) const {
        return (*this) > value ; // inner between a mv and a scalar gives 0 (empty multivector)
    }

    template<typename T>
    Mvec<T> &Mvec<T>::operator|=(const Mvec &mv) {
        *this = *this | mv;
        return *this;
    }

    template<typename T>
    Mvec<T> Mvec<T>::operator>(const Mvec<T> &mv2) const{
        // Loop over non-empty grade of mv1 and mv2
        // call the right explicit unrolled inner function using the functions pointer called innerFunctionsContainer
        Mvec<T> mv3;
        for(const auto & itMv1 : this->mvData)
            for(const auto & itMv2 : mv2.mvData){

                // right contraction constraint: gradeMv1 >= gradeMv2
                if(itMv1.grade < itMv2.grade)
                    continue;

                // perform the inner product
                int absGradeMv3 = std::abs((int)(itMv1.grade - itMv2.grade));
                auto itMv3 = mv3.createVectorXdIfDoesNotExist(absGradeMv3);
                innerFunctionsContainer<T>[itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);

                // check if the result is non-zero
                if(!((itMv3->vec.array() != 0.0).any())){
                    mv3.mvData.erase(itMv3);
                    mv3.gradeBitmap &= ~(1<<absGradeMv3);
                }
            }

        return mv3;
    }

    template<typename U, typename S>
    Mvec<U> operator>(const S &value, const Mvec<U> &mv) {
        if( (mv.gradeBitmap & 1) == 0) return Mvec<U>();
        else return Mvec<U>( U(mv.mvData.begin()->vec.coeff(0) * value ) );
    }

    template<typename T>
    template<typename S>
    Mvec<T> Mvec<T>::operator>(const S &value) const {
        // return mv x value
        Mvec<T> mv(*this);
        for(auto & itMv : mv.mvData)
            itMv.vec *= T(value);
        return mv;
    }

    template<typename T>
    Mvec<T> Mvec<T>::operator<(const Mvec<T> &mv2) const{

        // Loop over non-empty grade of mv1 and mv2
        // call the right explicit unrolled inner function using the functions pointer called innerFunctionsContainer
        Mvec<T> mv3;
        for(const auto & itMv1 : this->mvData)
            for(const auto & itMv2 : mv2.mvData){

                // left contraction constraint: gradeMv1 <= gradeMv2
                if(itMv1.grade > itMv2.grade)
                    continue;

                // perform the inner product
                int absGradeMv3 = std::abs((int)(itMv1.grade - itMv2.grade));
                auto itMv3 = mv3.createVectorXdIfDoesNotExist(absGradeMv3);
                innerFunctionsContainer<T>[itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);

                // check if the result is non-zero
                if(!((itMv3->vec.array() != 0.0).any())){
                    mv3.mvData.erase(itMv3);
                    mv3.gradeBitmap &= ~(1<<absGradeMv3);
                }
            }
        return mv3;
    }

    template<typename U, typename S>
    Mvec<U> operator<(const S &value, const Mvec<U> &mv) {
        // return mv x value
        Mvec<U> mv3(mv);
        for(auto & itMv : mv3.mvData)
            itMv.vec *= U(value);
        return mv3;
    }

    template<typename T>
    template<typename S>
    Mvec<T> Mvec<T>::operator<(const S &value) const {
        if( (gradeBitmap & 1) == 0) return Mvec<T>();
        else return Mvec<T>( T(mvData.begin()->vec.coeff(0) * value ) );
    }

    template<typename T>
    Mvec<T> Mvec<T>::scalarProduct(const Mvec<T> &mv2) const{
        // Loop over non-empty grade of mv1 and mv2
        // call the right explicit unrolled inner function using the functions pointer called innerFunctionsContainer
        Mvec<T> mv3;
        for(const auto & itMv1 : this->mvData)
            for(const auto & itMv2 : mv2.mvData){

                if(itMv1.grade != itMv2.grade)
                    continue;

                // perform the inner product
                int absGradeMv3 = 0;
                auto itMv3 = mv3.createVectorXdIfDoesNotExist(absGradeMv3);
                innerFunctionsContainer<T>[itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);

                // check if the result is non-zero
                if(!((itMv3->vec.array() != 0.0).any())){
                    mv3.mvData.erase(itMv3);
                    mv3.gradeBitmap &= ~(1<<absGradeMv3);
                }
            }
        return mv3;
    }

project_singular_metric_comment_begin
    template<typename T>
    Mvec<T> Mvec<T>::outerPrimalDual(const Mvec<T> &mv2) const{
        Mvec<T> mv3;

        for(const auto & itMv1 : this->mvData)    // all per-grade component of mv1
            for(const auto & itMv2 : mv2.mvData)  // all per-grade component of mv2
            {
                unsigned int grade_mv3 = itMv1.grade + (algebraDimension-itMv2.grade);
                if(grade_mv3 <= algebraDimension) {
                    // handle the scalar product as well as the left contraction
                    auto itMv3 = mv3.createVectorXdIfDoesNotExist(grade_mv3);
                    outerProductPrimalDual(itMv1.vec, itMv2.vec, itMv3->vec,
                                           itMv1.grade, itMv2.grade, (unsigned)(algebraDimension-grade_mv3));

                    // check if the result is non-zero
                    if(!((itMv3->vec.array() != 0.0).any())){
                        mv3.mvData.erase(itMv3);
                        mv3.gradeBitmap &= ~(1<<grade_mv3);
                    }
                }
            }

        return mv3;
    }

    template<typename T>
    Mvec<T> Mvec<T>::outerDualPrimal(const Mvec<T> &mv2) const{
        Mvec<T> mv3;

        for(const auto & itMv1 : this->mvData)    // all per-grade component of mv1
            for(const auto & itMv2 : mv2.mvData)  // all per-grade component of mv2
            {
                unsigned int grade_mv3 = itMv1.grade + (algebraDimension-itMv2.grade);
                if(grade_mv3 <= algebraDimension) {
                    // handle the scalar product as well as the left contraction
                    auto itMv3 = mv3.createVectorXdIfDoesNotExist(grade_mv3);
                    outerProductDualPrimal(itMv1.vec, itMv2.vec, itMv3->vec,
                                           itMv1.grade, itMv2.grade, (unsigned)(algebraDimension-grade_mv3));

                    // check if the result is non-zero
                    if(!((itMv3->vec.array() != 0.0).any())){
                        mv3.mvData.erase(itMv3);
                        mv3.gradeBitmap &= ~(1<<grade_mv3);
                    }
                }
            }

        return mv3;

    }

    template<typename T>
    Mvec<T> Mvec<T>::outerDualDual(const Mvec<T> &mv2) const{
        Mvec<T> mv3;

        for(const auto & itMv1 : this->mvData)    // all per-grade component of mv1
            for(const auto & itMv2 : mv2.mvData)  // all per-grade component of mv2
            {
                unsigned int grade_mv3 = itMv1.grade + (algebraDimension-itMv2.grade);
                if(grade_mv3 <= algebraDimension) {
                    // handle the scalar product as well as the left contraction
                    auto itMv3 = mv3.createVectorXdIfDoesNotExist(grade_mv3);
                    outerProductDualDual(itMv1.vec, itMv2.vec, itMv3->vec,
                                           itMv1.grade, itMv2.grade, (unsigned)(algebraDimension-grade_mv3));

                    // check if the result is non-zero
                    if(!((itMv3->vec.array() != 0.0).any())){
                        mv3.mvData.erase(itMv3);
                        mv3.gradeBitmap &= ~(1<<grade_mv3);
                    }
                }
            }

        return mv3;

    }

project_singular_metric_comment_end

    template<typename T>
    Mvec<T> Mvec<T>::hestenesProduct(const Mvec<T> &mv2) const{
        // Loop over non-empty grade of mv1 and mv2
        // call the right explicit unrolled inner function using the functions pointer called innerFunctionsContainer
        Mvec<T> mv3;
        for(const auto & itMv1 : this->mvData)
            for(const auto & itMv2 : mv2.mvData){

                if(itMv1.grade*itMv2.grade == 0)
                    continue;

                // perform the inner product
                int absGradeMv3 = std::abs((int)(itMv1.grade - itMv2.grade));
                auto itMv3 = mv3.createVectorXdIfDoesNotExist(absGradeMv3);
                innerFunctionsContainer<T>[itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);

                // check if the result is non-zero
                if(!((itMv3->vec.array() != 0.0).any())){
                    mv3.mvData.erase(itMv3);
                    mv3.gradeBitmap &= ~(1<<absGradeMv3);
                }
            }
        return mv3;
    }

    template<typename T>
    Mvec<T> Mvec<T>::operator*(const Mvec<T> &mv2) const {
        // Loop over non-empty grade of mv1 and mv2
        // call the right explicit unrolled product function using the functions pointer
        Mvec<T> mv3;
        for(const auto & itMv1 : this->mvData)
            for(const auto & itMv2 : mv2.mvData){
                project_select_recursive_geometric_product_template
                // outer product block
                unsigned int gradeOuter = itMv1.grade + itMv2.grade;
                if(gradeOuter <=  algebraDimension ){
                    auto itMv3 = mv3.createVectorXdIfDoesNotExist(gradeOuter);
                    outerFunctionsContainer<T>[itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);
                    if(!((itMv3->vec.array() != 0.0).any())){
                        mv3.mvData.erase(itMv3);
                        mv3.gradeBitmap &= ~(1<<gradeOuter);
                    }
                }

                // inner product block
                unsigned int gradeInner = (unsigned int)std::abs((int)(itMv1.grade-itMv2.grade));
                // when the grade of one of the kvectors is zero, the inner product is the same as the outer product
                if(gradeInner != gradeOuter) {
                    auto itMv3 = mv3.createVectorXdIfDoesNotExist(gradeInner);
                    innerFunctionsContainer<T>[itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);
                    // check if the result is non-zero
                    if(!((itMv3->vec.array() != 0.0).any())){
                        mv3.mvData.erase(itMv3);
                        mv3.gradeBitmap &= ~(1<<gradeInner);
                    }

                    // geometric product part
                    int gradeMax = std::min(((2*algebraDimension)-gradeOuter)+1,gradeOuter);
                    for (int gradeResult = gradeInner+2; gradeResult < gradeMax; gradeResult+=2) {
                        auto itMv3 = mv3.createVectorXdIfDoesNotExist(gradeResult);
                        geometricFunctionsContainer<T>[gradeResult][itMv1.grade][itMv2.grade](itMv1.vec, itMv2.vec, itMv3->vec);
                        // check if the result is non-zero
                        if(!((itMv3->vec.array() != 0.0).any())){
                            mv3.mvData.erase(itMv3);
                            mv3.gradeBitmap &= ~(1<<gradeResult);
                        }
                    }
                }
            }
        return mv3;
    }

    template<typename U, typename S>
    Mvec<U> operator*(const S &value, const Mvec<U> &mv){
        return mv * value;
    }

    template<typename T>
    template<typename S>
    Mvec<T> Mvec<T>::operator*(const S &value) const{
        Mvec<T> mv(*this);
        for(auto & itMv : mv.mvData)
            itMv.vec *= T(value);
        return mv;
    }

    template<typename T>
    Mvec<T> &Mvec<T>::operator*=(const Mvec &mv) {
        *this = *this * mv;
        return *this;
    }

    template<typename T>
    Mvec<T> Mvec<T>::operator/(const Mvec<T> &mv2) const {
        return *this * mv2.inv();
    }

    template<typename U, typename S>
    Mvec<U> operator/(const S &value, const Mvec<U> &mv){
        return value * mv.inv();
    }

    template<typename T>
    template<typename S>
    Mvec<T> Mvec<T>::operator/(const S &value) const {
        Mvec<T> mv(*this);
        for(auto & itMv : mv.mvData)
            itMv.vec /= value;
        return mv;
    }

    template<typename T>
    Mvec<T> &Mvec<T>::operator/=(const Mvec &mv) {
        *this = *this / mv;
        return *this;
    }

    template<typename T>
    Mvec<T> operator~(const Mvec<T> &mv){
        return mv.reverse();
    }

    template<typename T>
    Mvec<T> Mvec<T>::inv() const {
        T n = this->quadraticNorm();
        if(n<std::numeric_limits<T>::epsilon() && n>-std::numeric_limits<T>::epsilon())
            return Mvec<T>(); // return 0, this is was gaviewer does.
        return this->reverse() / n;
    };

    template<typename U>
    Mvec<U> operator!(const Mvec<U> &mv) {
        return mv.dual();
    }

    /// \brief defines the left contraction between two multivectors
    /// \param mv1 - a multivector
    /// \param mv2 - second operand, corresponds to a multivector
    /// \return mv1 left contraction mv2
    template<typename T>
    Mvec<T> leftContraction(const Mvec<T> &mv1, const Mvec<T> &mv2){
        return mv1 < mv2;
    }

    /// \brief defines the right contraction between two multivectors
    /// \param mv1 - a multivector
    /// \param mv2 - second operand, corresponds to a multivector
    /// \return mv1 right contraction mv2
    template<typename T>
    Mvec<T> rightContraction(const Mvec<T> &mv1, const Mvec<T> &mv2){
        return mv1 > mv2;
    }

    /// \brief returns a multivector that only contains the coefficient associated to the pseudoscalar.
    /// \return an empty Mvec if the requested element is not part of the multivector, or the multivector that contains only this element if present in the current multivector.
    template<typename T>
    Mvec<T> I() {
        Mvec<T> mvec;
        mvec[project_pseudo_scalar] = 1.0;
        return mvec;
    }

project_singular_metric_comment_begin
    /// \brief return the inverse of the pseudo scalar.
    /// \return returns a multivector corresponding to the inverse of the pseudo scalar.
    template<typename T>
    Mvec<T> Iinv() {
        Mvec<T> mvec;
        mvec[project_pseudo_scalar] = pseudoScalarInverse; // we do not return just a scalar of type T because the grade should be dimension and not 0.
        return mvec;
    }
project_singular_metric_comment_end

    // compute the dual of a multivector (i.e mv* = mv.reverse() * Iinv)
    template<typename T>
    Mvec<T> Mvec<T>::dual() const {
        Mvec<T> mvResult;
        // for each k-vectors of the multivector
        for(auto itMv=mvData.rbegin(); itMv!=mvData.rend(); ++itMv){

            // create the dual k-vector
            Kvec<T> kvec ={itMv->vec,algebraDimension-(itMv->grade)};

            // some elements need to be permuted
            for(unsigned int i=0;i<binomialArray[itMv->grade];++i)
                kvec.vec.coeffRef(dualPermutations[itMv->grade][i]) = itMv->vec.coeff(i);

            // the inner product may involve some constant multiplucation for the dual elements
            kvec.vec = kvec.vec.cwiseProduct(dualCoefficients[itMv->grade].template cast<T>());

            // add the k-vector to the resulting multivector
            mvResult.mvData.push_back(kvec);
            mvResult.gradeBitmap |= (1 << kvec.grade);
        }
        return mvResult;
    };

    // \brief compute the reverse of a multivector
    // \return - the reverse of the multivector
    template<typename T>
    Mvec<T> Mvec<T>::reverse() const {
        Mvec<T> mv(*this);
        for(auto & itMv : mv.mvData)
            if(signReversePerGrade[itMv.grade] == -1)
                itMv.vec *= -1;
        return mv;
    }

    template<typename T>
    Mvec<T> Mvec<T>::grade(const int i) const{

        auto it = findGrade(i);
        Mvec<T> mv;

        // if not found, return the empty multivector
        if(it == mvData.end())
            return mv;

        // else return the grade 'i' data
        mv.mvData.push_back(*it);
        mv.gradeBitmap = 1 << (i);

        return mv;
    }

    template<typename T>
    std::vector<unsigned int> Mvec<T>::grades() const {
        std::vector<unsigned int> mvGrades;
        for(unsigned int i=0; i< algebraDimension+1; ++i){

            if( (gradeBitmap & (1<<i)))
                mvGrades.push_back(i);
        }

        return mvGrades;
    }

    template<typename T>
    void Mvec<T>::roundZero(const T epsilon) {
        // loop over each k-vector of the multivector
        auto itMv=mvData.begin();
        while(itMv != mvData.end()){
            // loop over each element of the k-vector
            for(unsigned int i=0; i<(unsigned int)itMv->vec.size(); ++i)
                if(fabs(itMv->vec.coeff(i)) <= epsilon)
                    itMv->vec.coeffRef(i) = 0.0;
            // if the k-vector is full of 0, remove it
            if(!((itMv->vec.array() != 0.0).any())){
                gradeBitmap = gradeBitmap - (1 << itMv->grade);
                mvData.erase(itMv++);
            }
            else ++itMv;
        }
    }

    template<typename T>
    void Mvec<T>::clear(const int grade) {

        // full erase
        if(grade < 0){
            mvData.clear();
            gradeBitmap = 0;
            return;
        }

        // partial erase
        auto iter = findGrade(grade);
        if(iter != mvData.end()) {
            gradeBitmap = gradeBitmap - (1 << iter->grade);
            iter = mvData.erase(iter);
        }
    }

    template<typename T>
    void Mvec<T>::display() const {
        if(mvData.size() == 0)
            std::cout << " null grade , null value " <<std::endl;

        for(auto itmv=mvData.begin(); itmv!=mvData.end(); ++itmv){
            std::cout << "  grade   : " << itmv->grade  << std::endl;
            std::cout << "  kvector : " << itmv->vec.transpose() << std::endl;
        }
        std::cout << std::endl;
    }

    /// \cond DEV
    template<typename U>
    void traverseKVector(std::ostream &stream, const Eigen::Matrix<U, Eigen::Dynamic, 1> kvector, unsigned int gradeMV, bool& moreThanOne ){

        // version with XOR indices
        // for the current grade, generate all the XOR indices
        std::vector<bool> booleanXorIndex(algebraDimension);
        std::fill(booleanXorIndex.begin(), booleanXorIndex.end(), false);
        // build the first xorIndex of grade 'grade' (with 'grade' values to true).
        std::fill(booleanXorIndex.begin(), booleanXorIndex.begin() + gradeMV, true);
        unsigned int positionInKVector = 0;

        do {
            // convert the vector of bool to the string containing all the basis vectors
            std::string basisBlades={};
            for(unsigned int i=0; i<algebraDimension; ++i) {
                if(booleanXorIndex[i]) {
                    basisBlades += basisVectors[i];
                }
            }

            if(kvector.coeff(positionInKVector)!= 0) {
                if(!(moreThanOne)){
                    stream<< kvector.coeff(positionInKVector) << "*e"+ basisBlades;
                    moreThanOne = true;
                }else{
                    if(kvector.coeff(positionInKVector)>0)
                        stream<< " + " << kvector.coeff(positionInKVector) << "*e" + basisBlades;
                    else
                        stream<< " - " << -kvector.coeff(positionInKVector) << "*e" + basisBlades;
                }
            }

            positionInKVector++;

        } while(std::prev_permutation(booleanXorIndex.begin(), booleanXorIndex.end())); // compute next permutation of the true values of booleanXorIndex

    }
    /// \endcond

    template<typename U>
    std::ostream &operator<<(std::ostream &stream, const Mvec<U> &mvec) {
        if(mvec.mvData.size()==0){
            stream << " 0 ";
            return stream;
        }

        bool moreThanOne = false;
        auto mvIterator = mvec.mvData.begin();

        // if the iterator corresponds to the scalar
        if(mvIterator->grade == 0){
            stream << mvIterator->vec.coeff(0);
            moreThanOne = true;
            ++mvIterator;
        }

        // for all other k-vectors of mvec
        for(; mvIterator!=mvec.mvData.end(); ++mvIterator){

            // call the function that covers a single k-vector
            traverseKVector(stream,mvIterator->vec,mvIterator->grade, moreThanOne);
        }

        if(!moreThanOne)
            stream << " 0 ";

        return stream;
    }

    // static unit multivector functions (returns a multivector with only one component)
project_static_multivector_one_component

    //    template<typename U>
    //    void recursiveTraversalMultivector(std::ostream &stream, const Mvec<U> &mvec, unsigned int currentGrade, int currentIndex, std::vector<int> listBasisBlades, unsigned int lastIndex, unsigned int gradeMV, bool& moreThanOne);

    void temporaryFunction1();

}     /// End of Namespace

#endif // project_inclusion_guard


#include <metaSMT/frontend/QF_BV.hpp>

#include <metaSMT/DirectSolver_Context.hpp>
#include <metaSMT/backend/Boolector.hpp>
#include <metaSMT/backend/SAT_Clause.hpp>
#include <metaSMT/backend/MiniSAT.hpp>
#include <metaSMT/backend/PicoSAT.hpp>
#include <metaSMT/backend/CUDD_Context.hpp>

#include <metaSMT/BitBlast.hpp>
#include <metaSMT/support/run_algorithm.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <boost/bimap.hpp>
#include <boost/timer.hpp>

using namespace metaSMT;
using namespace metaSMT::logic;
using namespace metaSMT::logic::QF_BV;
using namespace metaSMT::solver;
using namespace std;

#define foreach BOOST_FOREACH

typedef std::vector<unsigned> guess_type;

class secret {
  public:
    secret ( unsigned width=4, unsigned noptions=6)
    : _noptions(noptions), _solution(width)
    {
      foreach( unsigned & v, _solution) {
        v = rand() % noptions;
      }
      //for (unsigned i = 0; i < width; ++i) {
      //  _solution[i] = (1+i)%noptions;
      //}
    }

    unsigned options() { return _noptions; }
    unsigned width() { return _solution.size(); }

    std::pair<unsigned, unsigned> check( guess_type const & guess )
    {
      unsigned correct = 0;
      unsigned wrong_place = 0;

      assert(guess.size() == _solution.size() );

      std::vector<int> count( _noptions, 0);

      for (unsigned i = 0; i < _solution.size(); ++i) {
        if ( guess[i] == _solution[i] ) {
          ++ correct;
        }
        ++ count[ guess[i] ];
      }

      foreach( unsigned v, _solution) {
        if( count[v] >0 ) {
          ++wrong_place;
          --count[v];
        }
      }

      return std::make_pair(correct, wrong_place-correct);
    }

    template <typename O>
    friend O & operator<< (O& o, secret  const & s) {
      bool next=false;
      foreach( unsigned v, s._solution) {
        if(next) o << ", ";
        else next =true;
        o << v;

      }
      return o;
    }

  private:
    const unsigned _noptions;
    std::vector<unsigned> _solution;

};

template<typename Solver>
struct guesser
{
  guesser( unsigned width, unsigned noptions)
    : _secret(width, noptions)
    , _bitwidth( _secret.options() )
  {
    init_solver();
  }

  typedef bool result_type;

  result_type operator() () {
    boost::timer all_timer;

    boost::format fmt(" -> %2u / %2u ");

    guess_type guess( _secret.width() );
    //for (unsigned i = 0; i < 20; ++i) {
    for (unsigned i = 0; true; ++i) {
      boost::timer cur_timer;
      if( ! solve(_solver) ) {
        std::cout << "problem instance unsatisfiable!" << std::endl;
        break;
      }
      double cur_t = cur_timer.elapsed();
      cout << "guess " << boost::format("%2u")%i << ":";
      for (unsigned k = 0; k < _secret.width(); ++k) {
        guess[k] = read_value( _solver, _vars[k]);
        cout << " " << boost::format("%2u") % guess[k];
      }
      cout.flush();
      unsigned correct, wrong_place;
      boost::tie( correct, wrong_place) = _secret.check(guess);
      std::cout
        << fmt % correct % wrong_place
        << boost::format(" (%3.2fs, %3.2fs)")
          % cur_t
          % all_timer.elapsed()
        << std::endl;
      if( correct == _secret.width()) {
        return true;
      } else {
        block_correct( guess, correct);
        if( wrong_place > 0) {
          block_wrong_place( guess, correct + wrong_place);
        }
      }
    }

    return false;
  }

  void init_solver() {
    for (unsigned i = 0; i < _secret.width(); ++i) {
      _vars.push_back( new_bitvector( _bitwidth ) );
      metaSMT::assertion ( _solver, bvult( _vars[i],
          bvuint( _secret.options(), _bitwidth)) );
    }
  }

  void block_correct( guess_type const & guess
    , unsigned correct
  ) {
    unsigned width = _secret.width();

    typename Solver::result_type sum_equal = evaluate( _solver, bvuint(0 , width));
    for (unsigned i = 0; i < width; ++i) {
      sum_equal = evaluate( _solver, bvadd( sum_equal,
        zero_extend(width-1, bvcomp( _vars[i], bvuint( guess[i], _bitwidth)))
        ));
    }
    metaSMT::assertion( _solver, metaSMT::logic::equal( sum_equal, bvuint( correct, width)));
    metaSMT::assertion( _solver, nequal( sum_equal, bvuint( width, width)));
  }

  void block_wrong_place( guess_type const & guess
    , unsigned anywhere
  ) {
    unsigned width   = _secret.width();
    unsigned options = _secret.options();

    std::vector<unsigned> by_color ( options, 0);
    foreach( unsigned g, guess) {
      ++by_color[g];
    }

    std::vector< typename Solver::result_type> count_by_color( options,
        evaluate( _solver, bvuint(0 , width)));

    for (unsigned i = 0; i < width; ++i) {
      // iterate over all variables
      // and count by colors if less than by_color

      for (unsigned c = 0; c < options; ++c) {
        count_by_color[c] = evaluate( _solver,
            Ite(
                And( metaSMT::logic::equal(_vars[i], bvuint( c, _bitwidth) ),
                   bvult(count_by_color[c], bvuint( by_color[c], width)))
              , bvadd( count_by_color[c], bvuint(1, width))
              , count_by_color[c]
            ));
      }
    }

    typename Solver::result_type sum = evaluate( _solver, bvuint(0 , width));
    foreach( typename Solver::result_type r, count_by_color) {
      sum = evaluate(_solver, bvadd( sum, r));
    }

    metaSMT::assertion( _solver, metaSMT::logic::equal(sum, bvuint(anywhere, width)) );
  }

  secret _secret;
  Solver _solver;
  std::vector< bitvector > _vars;
  unsigned _bitwidth;
};

int
main(int argc, const char *argv[])
{
  FILE *dev_urandom;
  uint32_t seed;
  if((dev_urandom = fopen("/dev/urandom", "r")) == NULL) return 2;
  if(fread(&seed, sizeof(seed), 1, dev_urandom) == 0) return 3;
  if(fclose(dev_urandom) != 0) return 4;
  srand( seed );
  typedef mpl::vector <
      DirectSolver_Context < Boolector >
    , DirectSolver_Context < BitBlast < SAT_Clause < MiniSAT > > >
    , DirectSolver_Context < BitBlast < SAT_Clause < PicoSAT > > >
    , DirectSolver_Context < BitBlast < CUDD_Context > >
    > SolverVec;

  if( argc < 2) {
    cout << "usage: "<< argv[0] << "  solver ?width ?options\nsolver:\n\t0 - Boolector (SMT)\n\t1 - MiniSAT (SAT)\n\t2 - PicoSAT (SAT)\n\t3 - CUDD (BDD)" << endl;
    exit(1);
  }

  unsigned solver = atoi ( argv[1] );
  unsigned width = (argc > 2) ? atoi(argv[2]) : 4;
  unsigned options = (argc > 3) ? atoi(argv[3]) : 6;

  bool val = run_algorithm<SolverVec, guesser> ( solver, width, options );

  std::cout << "secret guessed? " << (val ? "yes" : "no") << std::endl;

  return val ? 0 : 1;
}


/* This source file contains all utilities that are exported to R through Rcpp. Any other function
 * should be put here in order to preserve coherence in the code
 */

#ifndef SPMIX_EXPORTS
#define SPMIX_EXPORTS

// [[Rcpp::depends(BH)]]
// [[Rcpp::depends(RcppEigen)]]
// [[Rcpp::depends(RcppParallel)]]
// [[Rcpp::depends(StanHeaders)]]
#define STRICT_R_HEADERS
#include <stan/math/fwd/mat.hpp>
#include <stan/math/mix/mat.hpp>
#include <stan/math.hpp>
#include <Rcpp.h>
#include <RcppEigen.h>

#include <deque>
#include <string>
#include <vector>
#include <Eigen/Dense>
#include <progress.hpp>
#include <progress_bar.hpp>
#include <utility>
#include <exception>
#include <google/protobuf/text_format.h>

#include "utils.h"
#include "functors.h"
#include "sampler.h"
#include "sampler_rjmcmc.h"
#include "univariate_mixture_state.pb.h"

//' Additive Log Ratio
//'
//' \loadmathjax This utility computes the additive log-ratio transform of a given vector. Given a generic vector of the simplex
//' \mjseqn{x \in S^{H}}, the transformation is defined as:
//' \mjsdeqn{ \operatorname{alr}(x)_j = \log \left( \frac{x_j}{x_H} \right) \quad \forall j=1,\dots,H-1 }
//'
//' @param x Vector of double in the simplex \mjseqn{S^H}.
//' @return Vector of double in \mjseqn{\mathbb{R}^{H-1}} i.e. \mjseqn{\operatorname{alr}(x)}.
//' @export
// [[Rcpp::export]]
Eigen::VectorXd Alr(Eigen::VectorXd x) {
	return utils::Alr(x, false);
}

/*
&=& \frac{ \operatorname{e}^{x_j} }{ 1 + \sum_{h=1}^{H-1} \operatorname{e}^{x_h} }
*/

//' Inverse Additive Log Ratio
//'
//' \loadmathjax This utility computes the inverse additive log-ratio transform of a given vector. Given a generic vector
//' \mjseqn{ x \in \mathbb{R}^{H-1} }, the transformation is defined as:
//' \mjsdeqn{ \begin{eqnarray*}
//'	\operatorname{alr}^{-1}(x)_{j} &=& \textstyle\frac{\operatorname{e}^{x_j}}{\sum _{h} \operatorname{e}^{x_h}} \quad \forall j=1,\dots,H-1 \cr
//' \operatorname{alr}^{-1}(x)_H &=& 1 - \textstyle\sum _{h} \operatorname{alr}^{-1}(x)_h
//' 		 \end{eqnarray*} }
//'
//' @param x Vector of double in \mjseqn{ \mathbb{R}^{H-1} }.
//' @return Vector of double in the simplex \mjseqn{S^H} i.e. \mjseqn{\operatorname{alr}^{-1}(x)}.
//' @export
// [[Rcpp::export]]
Eigen::VectorXd InvAlr(Eigen::VectorXd x) {
  return utils::InvAlr(x, false);
}

/* Spatial Sampler execution routine (no RJMCMC, w/ or w/o covariates,data,W and params as strings or proper data types from R)*/
// [[Rcpp::export]]
std::vector<Rcpp::RawVector> runSpatialSampler(int burnin, int niter, int thin, const std::vector<std::vector<double>> & data,
    										   const Eigen::MatrixXd & W, std::string params_str,
    										   const std::vector<Eigen::MatrixXd> & covariates, bool display_progress) {

	// Messages Parsing
	SamplerParams params; google::protobuf::TextFormat::ParseFromString(params_str, &params);

    // Initializarion
    SpatialMixtureSampler spSampler(params, data, W, covariates);
    spSampler.init();

    // Initialize output container
    std::vector<Rcpp::RawVector> out;

    // Sampling
    auto start = std::chrono::high_resolution_clock::now();

    if (burnin > 0) {
		REprintf("SPMIX Sampler: Burn-in\n");
		Progress p_burn(burnin, display_progress);
		for (int i=0; i < burnin; i++) {
			spSampler.sample();
			p_burn.increment();
		}
		p_burn.cleanup();
		Rcpp::Rcout << std::endl;
	}

	if (niter > 0) {
		REprintf("SPMIX Sampler: Running\n");
		Progress p_run(niter, display_progress);
		for (int i=0; i < niter; i++) {
			spSampler.sample();
			if ((i+1) % thin == 0) {
				std::string s;
				spSampler.getStateAsProto().SerializeToString(&s);
				out.push_back(utils::str2raw(s));
			}
			p_run.increment();
		}
		Rcpp::Rcout << std::endl;
	}

    auto end = std::chrono::high_resolution_clock::now();

    double duration = std::chrono::duration<double>(end - start).count();
    Rcpp::Rcout << "Duration: " << duration << std::endl;
    return out;
}

/* Spatial Sampler execution routine (RJMCMC, w/ or w/o covariates,data,W and params as strings or proper data types from R)*/
// [[Rcpp::export]]
std::vector<Rcpp::RawVector> runSpatialRJSampler(int burnin, int niter, int thin, const std::vector<std::vector<double>> & data,
    											 const Eigen::MatrixXd & W, std::string params_str,
    											 const std::vector<Eigen::MatrixXd> & covariates,
    											 const std::string & options_str, bool boundary_detection, bool display_progress) {

	// Messages Parsing
	SamplerParams params; google::protobuf::TextFormat::ParseFromString(params_str, &params);
	OptimOptions options; google::protobuf::TextFormat::ParseFromString(options_str, &options);

	// Initializarion
	SpatialMixtureRJSampler spSampler(params, data, W, options, covariates, boundary_detection);
	spSampler.init();

	// Initialize output container
	std::vector<Rcpp::RawVector> out;

    // Sampling
    auto start = std::chrono::high_resolution_clock::now();

    if (burnin > 0) {
		REprintf("SPMIX RJ Sampler: Burn-in\n");
		Progress p_burn(burnin, display_progress);
		for (int i=0; i < burnin; i++) {
			spSampler.sample();
			p_burn.increment();
		}
		p_burn.cleanup();
		Rcpp::Rcout << std::endl;
	}

	if (niter > 0) {
		REprintf("SPMIX RJ Sampler: Running\n");
		Progress p_run(niter, display_progress);
		for (int i=0; i < niter; i++) {
			spSampler.sample();
			if ((i+1) % thin == 0) {
				std::string s;
				spSampler.getStateAsProto().SerializeToString(&s);
				out.push_back(utils::str2raw(s));
			}
			p_run.increment();
		}
		Rcpp::Rcout << std::endl;
	}

    auto end = std::chrono::high_resolution_clock::now();

	double duration = std::chrono::duration<double>(end - start).count();
	Rcpp::Rcout << "Duration: " << duration << std::endl;
	return out;
}

//' Import Proximity Matrix from File
//'
//' \loadmathjax This function simply reads the proximity matrix \mjseqn{G} of the Spatial Mixture Model from a \code{.csv} file.
//' This file should not have columns or row headers and it must be written in the visual form of a matrix
//' only composed by either \mjseqn{0} or \mjseqn{1}. As assumption, the diagonal of this matrix should be \mjseqn{0}.
//' @param filename A string identifying the path to a \code{.csv} file from which the matrix will be read.
//' @return The proximity matrix as a usual \code{R matrix} object.
//' @export
// [[Rcpp::export]]
Eigen::MatrixXd ReadMatrixFromCSV(std::string filename) {
    return utils::readMatrixFromCSV(filename);
}

//' Import Data from File
//'
//' \loadmathjax This utility reads the input data for the sampler from a \code{.csv} file. This file needs to be provided
//' with column headers. Moreover, it should be organized in two columns for the correct parsing:
//' \itemize{
//' \item{\strong{group}, an integer (from \mjseqn{0} to \mjseqn{I-1}) describing the area to which the data belongs to;}
//' \item{\strong{data}, the actual data, which will be assigned to area indicated by the "group" column.}
//' }
//' @return A list of dimension \mjseqn{I}, in which the \mjseqn{i}-th element is a vector containing all data that
//' have been assigned to the \mjseqn{i}-th location.
//' @export
// [[Rcpp::export]]
std::vector<std::vector<double>> ReadDataFromCSV(std::string filename) {
    return utils::readDataFromCSV(filename);
}

#endif // SPMIX_EXPORTS


// Copyright (C) 2017-2018 Vicente J. Botet Escriba
//
//  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)
// Based on https://github.com/akrzemi1/explicit/blob/master/test/test_explicit.cpp

// <experimental/numerics/v1/numbers/double_wide_types.hpp>

//#define JASEL_CONFIG_CONTRACT_LEVEL_MASK 0x111
#define JASEL_CONFIG_CONTRACT_VIOLATION_THROWS_V 1

#include <iostream>
#include <experimental/numerics/v1/numbers/double_wide_types.hpp>
#include <string>

#include <boost/detail/lightweight_test.hpp>

namespace stdex = std::experimental;
namespace nmx = std::experimental::numerics;

static_assert(std::is_same<decltype(nmx::to_signed_cast(1)), int>::value, "error");
static_assert(std::is_same<decltype(nmx::to_signed_cast(1u)), int>::value, "error");
static_assert(std::is_same<decltype(nmx::to_unsigned_cast(1)), unsigned int>::value, "error");
static_assert(std::is_same<decltype(nmx::to_unsigned_cast(1u)), unsigned int>::value, "error");

static_assert(std::is_same<decltype(nmx::to_double_width(static_cast<std::int8_t>(1))), std::int16_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_double_width(static_cast<std::uint8_t>(1))), std::uint16_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_double_width(static_cast<std::int16_t>(1))), std::int32_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_double_width(static_cast<std::uint16_t>(1))), std::uint32_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_double_width(static_cast<std::int32_t>(1))), std::int64_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_double_width(static_cast<std::uint32_t>(1))), std::uint64_t>::value, "error");

static_assert(std::is_same<decltype(nmx::to_half_width(static_cast<std::int16_t>(1))), std::int8_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_half_width(static_cast<std::uint16_t>(1))), std::uint8_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_half_width(static_cast<std::int32_t>(1))), std::int16_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_half_width(static_cast<std::uint32_t>(1))), std::uint16_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_half_width(static_cast<std::int64_t>(1))), std::int32_t>::value, "error");
static_assert(std::is_same<decltype(nmx::to_half_width(static_cast<std::uint64_t>(1))), std::uint32_t>::value, "error");

#if __cplusplus >= 201402L

static_assert(nmx::to_unsigned_cast(1)==1u, "error");
static_assert(nmx::to_signed_cast(1)==1, "error");

#endif

int main()
{
  //BOOST_TEST(  ! nmx::in_domain<unsigned int>(-1) );

  {
      JASEL_CXX14_CONSTEXPR auto r = nmx::to_signed_cast(1);
      (void)r;
  }

  BOOST_TEST_EQ(nmx::to_signed_cast(1), 1);
  BOOST_TEST_EQ(nmx::to_signed_cast(1u), 1);

  {
      JASEL_CXX14_CONSTEXPR auto r = nmx::to_unsigned_cast(1);
      (void)r;
  }

  BOOST_TEST_EQ(nmx::to_unsigned_cast(1), 1u);
  BOOST_TEST_EQ(nmx::to_unsigned_cast(1u), 1u);

  {
      JASEL_CXX14_CONSTEXPR auto r = nmx::to_double_width(static_cast<std::int32_t>(1));
      BOOST_TEST_EQ(r, static_cast<std::int64_t>(1));
  }

  BOOST_TEST_EQ(nmx::to_double_width(static_cast<std::int32_t>(1)), static_cast<std::int64_t>(1));
  BOOST_TEST_EQ(nmx::to_double_width(static_cast<std::uint32_t>(1u)), static_cast<std::uint64_t>(1u));

  {
      JASEL_CXX14_CONSTEXPR auto r = nmx::to_half_width(static_cast<std::int32_t>(1));
      BOOST_TEST_EQ(r, static_cast<std::int16_t>(1));
  }
  BOOST_TEST_EQ(nmx::to_half_width(static_cast<std::int32_t>(1)), static_cast<std::int16_t>(1));
  BOOST_TEST_EQ(nmx::to_half_width(static_cast<std::uint32_t>(1u)), static_cast<std::uint16_t>(1u));

  {
      JASEL_CXX14_CONSTEXPR auto r = nmx::split_upper(0x00010002);
      BOOST_TEST_EQ(r, static_cast<unsigned short>(0x0001));
  }
  BOOST_TEST_EQ(nmx::split_upper(0x00010002), static_cast<unsigned short>(0x0001));
  BOOST_TEST_EQ(nmx::split_lower(0x00010002), static_cast<unsigned short>(0x0002));

  BOOST_TEST_EQ(nmx::split_upper(-1), static_cast<unsigned short>(0xffff));
  BOOST_TEST_EQ(nmx::split_lower(-1), static_cast<unsigned short>(0xffff));

  {
      JASEL_CXX14_CONSTEXPR auto r = nmx::wide_signed(static_cast<unsigned short>(0xffff), static_cast<unsigned short>(0xffff));
      BOOST_TEST_EQ(r, -1);
  }
  BOOST_TEST_EQ(nmx::wide_signed(static_cast<unsigned short>(0xffff), static_cast<unsigned short>(0xffff)), -1);
  {
      JASEL_CXX14_CONSTEXPR auto r = nmx::wide_unsigned(static_cast<unsigned short>(0xffff), static_cast<unsigned short>(0xffff));
      BOOST_TEST_EQ(r, static_cast<unsigned>(-1));
  }
  BOOST_TEST_EQ(nmx::wide_unsigned(static_cast<unsigned short>(0xffff), static_cast<unsigned short>(0xffff)), static_cast<unsigned>(-1));

  return ::boost::report_errors();
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/51-100/78/problem78.hpp"

BOOST_AUTO_TEST_SUITE( Problem78 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem78::solve(7);
        BOOST_CHECK_EQUAL(res, 5);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem78::solve();
        BOOST_CHECK_EQUAL(res, 55374);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <anie/details/kernels.hpp>

#include <boost/compute.hpp>

namespace anie::details
{
	const std::string kernel_matrix_add = BOOST_COMPUTE_STRINGIZE_SOURCE(
		__kernel void matrix_add(__global double* lhs, __global const double* rhs,
								 const uint width)
		{
			const uint y = get_global_id(0);

			for (uint i = 0; i < width; ++i)
			{
				lhs[width * y + i] += rhs[width * y + i];
			}
		}
	);
	const std::string kernel_matrix_sub = BOOST_COMPUTE_STRINGIZE_SOURCE(
		__kernel void matrix_sub(__global double* lhs, __global const double* rhs,
								 const uint width)
		{
			const uint y = get_global_id(0);

			for (uint i = 0; i < width; ++i)
			{
				lhs[width * y + i] -= rhs[width * y + i];
			}
		}
	);
	const std::string kernel_matrix_multiply = BOOST_COMPUTE_STRINGIZE_SOURCE(
		__kernel void matrix_multiply(__global double* dest, __global const double* src_lhs, __global const double* src_rhs,
									  const uint src_lhs_width, const uint src_rhs_width)
		{
			const uint width = get_global_size(0);
			const uint x = get_global_id(0);
			const uint y = get_global_id(1);
			const uint index = y * width + x;

			double sum = 0.;

			for (uint i = 0; i < src_lhs_width; ++i)
			{
				sum += src_lhs[src_lhs_width * y + i] * src_rhs[src_rhs_width * i + x];
			}

			dest[index] = sum;
		}
	);
	const std::string kernel_matrix_transpose = BOOST_COMPUTE_STRINGIZE_SOURCE(
		__kernel void matrix_transpose(__global double* dest, __global const double* src,
								 const uint src_width, const uint src_height)
		{
			const uint y = get_global_id(0);

			for (uint i = 0; i < src_width; ++i)
			{
				dest[i * src_height + y] = src[y * src_width + i];
			}
		}
	);
}


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

 * 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  Pose3.cpp
 * @brief 3D Pose
 */

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Lie-inl.h>
#include <boost/foreach.hpp>
#include <iostream>
#include <cmath>

using namespace std;

namespace gtsam {

  /** Explicit instantiation of base class to export members */
  INSTANTIATE_LIE(Pose3);

  /** instantiate concept checks */
  GTSAM_CONCEPT_POSE_INST(Pose3);

  static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3;
  static const Matrix6 I6 = eye(6);

  /* ************************************************************************* */
  Pose3::Pose3(const Pose2& pose2) :
      R_(Rot3::rodriguez(0, 0, pose2.theta())),
      t_(Point3(pose2.x(), pose2.y(), 0)) {
  }

  /* ************************************************************************* */
  // Calculate Adjoint map
  // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
  // Experimental - unit tests of derivatives based on it do not check out yet
  Matrix6 Pose3::AdjointMap() const {
    const Matrix3 R = R_.matrix();
    const Vector3 t = t_.vector();
    Matrix3 A = skewSymmetric(t)*R;
    Matrix6 adj;
    adj << R, Z3, A, R;
    return adj;
  }

  /* ************************************************************************* */
  Matrix6 Pose3::adjointMap(const Vector& xi) {
    Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
    Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
    Matrix6 adj;
    adj << w_hat, Z3, v_hat, w_hat;

    return adj;
  }

  /* ************************************************************************* */
  Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
    if (H) {
      *H = zeros(6,6);
      for (int i = 0; i<6; ++i) {
        Vector dxi = zero(6); dxi(i) = 1.0;
        Matrix Gi = adjointMap(dxi);
        (*H).col(i) = Gi*y;
      }
    }
    return adjointMap(xi)*y;
  }

  /* ************************************************************************* */
  Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
    if (H) {
      *H = zeros(6,6);
      for (int i = 0; i<6; ++i) {
        Vector dxi = zero(6); dxi(i) = 1.0;
        Matrix GTi = adjointMap(dxi).transpose();
        (*H).col(i) = GTi*y;
      }
    }
    Matrix adjT = adjointMap(xi).transpose();
    return adjointMap(xi).transpose() * y;
  }

  /* ************************************************************************* */
  Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
    // Bernoulli numbers, from Wikipedia
    static const Vector B = Vector_(9, 1.0, -1.0/2.0, 1./6., 0.0, -1.0/30.0, 0.0, 1.0/42.0, 0.0, -1.0/30);
    static const int N = 5; // order of approximation
    Matrix res = I6;
    Matrix6 ad_i = I6;
    Matrix6 ad_xi = adjointMap(xi);
    double fac = 1.0;
    for (int i = 1 ; i<N; ++i) {
      ad_i = ad_xi * ad_i;
      fac = fac*i;
      res = res + B(i)/fac*ad_i;
    }
    return res;
  }

  /* ************************************************************************* */
  void Pose3::print(const string& s) const {
    cout << s;
    R_.print("R:\n");
    t_.print("t: ");
  }

  /* ************************************************************************* */
  bool Pose3::equals(const Pose3& pose, double tol) const {
    return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
  }

  /* ************************************************************************* */
  /** Modified from Murray94book version (which assumes w and v normalized?) */
  Pose3 Pose3::Expmap(const Vector& xi) {

    // get angular velocity omega and translational velocity v from twist xi
    Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5));

    double theta = w.norm();
    if (theta < 1e-10) {
      static const Rot3 I;
      return Pose3(I, v);
    }
    else {
      Point3 n(w/theta); // axis unit vector
      Rot3 R = Rot3::rodriguez(n.vector(),theta);
      double vn = n.dot(v); // translation parallel to n
      Point3 n_cross_v = n.cross(v); // points towards axis
      Point3 t = (n_cross_v - R*n_cross_v)/theta + vn*n;
      return Pose3(R, t);
    }
  }

  /* ************************************************************************* */
  Vector6 Pose3::Logmap(const Pose3& p) {
    Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
    double t = w.norm();
    if (t < 1e-10) {
      Vector6 log;
      log << w, T;
      return log;
    }
    else {
      Matrix3 W = skewSymmetric(w/t);
      // Formula from Agrawal06iros, equation (14)
      // simplified with Mathematica, and multiplying in T to avoid matrix math
      double Tan = tan(0.5*t);
      Vector3 WT = W*T;
      Vector3 u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT);
      Vector6 log;
      log << w, u;
      return log;
    }
  }

  /* ************************************************************************* */
  Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
      Vector3 omega(sub(xi, 0, 3));
      Point3 v(sub(xi, 3, 6));
      Rot3 R = R_.retract(omega);  // R is done exactly
      Point3 t = t_ + R_ * v; // First order t approximation
      return Pose3(R, t);
  }

  /* ************************************************************************* */
  // Different versions of retract
  Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
    if(mode == Pose3::EXPMAP) {
      // Lie group exponential map, traces out geodesic
      return compose(Expmap(xi));
    } else if(mode == Pose3::FIRST_ORDER) {
      // First order
      return retractFirstOrder(xi);
    } else {
      // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
      // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
      assert(false);
      exit(1);
    }
  }

  /* ************************************************************************* */
  // different versions of localCoordinates
  Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const {
    if(mode == Pose3::EXPMAP) {
      // Lie group logarithm map, exact inverse of exponential map
      return Logmap(between(T));
    } else if(mode == Pose3::FIRST_ORDER) {
      // R is always done exactly in all three retract versions below
      Vector3 omega = R_.localCoordinates(T.rotation());

      // Incorrect version
      // Independently computes the logmap of the translation and rotation
      // Vector v = t_.localCoordinates(T.translation());

      // Correct first order t inverse
      Point3 d = R_.unrotate(T.translation() - t_);

      // TODO: correct second order t inverse
      Vector6 local;
      local << omega(0),omega(1),omega(2),d.x(),d.y(),d.z();
      return local;
    } else {
      assert(false);
      exit(1);
    }
  }

  /* ************************************************************************* */
  Matrix4 Pose3::matrix() const {
    const Matrix3 R = R_.matrix();
    const Vector3 T = t_.vector();
    Eigen::Matrix<double,1,4> A14;
    A14 << 0.0, 0.0, 0.0, 1.0;
    Matrix4 mat;
    mat << R, T, A14;
    return mat;
  }

  /* ************************************************************************* */
  Pose3 Pose3::transform_to(const Pose3& pose) const {
    Rot3 cRv = R_ * Rot3(pose.R_.inverse());
    Point3 t = pose.transform_to(t_);
    return Pose3(cRv, t);
  }

  /* ************************************************************************* */
  Point3 Pose3::transform_from(const Point3& p,
      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) {
      const Matrix R = R_.matrix();
      Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
      H1->resize(3,6);
      (*H1) << DR, R;
    }
    if (H2) *H2 = R_.matrix();
    return R_ * p + t_;
  }

  /* ************************************************************************* */
  Point3 Pose3::transform_to(const Point3& p,
            boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    const Point3 result = R_.unrotate(p - t_);
    if (H1) {
      const Point3& q = result;
      Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
      H1->resize(3,6);
      (*H1) << DR, _I3;
    }
    if (H2) *H2 = R_.transpose();
    return result;
  }

  /* ************************************************************************* */
  Pose3 Pose3::compose(const Pose3& p2,
        boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = p2.inverse().AdjointMap();
    if (H2) *H2 = I6;
    return (*this) * p2;
  }

  /* ************************************************************************* */
  Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
    if (H1) *H1 = -AdjointMap();
    Rot3 Rt = R_.inverse();
    return Pose3(Rt, Rt*(-t_));
  }

  /* ************************************************************************* */
  // between = compose(p2,inverse(p1));
  Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
      boost::optional<Matrix&> H2) const {
    Pose3 result = inverse()*p2;
    if (H1) *H1 = -result.inverse().AdjointMap();
    if (H2) *H2 = I6;
    return result;
  }

  /* ************************************************************************* */
  double Pose3::range(const Point3& point,
      boost::optional<Matrix&> H1,
      boost::optional<Matrix&> H2) const {
    if (!H1 && !H2) return transform_to(point).norm();
    Point3 d = transform_to(point, H1, H2);
    double x = d.x(), y = d.y(), z = d.z(),
       d2 = x * x + y * y + z * z, n = sqrt(d2);
    Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n);
    if (H1) *H1 = D_result_d * (*H1);
    if (H2) *H2 = D_result_d * (*H2);
    return n;
  }

  /* ************************************************************************* */
  double Pose3::range(const Pose3& point,
        boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
     double r = range(point.translation(), H1, H2);
     if (H2) {
       Matrix H2_ = *H2 * point.rotation().matrix();
       *H2 = zeros(1, 6);
       insertSub(*H2, H2_, 0, 3);
     }
     return r;
  }

  /* ************************************************************************* */
  boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
    const size_t n = pairs.size();
    if (n<3) return boost::none; // we need at least three pairs

    // calculate centroids
    Vector cp = zero(3),cq = zero(3);
    BOOST_FOREACH(const Point3Pair& pair, pairs) {
      cp += pair.first.vector();
      cq += pair.second.vector();
    }
    double f = 1.0/n;
    cp *= f; cq *= f;

    // Add to form H matrix
    Matrix H = zeros(3,3);
    BOOST_FOREACH(const Point3Pair& pair, pairs) {
      Vector dp = pair.first.vector()  - cp;
      Vector dq = pair.second.vector() - cq;
      H += dp * dq.transpose();
    }

    // Compute SVD
    Matrix U,V;
    Vector S;
    svd(H,U,S,V);

    // Recover transform with correction from Eggert97machinevisionandapplications
    Matrix UVtranspose = U * V.transpose();
    Matrix detWeighting = eye(3,3);
    detWeighting(2,2) = UVtranspose.determinant();
    Rot3 R(Matrix(V * detWeighting * U.transpose()));
    Point3 t = Point3(cq) - R * Point3(cp);
    return Pose3(R, t);
  }

  /* ************************************************************************* */
  std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
    os << pose.rotation() << "\n" << pose.translation() << endl;
    return os;
  }

} // namespace gtsam


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <fstream>
#include <iostream>
#include <string>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>

using namespace boost;

namespace std
{
  template < typename T >
    std::istream & operator >> (std::istream & in, std::pair < T, T > &p)
  {
    in >> p.first >> p.second;
    return in;
  }
}

typedef adjacency_list < listS, // Store out-edges of each vertex in a std::list
  vecS,                         // Store vertex set in a std::vector
  directedS                     // The file dependency graph is directed
> file_dep_graph;

typedef graph_traits < file_dep_graph >::vertex_descriptor vertex_t;
typedef graph_traits < file_dep_graph >::edge_descriptor edge_t;

void
topo_sort_dfs(const file_dep_graph & g, vertex_t u, vertex_t * &topo_order,
              int *mark)
{
  mark[u] = 1;                  // 1 means visited, 0 means not yet visited
  graph_traits < file_dep_graph >::adjacency_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = adjacent_vertices(u, g); vi != vi_end; ++vi)
    if (mark[*vi] == 0)
      topo_sort_dfs(g, *vi, topo_order, mark);

  *--topo_order = u;
}

void
topo_sort(const file_dep_graph & g, vertex_t * topo_order)
{
  std::vector < int >mark(num_vertices(g), 0);
  graph_traits < file_dep_graph >::vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    if (mark[*vi] == 0)
      topo_sort_dfs(g, *vi, topo_order, &mark[0]);
}

int
main()
{
  std::ifstream file_in("makefile-dependencies.dat");
  typedef graph_traits < file_dep_graph >::vertices_size_type size_type;
  size_type n_vertices;
  file_in >> n_vertices;        // read in number of vertices
  std::istream_iterator < std::pair < size_type, size_type > >
    input_begin(file_in), input_end;
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ can't handle the iterator constructor
  file_dep_graph g(n_vertices);
  while (input_begin != input_end) {
    size_type i, j;
    boost::tie(i, j) = *input_begin++;
    add_edge(i, j, g);
  }
#else
  file_dep_graph g(input_begin, input_end, n_vertices);
#endif

  std::vector < std::string > name(num_vertices(g));
  std::ifstream name_in("makefile-target-names.dat");
  graph_traits < file_dep_graph >::vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    name_in >> name[*vi];

  std::vector < vertex_t > order(num_vertices(g));
  topo_sort(g, &order[0] + num_vertices(g));
  for (size_type i = 0; i < num_vertices(g); ++i)
    std::cout << name[order[i]] << std::endl;

  return 0;
}