/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/

#include <SofaConstraint/LMConstraintDirectSolver.h>
#include <sofa/core/visual/VisualParams.h>
#include <SofaConstraint/ContactDescription.h>
#include <sofa/core/ObjectFactory.h>
#include <Eigen/QR>
#include <Eigen/SVD>

namespace sofa
{

namespace component
{

namespace constraintset
{

LMConstraintDirectSolver::LMConstraintDirectSolver()
    : solverAlgorithm(initData(&solverAlgorithm, "solverAlgorithm", "Algorithm used to solve the system W.Lambda=c"))
{
    //Add here other algo
    sofa::helper::OptionsGroup algo(1,"SVD");
    solverAlgorithm.setValue(algo);
}

bool LMConstraintDirectSolver::buildSystem(const core::ConstraintParams* cParams, MultiVecId res1, MultiVecId res2)
{
    bool sucess = LMConstraintSolver::buildSystem(cParams, res1, res2);

    return sucess;
}

bool LMConstraintDirectSolver::solveSystem(const core::ConstraintParams* cParams, MultiVecId res1, MultiVecId res2)
{
    //First, do n iterations of Gauss Seidel
    bool success = LMConstraintSolver::solveSystem(cParams, res1, res2);

    if (cParams->constOrder() != core::ConstraintParams::VEL) return success;

    //Then process to a direct solution of the system

    //We need to find all the constraint related to contact
    // 1. extract the information about the state of the contact and build the new L, L^T matrices
    // 2. build the full system
    // 3. solve

    //------------------------------------------------------------------
    // extract the information about the state of the contact
    //------------------------------------------------------------------

    //************************************************************
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("AnalyseConstraints");
#endif
    const helper::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints();

    JacobianRows rowsL ; rowsL.reserve(numConstraint);
    JacobianRows rowsLT; rowsLT.reserve(numConstraint);
    helper::vector< unsigned int > rightHandElements;

    analyseConstraints(LMConstraints, cParams->constOrder(),
            rowsL, rowsLT, rightHandElements);

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("AnalyseConstraints");
#endif
    if (rowsL.empty() || rowsLT.empty()) return success;

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("BuildFullSystem");
#endif
    //------------------------------------------------------------------
    // build c: right hand term
    //------------------------------------------------------------------
    VectorEigen previousC(c);
    //TODO: change newC by c
    c=VectorEigen::Zero(rowsL.size());
    unsigned int idx=0;
    for (helper::vector<unsigned int >::const_iterator it=rightHandElements.begin(); it!=rightHandElements.end(); ++it)
        c[idx++]=previousC[*it];

    //------------------------------------------------------------------
    // build the L and LT matrices
    //------------------------------------------------------------------

    DofToMatrix LMatricesDirectSolver;
    DofToMatrix LTMatricesDirectSolver;
    for (DofToMatrix::iterator it=LMatrices.begin(); it!=LMatrices.end(); ++it)
    {
        //------------------------------------------------------------------
        const SparseMatrixEigen& matrix= it->second;
        //Init the manipulator with the full matrix
        linearsolver::LMatrixManipulator manip;
        manip.init(matrix);

        //------------------------------------------------------------------
        SparseMatrixEigen  L (rowsL.size(),  matrix.cols());
        L.reserve(rowsL.size()*matrix.cols());
        manip.buildLMatrix(rowsL ,L);
        L.finalize();
        LMatricesDirectSolver.insert (std::make_pair(it->first,L ));

        //------------------------------------------------------------------
        SparseMatrixEigen  LT(rowsLT.size(), matrix.cols());
        LT.reserve(rowsLT.size()*matrix.cols());
        manip.buildLMatrix(rowsLT,LT);
        LT.finalize();
        LTMatricesDirectSolver.insert(std::make_pair(it->first,LT));
    }

    //------------------------------------------------------------------
    // build the full system
    //------------------------------------------------------------------
    const  int rows=rowsL.size();
    const  int cols=rowsLT.size();
    SparseColMajorMatrixEigen Wsparse(rows,cols);
    buildLeftRectangularMatrix(invMassMatrix, LMatricesDirectSolver, LTMatricesDirectSolver, Wsparse,invMass_Ltrans);

    //------------------------------------------------------------------
    // conversion from sparse to dense matrix
    //------------------------------------------------------------------
    Lambda=VectorEigen::Zero(rows);

    W=MatrixEigen::Zero(rows,cols);

    SparseMatrixEigen Wresult(Wsparse);
    for (int k=0; k<Wresult.outerSize(); ++k)
        for (SparseMatrixEigen::InnerIterator it(Wresult,k); it; ++it) W(it.row(),it.col()) = it.value();

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("BuildFullSystem");
#endif

    //------------------------------------------------------------------
    // Solve the system
    //------------------------------------------------------------------
    const std::string &algo=solverAlgorithm.getValue().getSelectedItem() ;
#ifdef SOFA_DUMP_VISITOR_INFO
    simulation::Visitor::TRACE_ARGUMENT arg1;
    arg1.push_back(std::make_pair("Algorithm", algo));
    arg1.push_back(std::make_pair("Dimension", printDimension(W)));
    sofa::simulation::Visitor::printNode("DirectSolveSystem", "",arg1);
#endif
    if(algo == "SVD")
    {
        Eigen::JacobiSVD< MatrixEigen > solverSVD(W);
        VectorEigen invSingularValues(solverSVD.singularValues());

        for (int i=0; i<invSingularValues.size(); ++i)
        {
            if (invSingularValues[i] < 1e-10) invSingularValues[i]=0;
            else invSingularValues[i]=1/invSingularValues[i];
        }
        Lambda.noalias() = solverSVD.matrixV()*invSingularValues.asDiagonal()*solverSVD.matrixU().transpose()*c;
    }

    if (this->f_printLog.getValue())
    {
        sout << "W" <<  printDimension(W) <<  "  Lambda" << printDimension(Lambda) << "  c" << printDimension(c) << sendl;
        sout << "\nW     ===============================================\n" << W
                <<  "\nLambda===============================================\n" << Lambda
                <<  "\nc     ===============================================\n" << c << sendl;
    }

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("DirectSolveSystem");
#endif
    return success;

}

void LMConstraintDirectSolver::analyseConstraints(const helper::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints, core::ConstraintParams::ConstOrder order,
        JacobianRows &rowsL,JacobianRows &rowsLT, helper::vector< unsigned int > &rightHandElements) const
{
    //Iterate among all the Sofa LMConstraint
    for (unsigned int componentConstraint=0; componentConstraint<LMConstraints.size(); ++componentConstraint)
    {
        sofa::core::behavior::BaseLMConstraint *constraint=LMConstraints[componentConstraint];
        //Find the constraint dealing with contact
        if (ContactDescriptionHandler* contactDescriptor=dynamic_cast<ContactDescriptionHandler*>(constraint))
        {
            const helper::vector< sofa::core::behavior::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order);
            //Iterate among all the contacts
            for (helper::vector< sofa::core::behavior::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup)
            {
                const sofa::core::behavior::ConstraintGroup* group=*itGroup;
                const sofa::component::constraintset::ContactDescription& contact=contactDescriptor->getContactDescription(group);

                const unsigned int idxEquation=group->getConstraint(0).idx;

                switch(contact.state)
                {
                case VANISHING:
                {
                    //                    serr <<"Constraint " << idxEquation << " VANISHING" << sendl;
                    //0 equation
                    break;
                }
                case STICKING:
                {
                    //                    serr << "Constraint " <<idxEquation << " STICKING" << sendl;
                    const unsigned int i=rowsL.size();
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation  ));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+1));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+2));

                    //3 equations
                    rowsLT.push_back(rowsL[i  ]);
                    rowsLT.push_back(rowsL[i+1]);
                    rowsLT.push_back(rowsL[i+2]);

                    rightHandElements.push_back(idxEquation  );
                    rightHandElements.push_back(idxEquation+1);
                    rightHandElements.push_back(idxEquation+2);
                    break;
                }
                case SLIDING:
                {
                    //                    serr << "Constraint " <<idxEquation << " SLIDING" << sendl;
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation  ));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+1));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+2));

                    //1 equation with the response force along the Coulomb friction cone
                    rowsLT.push_back(linearsolver::LLineManipulator()
                            .addCombination(idxEquation  ,contact.coeff[0])
                            .addCombination(idxEquation+1,contact.coeff[1])
                            .addCombination(idxEquation+2,contact.coeff[2]));

                    rightHandElements.push_back(idxEquation  );
                    rightHandElements.push_back(idxEquation+1);
                    rightHandElements.push_back(idxEquation+2);
                    break;
                }
                }
            }
        }
        else
        {
            //Non contact constraints: we add all the equations
            const helper::vector< sofa::core::behavior::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order);
            for (helper::vector< sofa::core::behavior::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup)
            {
                const sofa::core::behavior::ConstraintGroup* group=*itGroup;
                std::pair< sofa::core::behavior::ConstraintGroup::EquationConstIterator,sofa::core::behavior::ConstraintGroup::EquationConstIterator> range=group->data();
                for ( sofa::core::behavior::ConstraintGroup::EquationConstIterator it=range.first; it!=range.second; ++it)
                {
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(it->idx));
                    rowsLT.push_back(rowsL.back());
                    rightHandElements.push_back(it->idx);
                }
            }
        }
    }
}

void LMConstraintDirectSolver::buildLeftRectangularMatrix(const DofToMatrix& invMassMatrix,
        DofToMatrix& LMatrix, DofToMatrix& LTMatrix,
        SparseColMajorMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const
{
    invMass_Ltrans.clear();
    for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs)
    {
        const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs;
        const SparseMatrixEigen &invMass=invMassMatrix.find(dofs)->second;
        const SparseMatrixEigen &L =LMatrix[dofs];
        const SparseMatrixEigen &LT=LTMatrix[dofs];

        SparseMatrixEigen invMass_LT=invMass*LT.transpose();

        invMass_Ltrans.insert(std::make_pair(dofs, invMass_LT));
        //SparseColMajorMatrixEigen temp=L*invMass_LT;
        LeftMatrix += L*invMass_LT;
    }
}
int LMConstraintDirectSolverClass = core::RegisterObject("A Direct Constraint Solver working specifically with LMConstraint based components")
        .add< LMConstraintDirectSolver >();

SOFA_DECL_CLASS(LMConstraintDirectSolver);

} // namespace constraintset

} // namespace component

} // namespace sofa


// Boost.Geometry

// Copyright (c) 2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_AUTHALIC_RADIUS_SQR_HPP
#define BOOST_GEOMETRY_FORMULAS_AUTHALIC_RADIUS_SQR_HPP

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/formulas/eccentricity_sqr.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/not_implemented.hpp>

#include <boost/math/special_functions/atanh.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace formula_dispatch
{

template <typename ResultType, typename Geometry, typename Tag = typename tag<Geometry>::type>
struct authalic_radius_sqr
    : not_implemented<Tag>
{};

template <typename ResultType, typename Geometry>
struct authalic_radius_sqr<ResultType, Geometry, srs_sphere_tag>
{
    static inline ResultType apply(Geometry const& geometry)
    {
        return math::sqr<ResultType>(get_radius<0>(geometry));
    }
};

template <typename ResultType, typename Geometry>
struct authalic_radius_sqr<ResultType, Geometry, srs_spheroid_tag>
{
    static inline ResultType apply(Geometry const& geometry)
    {
        ResultType const a2 = math::sqr<ResultType>(get_radius<0>(geometry));
        ResultType const e2 = formula::eccentricity_sqr<ResultType>(geometry);

        return apply(a2, e2);
    }

    static inline ResultType apply(ResultType const& a2, ResultType const& e2)
    {
        ResultType const c0 = 0;

        if (math::equals(e2, c0))
        {
            return a2;
        }

        ResultType const e = math::sqrt(e2);
        ResultType const c2 = 2;

        //ResultType const b2 = math::sqr(get_radius<2>(geometry));
        //return a2 / c2 + b2 * boost::math::atanh(e) / (c2 * e);

        ResultType const c1 = 1;
        return (a2 / c2) * ( c1 + (c1 - e2) * boost::math::atanh(e) / e );
    }
};

} // namespace formula_dispatch
#endif // DOXYGEN_NO_DISPATCH

#ifndef DOXYGEN_NO_DETAIL
namespace formula
{

template <typename ResultType, typename Geometry>
inline ResultType authalic_radius_sqr(Geometry const& geometry)
{
    return formula_dispatch::authalic_radius_sqr<ResultType, Geometry>::apply(geometry);
}

} // namespace formula
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_FORMULAS_AUTHALIC_RADIUS_SQR_HPP


#pragma once

#include <cstddef>
#include <random>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>

#include "nifty/graph/detail/andres/grid-graph.hxx"

#include "nifty/tools/runtime_check.hxx"
#include "nifty/tools/for_each_coordinate.hxx"
#include "nifty/graph/undirected_graph_base.hxx"
#include "nifty/graph/detail/adjacency.hxx"
#include "nifty/graph/graph_tags.hxx"
#include "nifty/parallel/threadpool.hxx"
#include "nifty/array/arithmetic_array.hxx"
#include "nifty/xtensor/xtensor.hxx"

namespace nifty{
namespace graph{

namespace detail_graph{

    template<std::size_t DIM, bool SIMPLE_NH>
    class UndirectedGridGraphIter{
    public:
        typedef andres::graph::GridGraph<DIM> AGridGraph;
        typedef typename AGridGraph::AdjacencyIterator AGridGraphAdjacencyIter;
        typedef UndirectedAdjacency<int64_t,int64_t,int64_t,int64_t> NodeAdjacency;

        struct UnaryFunction{
            typedef NodeAdjacency value_type;
            template<class ADJ>
            NodeAdjacency operator()(const ADJ & adjacency)const{
                return NodeAdjacency(adjacency.vertex(), adjacency.vertex());
            }
        };

        typedef boost::transform_iterator<
            UnaryFunction,
            typename AGridGraph::AdjacencyIterator,
            NodeAdjacency,
            NodeAdjacency
        > OldAdjacencyIter;

        class AdjacencyIter
        : public boost::iterator_facade<
            AdjacencyIter,
            NodeAdjacency,
            std::random_access_iterator_tag,
            const NodeAdjacency &
        >
        {
        public:
            AdjacencyIter(const AGridGraphAdjacencyIter & iter)
            :   iter_(iter),
                adjacency_(){
            }
            bool equal(const AdjacencyIter & other)const{
                return iter_ == other.iter_;
            }
            void increment(){
                ++iter_;
            }
            void dencrement(){
                --iter_;
            }
            void advance(const std::size_t n){
                iter_+=n;
            }
            std::ptrdiff_t distance_to(const AdjacencyIter & other)const{
                return std::distance(iter_, other.iter_);
            }
            const NodeAdjacency & dereference()const{
                adjacency_ = NodeAdjacency(iter_->vertex(), iter_->edge());
                return adjacency_;
            }
        private:
            mutable AGridGraphAdjacencyIter iter_;
            mutable NodeAdjacency adjacency_;
        };

        class NodeIter : public boost::counting_iterator<int64_t>{
            using boost::counting_iterator<int64_t>::counting_iterator;
            using boost::counting_iterator<int64_t>::operator=;
        };

        class EdgeIter : public boost::counting_iterator<int64_t>{
            using boost::counting_iterator<int64_t>::counting_iterator;
            using boost::counting_iterator<int64_t>::operator=;
        };
    };

    //
    // distance functions along channel dimension
    //

    template<unsigned DIM, class IMAGE>
    double l2Distance(
        const IMAGE & image,
        const unsigned nChannels,
        nifty::array::StaticArray<int64_t, DIM+1> coordU,
        nifty::array::StaticArray<int64_t, DIM+1> coordV
    ) {
        double val = 0;
        for(unsigned c = 0; c < nChannels; ++c) {
            coordU[0] = c;
            coordV[0] = c;
            const auto uVal = xtensor::read(image, coordU);
            const auto vVal = xtensor::read(image, coordV);
            val += (uVal - vVal) * (uVal - vVal);
        }
        return std::sqrt(val);
    }

    template<unsigned DIM, class IMAGE>
    double l1Distance(
        const IMAGE & image,
        const unsigned nChannels,
        nifty::array::StaticArray<int64_t, DIM+1> coordU,
        nifty::array::StaticArray<int64_t, DIM+1> coordV
    ) {
        double val = 0;
        for(unsigned c = 0; c < nChannels; ++c) {
            coordU[0] = c;
            coordV[0] = c;
            const auto uVal = xtensor::read(image, coordU);
            const auto vVal = xtensor::read(image, coordV);
            val += std::abs(uVal - vVal);
        }
        return val;
    }

    template<unsigned DIM, class IMAGE>
    double cosineDistance(
        const IMAGE & image,
        const unsigned nChannels,
        nifty::array::StaticArray<int64_t, DIM+1> coordU,
        nifty::array::StaticArray<int64_t, DIM+1> coordV
    ) {
        const double eps = 1e-7;

        double val = 0;
        double normU = 0;
        double normV = 0;

        for(unsigned c = 0; c < nChannels; ++c) {
            coordU[0] = c;
            coordV[0] = c;
            const auto uVal = xtensor::read(image, coordU);
            const auto vVal = xtensor::read(image, coordV);
            val += uVal * vVal;
            normU += uVal * uVal;
            normV += vVal * vVal;
        }
        normU = std::sqrt(normU) + eps;
        normV = std::sqrt(normV) + eps;
        val = 1. - (val / normU / normV);
        return val;
    }

};

template<std::size_t DIM, bool SIMPLE_NH>
class UndirectedGridGraph;

template<std::size_t DIM>
class UndirectedGridGraph<DIM,true> : public
    UndirectedGraphBase<
        UndirectedGridGraph<DIM, true>,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::NodeIter,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::EdgeIter,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::AdjacencyIter
    >
{
private:
    typedef andres::graph::GridGraph<DIM> AndresGridGraphType;
    typedef typename AndresGridGraphType::VertexCoordinate AndresVertexCoordinate;
public:
    typedef nifty::array::StaticArray<int64_t, DIM> ShapeType;
    typedef nifty::array::StaticArray<int64_t, DIM> CoordinateType;

    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::NodeIter      NodeIter;
    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::EdgeIter      EdgeIter;
    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::AdjacencyIter AdjacencyIter;

    typedef ContiguousTag EdgeIdTag;
    typedef ContiguousTag NodeIdTag;

    typedef SortedTag EdgeIdOrderTag;
    typedef SortedTag NodeIdOrderTag;

    UndirectedGridGraph()
    : gridGraph_(){
    }

    template<class T>
    UndirectedGridGraph(const nifty::array::StaticArray<T, DIM> & shape)
    : gridGraph_(){

        AndresVertexCoordinate ashape;
        std::copy(shape.rbegin(), shape.rend(), ashape.begin());
        gridGraph_.assign(ashape);

    }

    template<class T>
    void assign(const nifty::array::StaticArray<T, DIM> & shape){

        AndresVertexCoordinate ashape;
        std::copy(shape.rbegin(), shape.rend(), ashape.begin());
        gridGraph_.assign(ashape);

    }

    //void assign(const uint64_t numberOfNodes = 0, const uint64_t reserveNumberOfEdges = 0);

    // MUST IMPL INTERFACE
    int64_t u(const int64_t e)const{
        return gridGraph_.vertexOfEdge(e,0);
    }
    int64_t v(const int64_t e)const{
        return gridGraph_.vertexOfEdge(e,1);
    }

    int64_t findEdge(const int64_t u, const int64_t v)const{
        const auto r = gridGraph_.findEdge(u,v);
        if(r.first)
            return r.second;
        else
            return -1;
    }
    int64_t nodeIdUpperBound() const{
         return numberOfNodes() == 0 ? 0 : numberOfNodes()-1;
    }
    int64_t edgeIdUpperBound() const{
        return numberOfEdges() == 0 ? 0 : numberOfEdges()-1;
    }

    uint64_t numberOfEdges() const{
        return gridGraph_.numberOfEdges();
    }
    uint64_t numberOfNodes() const{
        return gridGraph_.numberOfVertices();
    }

    NodeIter nodesBegin()const{
        return NodeIter(0);
    }
    NodeIter nodesEnd()const{
        return NodeIter(this->numberOfNodes());
    }
    EdgeIter edgesBegin()const{
        return EdgeIter(0);
    }
    EdgeIter edgesEnd()const{
        return EdgeIter(this->numberOfEdges());
    }

    AdjacencyIter adjacencyBegin(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexBegin(node));
    }
    AdjacencyIter adjacencyEnd(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexEnd(node));
    }
    AdjacencyIter adjacencyOutBegin(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexBegin(node));
    }
     AdjacencyIter adjacencyOutEnd(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexEnd(node));
    }

    // optional (with default impl in base)
    //std::pair<int64_t,int64_t> uv(const int64_t e)const;

    template<class F>
    void forEachEdge(F && f)const{
        for(uint64_t edge=0; edge< numberOfEdges(); ++edge){
            f(edge);
        }
    }

    template<class F>
    void forEachNode(F && f)const{
        for(uint64_t node=0; node< numberOfNodes(); ++node){
            f(node);
        }
    }

    // serialization de-serialization

    uint64_t serializationSize() const{
        return DIM + 1;
    }

    template<class ITER>
    void serialize(ITER iter) const{
        for(auto d=0; d<DIM; ++d){
            *iter = gridGraph_.shape(d);
            ++iter;
        }
        // simple nh?
        *iter = true;
        ++iter;
    }

    template<class ITER>
    void deserialize(ITER iter);

    /**
     * @brief convert an image with DIM dimension to an edge map
     * @details convert an image with DIM dimension to an edge map
     * by applying a binary functor to the values of a node map at
     * the endpoints of an edge.
     *
     * @param       image the  input image
     * @param       binaryFunctor a binary functor
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class IMAGE, class BINARY_FUNCTOR, class EDGE_MAP>
    void imageToEdgeMap(
        const IMAGE & image,
        BINARY_FUNCTOR binaryFunctor,
        EDGE_MAP & edgeMap
    )const{
        for(const auto edge : this->edges()){
            const auto uv = this->uv(edge);
            CoordinateType cU, cV;
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);
            const auto uVal = xtensor::read(image, cU.asStdArray());
            const auto vVal = xtensor::read(image, cV.asStdArray());

            edgeMap[edge] = binaryFunctor(uVal, vVal);
        }
    }

    /**
     * @brief convert an image with DIM + 1 dimension to an edge map
     * @details convert an image with DIM + 1 dimension to an edge map
     * by computing the distance between the values of a node map at
     * the endpoints of an edge.
     *
     * @param       image the  input image
     * @param       distance   the distance (l1, l2 or cosine)
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class IMAGE, class EDGE_MAP>
    void imageWithChannelsToEdgeMap(
        const IMAGE & image,
        const std::string & distance,
        EDGE_MAP & edgeMap
    ) const {
        if(distance == "l1") {
            imageWithChannelsToEdgeMapImpl(image, edgeMap, detail_graph::l1Distance<DIM, IMAGE>);
        } else if(distance == "l2") {
            imageWithChannelsToEdgeMapImpl(image, edgeMap, detail_graph::l2Distance<DIM, IMAGE>);
        } else if(distance == "cosine") {
            imageWithChannelsToEdgeMapImpl(image, edgeMap, detail_graph::cosineDistance<DIM, IMAGE>);
        } else {
            throw std::runtime_error("Invalid distance.");
        }
    }

    template<class IMAGE, class EDGES, class EDGE_MAP>
    std::size_t imageWithChannelsToEdgeMapWithOffsets(
        const IMAGE & image,
        const std::string & distance,
        const std::vector<std::vector<int>> & offsets,
        EDGES & edges,
        EDGE_MAP & edgeMap
    ) const {

        std::size_t edgeId = 0;
        auto sampler = [](const nifty::array::StaticArray<int64_t, DIM+1> & coord){return true;};

        if(distance == "l1") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::l1Distance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else if(distance == "l2") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::l2Distance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else if(distance == "cosine") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::cosineDistance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else {
            throw std::runtime_error("Invalid distance.");
        }

        return edgeId;
    }

    template<class IMAGE, class EDGES, class EDGE_MAP>
    std::size_t imageWithChannelsToEdgeMapWithOffsets(
        const IMAGE & image,
        const std::string & distance,
        const std::vector<std::vector<int>> & offsets,
        const std::vector<int> & strides,
        EDGES & edges,
        EDGE_MAP & edgeMap
    ) const {

        std::size_t edgeId = 0;

        auto sampler = [&strides](const nifty::array::StaticArray<int64_t, DIM+1> & coord){
            bool inStride = true;
            for(unsigned d = 0; d < DIM; ++d) {
                if(coord[d+1] % strides[d] != 0) {
                    inStride = false;
                    break;
                }
            }
            return inStride;
        };

        if(distance == "l1") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::l1Distance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else if(distance == "l2") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::l2Distance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else if(distance == "cosine") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::cosineDistance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else {
            throw std::runtime_error("Invalid distance.");
        }

        return edgeId;
    }

    template<class IMAGE, class EDGES, class EDGE_MAP>
    std::size_t imageWithChannelsToEdgeMapWithOffsets(
        const IMAGE & image,
        const std::string & distance,
        const std::vector<std::vector<int>> & offsets,
        const double sampleProbability,
        EDGES & edges,
        EDGE_MAP & edgeMap
    ) const {

        std::size_t edgeId = 0;

        std::default_random_engine gen;
        std::uniform_real_distribution<double> distr;
        auto draw = std::bind(distr, gen);

        auto sampler = [&draw, sampleProbability](const nifty::array::StaticArray<int64_t, DIM+1> & coord){
            return draw() < sampleProbability;
        };

        if(distance == "l1") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::l1Distance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else if(distance == "l2") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::l2Distance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else if(distance == "cosine") {
            edgeId = imageWithChannelsToEdgeMapWithOffsetsImpl(image, offsets,
                                                               detail_graph::cosineDistance<DIM, IMAGE>,
                                                               sampler,
                                                               edges, edgeMap);
        } else {
            throw std::runtime_error("Invalid distance.");
        }

        return edgeId;
    }

    /**
     * @brief convert an affinity map with DIM+1 dimension to an edge map
     * @details convert an affinity map with DIM+1 dimension to an edge map
     * by assigning the affinity values to corresponding affinity values
     *
     * @param       image the input affinities
     * @param       whether affinities encode connectivity yo lower or upper pixel
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class AFFINITIES, class EDGE_MAP>
    void affinitiesToEdgeMap(const AFFINITIES & affinities,
                             EDGE_MAP & edgeMap,
                             const bool toLower=true) const {
        NIFTY_CHECK_OP(affinities.shape()[0], ==, DIM, "wrong number of affinity channels")
        for(auto d=1; d<DIM+1; ++d){
            NIFTY_CHECK_OP(shape(d-1), ==, affinities.shape()[d], "wrong shape")
        }

        typedef nifty::array::StaticArray<int64_t, DIM+1> AffinityCoordType;

        CoordinateType cU, cV;
        for(const auto edge : this->edges()){

            const auto uv = this->uv(edge);
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);

            // find the correct affinity edge
            AffinityCoordType affCoord;
            for(std::size_t d = 0; d < DIM; ++d) {
                auto diff = cU[d] - cV[d];
                if(diff == 0) {
                    affCoord[d + 1] = cU[d];
                }
                else {
                    affCoord[d + 1] = toLower ? (cU[d] < cV[d] ? cU[d] : cV[d]) : (cU[d] < cV[d] ? cV[d] : cU[d]);
                    affCoord[0] = d;
                }
            }

            edgeMap(edge) = xtensor::read(affinities, affCoord.asStdArray());
        }
    }

    template<class AFFINITIES, class EDGES, class EDGE_MAP>
    std::size_t affinitiesToEdgeMapWithOffsets(const AFFINITIES & affinities,
                                               const std::vector<std::vector<int>> & offsets,
                                               EDGES & edges,
                                               EDGE_MAP & edgeMap) const {
        auto sampler = [](const nifty::array::StaticArray<int64_t, DIM+1> & coord){return true;};
        const std::size_t edgeId = affinitiesToEdgeMapWithOffsetsImpl(
            affinities, offsets,
            edges, edgeMap, sampler
        );
        return edgeId;
    }

    template<class AFFINITIES, class EDGES, class EDGE_MAP>
    std::size_t affinitiesToEdgeMapWithOffsets(const AFFINITIES & affinities,
                                               const std::vector<std::vector<int>> & offsets,
                                               const std::vector<int> & strides,
                                               EDGES & edges,
                                               EDGE_MAP & edgeMap) const {

        auto sampler = [&strides](const nifty::array::StaticArray<int64_t, DIM+1> & coord){
            bool inStride = true;
            for(unsigned d = 0; d < DIM; ++d) {
                if(coord[d+1] % strides[d] != 0) {
                    inStride = false;
                    break;
                }
            }
            return inStride;
        };

        const std::size_t edgeId = affinitiesToEdgeMapWithOffsetsImpl(
            affinities, offsets,
            edges, edgeMap, sampler
        );
        return edgeId;
    }

    template<class AFFINITIES, class EDGES, class EDGE_MAP>
    std::size_t affinitiesToEdgeMapWithOffsets(const AFFINITIES & affinities,
                                               const std::vector<std::vector<int>> & offsets,
                                               const double sampleProbability,
                                               EDGES & edges,
                                               EDGE_MAP & edgeMap) const {

        std::default_random_engine gen;
        std::uniform_real_distribution<double> distr(0., 1.);
        auto draw = std::bind(distr, gen);

        auto sampler = [&draw, sampleProbability](const nifty::array::StaticArray<int64_t, DIM+1> & coord){
            return draw() < sampleProbability;
        };

        const std::size_t edgeId = affinitiesToEdgeMapWithOffsetsImpl(
            affinities, offsets,
            edges, edgeMap, sampler
        );
        return edgeId;
    }

    template<class AFFINITIES, class MASK, class EDGES, class EDGE_MAP>
    std::size_t affinitiesToEdgeMapWithOffsets(const AFFINITIES & affinities,
                                               const std::vector<std::vector<int>> & offsets,
                                               const MASK & mask,
                                               EDGES & edges,
                                               EDGE_MAP & edgeMap) const {

        std::default_random_engine gen;
        std::uniform_real_distribution<double> distr(0., 1.);
        auto draw = std::bind(distr, gen);

        auto sampler = [&mask](const nifty::array::StaticArray<int64_t, DIM+1> & coord){
            return xtensor::read(mask, coord);
        };

        const std::size_t edgeId = affinitiesToEdgeMapWithOffsetsImpl(
            affinities, offsets,
            edges, edgeMap, sampler
        );
        return edgeId;
    }

    /**
     * @brief convert an image with DIM dimension to an edge map
     * @details convert an image with DIM dimension to an edge map
     * by taking the values of the image at the
     * interpixel coordinates.
     * The shape of the image must be 2*shape-1
     *
     *
     * @param       image the  input image
     * @param       binaryFunctor a binary functor
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class IMAGE, class EDGE_MAP>
    void imageToInterpixelEdgeMap(
        const IMAGE & image,
        EDGE_MAP & edgeMap
    )const{

        for(auto d=0; d<DIM; ++d){
            NIFTY_CHECK_OP(shape(d)*2 - 1, ==, image.shape()[d],
                "wrong shape foer image to interpixel edge map")
        }

        for(const auto edge : this->edges()){
            const auto uv = this->uv(edge);
            CoordinateType cU,cV;
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);
            // FIXME I don't understand what is going on here ?!
            // But doesn't look right
            const auto uVal = xtensor::read(image, cU.asStdArray());
            cU += cV;
            edgeMap(edge) = xtensor::read(image, cU.asStdArray());
        }
    }

    uint64_t shape(const std::size_t d)const{
        return gridGraph_.shape(DIM-1-d);
    }

    // COORDINATE RELATED
    CoordinateType nodeToCoordinate(const uint64_t node)const{
        CoordinateType ret;
        nodeToCoordinate(node, ret);
        return ret;
    }

    template<class NODE_COORDINATE>
    void nodeToCoordinate(
        const uint64_t node,
        NODE_COORDINATE & coordinate
    )const{
        AndresVertexCoordinate aCoordinate;
        gridGraph_.vertex(node, aCoordinate);
        for(auto d=0; d<DIM; ++d){
            coordinate[d] = aCoordinate[DIM-1-d];
        }
    }

    template<class NODE_COORDINATE>
    uint64_t coordinateToNode(const NODE_COORDINATE & coordinate)const{
        AndresVertexCoordinate aCoordinate;
        for(auto d=0; d<DIM; ++d){
            aCoordinate[DIM-1-d] = coordinate[d];
        }
        return gridGraph_.vertex(aCoordinate);
    }

    //
    // edge id projection (with and w/o offsets)
    //

    template<class RET>
    void projectEdgeIdsToPixels(
        RET & ret
    ) const {
        typedef nifty::array::StaticArray<int64_t, DIM+1> EdgeCoordType;
        CoordinateType cU, cV;
        EdgeCoordType cEdge;
        for(const auto edge : this->edges()){
            const auto uv = this->uv(edge);
            nodeToCoordinate(uv.first, cU);
            nodeToCoordinate(uv.second, cV);

            for(unsigned d = 0; d < DIM; ++d) {
                cEdge[d+1] = cU[d];
                if(cU[d] != cV[d]) {
                    cEdge[0] = d;
                }
            }

            xtensor::write(ret, cEdge, edge);
        }
    }

    template<class RET>
    void projectEdgeIdsToPixels(
        const std::vector<std::vector<int>> & offsets,
        RET & ret
    ) const {
        auto sampler = [](const nifty::array::StaticArray<int64_t, DIM+1> & coord){return true;};
        projectEdgeIdsToPixelsImpl(offsets, sampler, ret);
    }

    template<class RET>
    void projectEdgeIdsToPixels(
        const std::vector<std::vector<int>> & offsets,
        const std::vector<int> & strides,
        RET & ret
    ) const {
        auto sampler = [&strides](const nifty::array::StaticArray<int64_t, DIM+1> & coord){
            bool inStride = true;
            for(unsigned d = 0; d < DIM; ++d) {
                if(coord[d+1] % strides[d] != 0) {
                    inStride = false;
                    break;
                }
            }
            return inStride;
        };
        projectEdgeIdsToPixelsImpl(offsets, sampler, ret);
    }

    template<class MASK, class RET>
    void projectEdgeIdsToPixels(
        const std::vector<std::vector<int>> & offsets,
        const MASK & mask,
        RET & ret
    ) const {
        auto sampler = [&mask](const nifty::array::StaticArray<int64_t, DIM+1> & coord){
            return xtensor::read(mask, coord);
        };
        projectEdgeIdsToPixelsImpl(offsets, sampler, ret);
    }

    template<class RET>
    void projectNodeIdsToPixels(RET & ret) const {

        CoordinateType shape_;
        for(unsigned d = 0; d < DIM; ++d) {
            shape_[d] = shape(d);
        }

        nifty::tools::forEachCoordinate(shape_, [&](const auto & coordP){
            const auto u = coordinateToNode(coordP);
            xtensor::write(ret, coordP, u);
        });
    }

private:

    //
    // implementation of imageWithChannelsToEdgeMap
    //

    template<class IMAGE, class EDGE_MAP, class F>
    void imageWithChannelsToEdgeMapImpl(
        const IMAGE & image,
        EDGE_MAP & edgeMap,
        F & dist
    ) const {
        const int nChannels = image.shape()[0];
        nifty::array::StaticArray<int64_t, DIM+1> coordU;
        nifty::array::StaticArray<int64_t, DIM+1> coordV;
        for(const auto edge : this->edges()){
            const auto uv = this->uv(edge);
            CoordinateType cU, cV;
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);
            for(unsigned d = 0; d < DIM; ++d) {
                coordU[d + 1] = cU[d];
                coordV[d + 1] = cV[d];
            }
            edgeMap(edge) = dist(image, nChannels, coordU, coordV);
        }
    }

    //
    // implementations of imageToEdgeMapWithChannelsWithOffsets
    //

    template<class IMAGE, class DIST, class SAMPLER,
             class EDGES, class EDGE_MAP>
    std::size_t imageWithChannelsToEdgeMapWithOffsetsImpl(
        const IMAGE & image,
        const std::vector<std::vector<int>> & offsets,
        DIST & dist,
        SAMPLER & sampler,
        EDGES & edges,
        EDGE_MAP & edgeMap
    ) const {
        typedef typename EDGE_MAP::value_type edgeValType;
        typedef nifty::array::StaticArray<int64_t, DIM+1> CoordWithChannelType;
        typedef nifty::array::StaticArray<int64_t, DIM+1> EdgeCoordType;

        const unsigned nChannels = image.shape()[0];
        const std::size_t nOffsets = offsets.size();

        CoordinateType coordU;
        CoordinateType coordV;
        CoordWithChannelType coordUC;
        CoordWithChannelType coordVC;

        for(auto d=1; d<DIM+1; ++d){
            NIFTY_CHECK_OP(shape(d-1), ==, image.shape()[d], "wrong shape")
        }

        EdgeCoordType edgeShape;
        edgeShape[0] = offsets.size();
        for(unsigned d = 0; d < DIM; ++d) {
            edgeShape[d + 1] = shape(d);
        }

        std::size_t edgeId = 0;
        tools::forEachCoordinate(edgeShape, [&](const EdgeCoordType & edgeCoord) {
            const auto & offset = offsets[edgeCoord[0]];

            // initialise the coordinates w/o and w/ channel
            bool isValid = true;
            for(unsigned d = 0; d < DIM; ++d) {
                coordU[d] = edgeCoord[d+1];
                coordV[d] = edgeCoord[d+1] + offset[d];
                // range check
                if(coordV[d] >= shape(d) || coordV[d] < 0) {
                    isValid = false;
                    break;
                }
                coordUC[d+1] = coordU[d];
                coordVC[d+1] = coordV[d];
            }
            if(!isValid) {
                return;
            }

            if(!sampler(edgeCoord)) {
                return;
            }

            const std::size_t u = coordinateToNode(coordU);
            const std::size_t v = coordinateToNode(coordV);

            edgeMap(edgeId) = dist(image, nChannels, coordUC, coordVC);
            edges(edgeId, 0) = std::min(u, v);
            edges(edgeId, 1) = std::max(u, v);
            ++edgeId;
        });
        return edgeId;
    }

    //
    // implementation of affinitiesToEdgeMapWithOffsets
    //

    template<class AFFINITIES, class EDGES, class EDGE_MAP, class F>
    std::size_t affinitiesToEdgeMapWithOffsetsImpl(const AFFINITIES & affinities,
                                                   const std::vector<std::vector<int>> & offsets,
                                                   EDGES & edges,
                                                   EDGE_MAP & edgeMap,
                                                   F & sampler) const {
        NIFTY_CHECK_OP(affinities.shape()[0], ==, offsets.size(), "wrong shape")
        for(auto d=1; d<DIM+1; ++d){
            NIFTY_CHECK_OP(shape(d-1), ==, affinities.shape()[d], "wrong shape")
        }

        typedef nifty::array::StaticArray<int64_t, DIM+1> EdgeCoordType;
        EdgeCoordType edgeShape;
        edgeShape[0] = offsets.size();
        for(unsigned d = 0; d < DIM; ++d) {
            edgeShape[d + 1] = shape(d);
        }

        CoordinateType cU, cV;
        std::size_t edgeId = 0;
        tools::forEachCoordinate(edgeShape, [&](const EdgeCoordType & edgeCoord) {
            const auto & offset = offsets[edgeCoord[0]];

            for(unsigned d = 0; d < DIM; ++d) {
                cU[d] = edgeCoord[d + 1];
                cV[d] = edgeCoord[d + 1] + offset[d];
                // range check
                if(cV[d] >= shape(d) || cV[d] < 0) {
                    return;
                }
            }

            // check if we keep this edge
            if(!sampler(edgeCoord)) {
                return;
            }

            const std::size_t u = coordinateToNode(cU);
            const std::size_t v = coordinateToNode(cV);

            edgeMap(edgeId) = xtensor::read(affinities, edgeCoord.asStdArray());
            edges(edgeId, 0) = std::min(u, v);
            edges(edgeId, 1) = std::max(u, v);
            ++edgeId;

        });
        return edgeId;
    }

    //
    // implementation of projectEdgeIdsToPixels (with offsets)
    //

    template<class RET, class SAMPLER>
    void projectEdgeIdsToPixelsImpl(
        const std::vector<std::vector<int>> & offsets,
        SAMPLER & sampler,
        RET & ret
    ) const {
        typedef typename RET::value_type retType;
        typedef nifty::array::StaticArray<int64_t, DIM+1> EdgeCoordType;

        EdgeCoordType edgeShape;
        edgeShape[0] = offsets.size();
        for(unsigned d = 0; d < DIM; ++d) {
            edgeShape[d + 1] = shape(d);
        }

        CoordinateType cV;
        retType edgeId = 0;
        nifty::tools::forEachCoordinate(edgeShape, [&](const auto & edgeCoord){
            const auto & offset = offsets[edgeCoord[0]];
            bool isValid = true;

            for(unsigned d = 0; d < DIM; ++d) {
                cV[d] = edgeCoord[d + 1] + offset[d];
                // range check
                if(cV[d] >= shape(d) || cV[d] < 0) {
                    isValid = false;
                    break;
                }
            }

            if(!isValid) {
                xtensor::write(ret, edgeCoord, -1);
                return;
            }

            if(!sampler(edgeCoord)) {
                xtensor::write(ret, edgeCoord, -1);
                return;
            }
            xtensor::write(ret, edgeCoord, edgeId);
            ++edgeId;
        });
    }

private:
    andres::graph::GridGraph<DIM> gridGraph_;
};

} // namespace nifty::graph
} // namespace nifty


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

 * 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    HessianFactor.cpp
 * @author  Richard Roberts
 * @date    Dec 8, 2010
 */

#include <sstream>

#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>

#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
string SlotEntry::toString() const {
	ostringstream oss;
	oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
	return oss.str();
}

/* ************************************************************************* */
void HessianFactor::assertInvariants() const {
	GaussianFactor::assertInvariants(); // The base class checks for unique keys
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const HessianFactor& gf) :
    		GaussianFactor(gf), info_(matrix_) {
	info_.assignNoalias(gf.info_);
	assertInvariants();
}

/* ************************************************************************* */
HessianFactor::HessianFactor() : info_(matrix_) {
  // The empty HessianFactor has only a constant error term of zero
  FastVector<size_t> dims;
  dims.push_back(1);
  info_.resize(dims.begin(), dims.end(), false);
  info_(0,0)(0,0) = 0.0;
	assertInvariants();
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Index j, const Matrix& G, const Vector& g, double f) :
      		GaussianFactor(j), info_(matrix_) {
	if(G.rows() != G.cols() || G.rows() != g.size())
		throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
	size_t dims[] = { G.rows(), 1 };
	InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1);
	BlockInfo infoMatrix(fullMatrix, dims, dims+2);
	infoMatrix(0,0) = G;
	infoMatrix.column(0,1,0) = g;
	infoMatrix(1,1)(0,0) = f;
	infoMatrix.swap(info_);
	assertInvariants();
}

/* ************************************************************************* */
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
HessianFactor::HessianFactor(Index j, const Vector& mu, const Matrix& Sigma) :
		GaussianFactor(j), info_(matrix_) {
	if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument(
			"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
	Matrix G = inverse(Sigma);
	Vector g = G * mu;
	double f = dot(mu, g);
	size_t dims[] = { G.rows(), 1 };
	InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1);
	BlockInfo infoMatrix(fullMatrix, dims, dims + 2);
	infoMatrix(0, 0) = G;
	infoMatrix.column(0, 1, 0) = g;
	infoMatrix(1, 1)(0, 0) = f;
	infoMatrix.swap(info_);
	assertInvariants();
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Index j1, Index j2,
		const Matrix& G11, const Matrix& G12, const Vector& g1,
		const Matrix& G22, const Vector& g2, double f) :
		GaussianFactor(j1, j2), info_(matrix_) {
	if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != g1.size() ||
			G22.cols() != G12.cols() || G22.cols() != g2.size())
		throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
	size_t dims[] = { G11.rows(), G22.rows(), 1 };
	InfoMatrix fullMatrix(G11.rows() + G22.rows() + 1, G11.rows() + G22.rows() + 1);
	BlockInfo infoMatrix(fullMatrix, dims, dims+3);
	infoMatrix(0,0) = G11;
	infoMatrix(0,1) = G12;
	infoMatrix.column(0,2,0) = g1;
	infoMatrix(1,1) = G22;
	infoMatrix.column(1,2,0) = g2;
	infoMatrix(2,2)(0,0) = f;
	infoMatrix.swap(info_);
	assertInvariants();
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Index j1, Index j2, Index j3,
    const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
    const Matrix& G22, const Matrix& G23, const Vector& g2,
    const Matrix& G33, const Vector& g3, double f) :
    GaussianFactor(j1, j2, j3), info_(matrix_) {
	if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows()  || G11.rows() != g1.size() ||
			G22.cols() != G12.cols() || G33.cols() != G13.cols() ||  G22.cols() != g2.size() || G33.cols() != g3.size())
		throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
	size_t dims[] = { G11.rows(), G22.rows(), G33.rows(), 1 };
	InfoMatrix fullMatrix(G11.rows() + G22.rows() + G33.rows() + 1, G11.rows() + G22.rows() + G33.rows() + 1);
	BlockInfo infoMatrix(fullMatrix, dims, dims+4);
	infoMatrix(0,0) = G11;
	infoMatrix(0,1) = G12;
	infoMatrix(0,2) = G13;
	infoMatrix.column(0,3,0) = g1;
	infoMatrix(1,1) = G22;
	infoMatrix(1,2) = G23;
	infoMatrix.column(1,3,0) = g2;
	infoMatrix(2,2) = G33;
	infoMatrix.column(2,3,0) = g3;
	infoMatrix(3,3)(0,0) = f;
	infoMatrix.swap(info_);
	assertInvariants();
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
        const std::vector<Vector>& gs, double f) : GaussianFactor(js), info_(matrix_) {

  // Get the number of variables
  size_t variable_count = js.size();

  // Verify the provided number of entries in the vectors are consistent
  if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2)
    throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
        in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");

  // Verify the dimensions of each provided matrix are consistent
  // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula
  for(size_t i = 0; i < variable_count; ++i){
    int block_size = gs[i].size();
    // Check rows
    for(size_t j = 0; j < variable_count-i; ++j){
      size_t index = i*(2*variable_count - i + 1)/2 + j;
      if(Gs[index].rows() != block_size){
        throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
      }
    }
    // Check cols
    for(size_t j = 0; j <= i; ++j){
      size_t index = j*(2*variable_count - j + 1)/2 + (i-j);
      if(Gs[index].cols() != block_size){
        throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
      }
    }
  }

  // Create the dims vector
  size_t* dims = (size_t*)alloca(sizeof(size_t)*(variable_count+1)); // FIXME: alloca is bad, just ask Google.
  size_t total_size = 0;
  for(unsigned int i = 0; i < variable_count; ++i){
    dims[i] = gs[i].size();
    total_size += gs[i].size();
  }
  dims[variable_count] = 1;
  total_size += 1;

  // Fill in the internal matrix with the supplied blocks
  InfoMatrix fullMatrix(total_size, total_size);
  BlockInfo infoMatrix(fullMatrix, dims, dims+variable_count+1);
  size_t index = 0;
  for(size_t i = 0; i < variable_count; ++i){
    for(size_t j = i; j < variable_count; ++j){
      infoMatrix(i,j) = Gs[index++];
    }
    infoMatrix.column(i,variable_count,0) = gs[i];
  }
  infoMatrix(variable_count,variable_count)(0,0) = f;

  // update the BlockView variable
  infoMatrix.swap(info_);

  assertInvariants();
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) {
	JacobianFactor jf(cg);
	info_.copyStructureFrom(jf.Ab_);
	matrix_.noalias() = jf.matrix_.transpose() * jf.matrix_;
	assertInvariants();
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
	// Copy the variable indices
	(GaussianFactor&)(*this) = gf;
	// Copy the matrix data depending on what type of factor we're copying from
	if(dynamic_cast<const JacobianFactor*>(&gf)) {
		const JacobianFactor& jf(static_cast<const JacobianFactor&>(gf));
		if(jf.model_->isConstrained())
			throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
		else {
			Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas());
			info_.copyStructureFrom(jf.Ab_);
			BlockInfo::constBlock A = jf.Ab_.full();
			matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A;
		}
	} else if(dynamic_cast<const HessianFactor*>(&gf)) {
		const HessianFactor& hf(static_cast<const HessianFactor&>(gf));
		info_.assignNoalias(hf.info_);
	} else
		throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
	assertInvariants();
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
		const vector<size_t>& dimensions, const Scatter& scatter) :
		info_(matrix_) {

	const bool debug = ISDEBUG("EliminateCholesky");
	// Form Ab' * Ab
	tic(1, "allocate");
	info_.resize(dimensions.begin(), dimensions.end(), false);
	// Fill in keys
	keys_.resize(scatter.size());
	std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1));
	toc(1, "allocate");
	tic(2, "zero");
	matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols());
	toc(2, "zero");
	tic(3, "update");
	if (debug) cout << "Combining " << factors.size() << " factors" << endl;
	BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
	{
		if(factor) {
			if(shared_ptr hessian = boost::dynamic_pointer_cast<HessianFactor>(factor))
				updateATA(*hessian, scatter);
			else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(factor))
				updateATA(*jacobianFactor, scatter);
			else
				throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
		}
	}
	toc(3, "update");

	if (debug) gtsam::print(matrix_, "Ab' * Ab: ");

	assertInvariants();
}

/* ************************************************************************* */
HessianFactor& HessianFactor::operator=(const HessianFactor& rhs) {
  this->Base::operator=(rhs);     // Copy keys
  info_.assignNoalias(rhs.info_); // Copy matrix and block structure
  return *this;
}

/* ************************************************************************* */
void HessianFactor::print(const std::string& s, const IndexFormatter& formatter) const {
	cout << s << "\n";
	cout << " keys: ";
	for(const_iterator key=this->begin(); key!=this->end(); ++key)
		cout << formatter(*key) << "(" << this->getDim(key) << ") ";
	cout << "\n";
	gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
}

/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
	if(!dynamic_cast<const HessianFactor*>(&lf))
		return false;
	else {
		Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
		thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
		Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>();
		rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
		return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
	}
}

/* ************************************************************************* */
Matrix HessianFactor::computeInformation() const {
  return info_.full().selfadjointView<Eigen::Upper>();
}

/* ************************************************************************* */
double HessianFactor::error(const VectorValues& c) const {
	// error 0.5*(f - 2*x'*g + x'*G*x)
	const double f = constantTerm();
	const double xtg = c.vector().dot(linearTerm());
	const double xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() *	c.vector();

	return 0.5 * (f - 2.0 * xtg +  xGx);
}

/* ************************************************************************* */
void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) {

	// This function updates 'combined' with the information in 'update'.
	// 'scatter' maps variables in the update factor to slots in the combined
	// factor.

	const bool debug = ISDEBUG("updateATA");

	// First build an array of slots
	tic(1, "slots");
	size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
	size_t slot = 0;
	BOOST_FOREACH(Index j, update) {
		slots[slot] = scatter.find(j)->second.slot;
		++ slot;
	}
	toc(1, "slots");

	if(debug) {
		this->print("Updating this: ");
		update.print("with (Hessian): ");
	}

	// Apply updates to the upper triangle
	tic(3, "update");
	for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
		size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
		for(size_t j1=0; j1<=j2; ++j1) {
			size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
			if(slot2 > slot1) {
				if(debug)
					cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
				matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
						update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols());
			} else if(slot1 > slot2) {
				if(debug)
					cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
				matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() +=
						update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()).transpose();
			} else {
				if(debug)
					cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
				matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView<Eigen::Upper>() +=
						update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols());
			}
			if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
			if(debug) this->print();
		}
	}
	toc(3, "update");
}

/* ************************************************************************* */
void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) {

	// This function updates 'combined' with the information in 'update'.
	// 'scatter' maps variables in the update factor to slots in the combined
	// factor.

	const bool debug = ISDEBUG("updateATA");

	// First build an array of slots
	tic(1, "slots");
	size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
	size_t slot = 0;
	BOOST_FOREACH(Index j, update) {
		slots[slot] = scatter.find(j)->second.slot;
		++ slot;
	}
	toc(1, "slots");

	tic(2, "form A^T*A");
	if(update.model_->isConstrained())
		throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model");

	if(debug) {
		this->print("Updating this: ");
		update.print("with (Jacobian): ");
	}

	typedef Eigen::Block<const JacobianFactor::AbMatrix> BlockUpdateMatrix;
	BlockUpdateMatrix updateA(update.matrix_.block(
			update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols()));
	if (debug) cout << "updateA: \n" << updateA << endl;

	Matrix updateInform;
	if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
		updateInform.noalias() = updateA.transpose() * updateA;
	} else {
		noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
		if(diagonal) {
			Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas());
			updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA;
		} else
			throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
	}
	if (debug) cout << "updateInform: \n" << updateInform << endl;
 	toc(2, "form A^T*A");

	// Apply updates to the upper triangle
	tic(3, "update");
	for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
		size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
		for(size_t j1=0; j1<=j2; ++j1) {
			size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
			size_t off0 = update.Ab_.offset(0);
			if(slot2 > slot1) {
				if(debug)
					cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
				matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
						updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols());
			} else if(slot1 > slot2) {
				if(debug)
					cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
				matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() +=
						updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()).transpose();
			} else {
				if(debug)
					cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
				matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView<Eigen::Upper>() +=
						updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols());
			}
			if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
			if(debug) this->print();
		}
	}
	toc(3, "update");
}

/* ************************************************************************* */
void HessianFactor::partialCholesky(size_t nrFrontals) {
	if(!choleskyPartial(matrix_, info_.offset(nrFrontals)))
		throw IndeterminantLinearSystemException(this->keys().front());
}

/* ************************************************************************* */
GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals) {

  static const bool debug = false;

  // Extract conditionals
  tic(1, "extract conditionals");
  GaussianConditional::shared_ptr conditional(new GaussianConditional());
  typedef VerticalBlockView<Matrix> BlockAb;
  BlockAb Ab(matrix_, info_);

  size_t varDim = info_.offset(nrFrontals);
  Ab.rowEnd() = Ab.rowStart() + varDim;

  // Create one big conditionals with many frontal variables.
  tic(2, "construct cond");
  Vector sigmas = Vector::Ones(varDim);
  conditional = boost::make_shared<ConditionalType>(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas);
  toc(2, "construct cond");
  if(debug) conditional->print("Extracted conditional: ");

  toc(1, "extract conditionals");

  // Take lower-right block of Ab_ to get the new factor
  tic(2, "remaining factor");
  info_.blockStart() = nrFrontals;
  // Assign the keys
  vector<Index> remainingKeys(keys_.size() - nrFrontals);
  remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
  keys_.swap(remainingKeys);
  toc(2, "remaining factor");

  return conditional;
}

/* ************************************************************************* */
GaussianFactor::shared_ptr HessianFactor::negate() const {
  // Copy Hessian Blocks from Hessian factor and invert
  std::vector<Index> js;
  std::vector<Matrix> Gs;
  std::vector<Vector> gs;
  double f;
  js.insert(js.end(), begin(), end());
  for(size_t i = 0; i < js.size(); ++i){
    for(size_t j = i; j < js.size(); ++j){
      Gs.push_back( -info(begin()+i, begin()+j) );
    }
    gs.push_back( -linearTerm(begin()+i) );
  }
  f = -constantTerm();

  // Create the Anti-Hessian Factor from the negated blocks
  return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
}

} // gtsam


#ifndef MAPNIK_SIMPLIFY_CONVERTER_HPP
#define MAPNIK_SIMPLIFY_CONVERTER_HPP

// mapnik
#include <mapnik/config.hpp>
#include <mapnik/box2d.hpp>
#include <mapnik/vertex.hpp>
#include <mapnik/simplify.hpp>
#include <mapnik/noncopyable.hpp>

// stl
#include <limits>
#include <set>
#include <vector>
#include <deque>
#include <cmath>
#include <stdexcept>

// boost
#include <boost/optional.hpp>

namespace mapnik
{

struct weighted_vertex : private mapnik::noncopyable
{
    vertex2d coord;
    double weight;
    weighted_vertex *prev;
    weighted_vertex *next;

    weighted_vertex(vertex2d coord_) :
        coord(coord_),
        weight(std::numeric_limits<double>::infinity()),
        prev(NULL),
        next(NULL) {}

    double nominalWeight()
    {
        if (prev == NULL || next == NULL || coord.cmd != SEG_LINETO) {
            return std::numeric_limits<double>::infinity();
        }
        vertex2d const& A = prev->coord;
        vertex2d const& B = next->coord;
        vertex2d const& C = coord;
        return std::abs((double)((A.x - C.x) * (B.y - A.y) - (A.x - B.x) * (C.y - A.y))) / 2.0;
    }

    struct ascending_sort
    {
        bool operator() (const weighted_vertex *a, const weighted_vertex *b)
        {
            return b->weight > a->weight;
        }
    };
};

struct sleeve
{
    vertex2d v[5];

    sleeve(vertex2d const& v0, vertex2d const& v1, double offset)
    {
        double a = std::atan2((v1.y - v0.y), (v1.x - v0.x));
        double dx = offset * std::cos(a);
        double dy = offset * std::sin(a);
        v[0].x = v0.x + dy;
        v[0].y = v0.y - dx;
        v[1].x = v0.x - dy;
        v[1].y = v0.y + dx;
        v[2].x = v1.x - dy;
        v[2].y = v1.y + dx;
        v[3].x = v1.x + dy;
        v[3].y = v1.y - dx;
        v[4].x = v0.x + dy;
        v[4].y = v0.y - dx;
    }

    bool inside(vertex2d const& q)
    {
        bool inside=false;

        for (unsigned i=0;i<4;++i)
        {
            if ((((v[i+1].y <= q.y) && (q.y < v[i].y)) ||
                 ((v[i].y <= q.y) && (q.y < v[i+1].y))) &&
                (q.x < (v[i].x - v[i+1].x) * (q.y - v[i+1].y)/ (v[i].y - v[i+1].y) + v[i+1].x))
                inside=!inside;
        }
        return inside;
    }
    void print()
    {
        std::cerr << "LINESTRING("
                  << v[0].x << " " << -v[0].y << ","
                  << v[1].x << " " << -v[1].y << ","
                  << v[2].x << " " << -v[2].y << ","
                  << v[3].x << " " << -v[3].y << ","
                  << v[0].x << " " << -v[0].y << ")" << std::endl;

    }
};

template <typename Geometry>
struct MAPNIK_DECL simplify_converter
{
public:
    simplify_converter(Geometry& geom)
        : geom_(geom),
        tolerance_(0.0),
        status_(initial),
        algorithm_(radial_distance),
        pos_(0)
    {}

    enum status
    {
        initial,
        process,
        closing,
        end,
        cache
    };

    simplify_algorithm_e get_simplify_algorithm()
    {
        return algorithm_;
    }

    void set_simplify_algorithm(simplify_algorithm_e value)
    {
        if (algorithm_ != value)
        {
            algorithm_ = value;
            reset();
        }
    }

    double get_simplify_tolerance()
    {
        return tolerance_;
    }

    void set_simplify_tolerance(double value)
    {
        if (tolerance_ != value) {
            tolerance_ = value;
            reset();
        }
    }

    void reset()
    {
        geom_.rewind(0);
        vertices_.clear();
        status_ = initial;
        pos_ = 0;
    }

    void rewind(unsigned int) const
    {
        pos_ = 0;
    }

    unsigned vertex(double* x, double* y)
    {
        if (tolerance_ == 0.0)
            return geom_.vertex(x, y);

        if (status_ == initial)
            init_vertices();

        return output_vertex(x, y);
    }

private:
    unsigned output_vertex(double* x, double* y)
    {
        switch (algorithm_)
        {
        case visvalingam_whyatt:
            return output_vertex_cached(x, y);
        case radial_distance:
            return output_vertex_distance(x, y);
        case zhao_saalfeld:
            return output_vertex_sleeve(x, y);
        default:
            throw std::runtime_error("simplification algorithm not yet implemented");
        }

        return SEG_END;
    }

    unsigned output_vertex_cached(double* x, double* y) {
        if (pos_ >= vertices_.size())
            return SEG_END;

        previous_vertex_ = vertices_[pos_];
        *x = previous_vertex_.x;
        *y = previous_vertex_.y;
        pos_++;
        return previous_vertex_.cmd;
    }

    unsigned output_vertex_distance(double* x, double* y) {
        if (status_ == closing) {
            status_ = end;
            return SEG_CLOSE;
        }

        vertex2d last(vertex2d::no_init);
        vertex2d vtx(vertex2d::no_init);
        while ((vtx.cmd = geom_.vertex(&vtx.x, &vtx.y)) != SEG_END)
        {
            if (vtx.cmd == SEG_LINETO) {
                if (distance_to_previous(vtx) > tolerance_) {
                    // Only output a vertex if it's far enough away from the previous
                    break;
                } else {
                    last = vtx;
                    // continue
                }
            } else if (vtx.cmd == SEG_CLOSE) {
                if (last.cmd == vertex2d::no_init) {
                    // The previous vertex was already output in the previous call.
                    // We can now safely output SEG_CLOSE.
                    status_ = end;
                } else {
                    // We eliminated the previous point because it was too close, but
                    // we have to output it now anyway, since this is the end of the
                    // vertex stream. Make sure that we output SEG_CLOSE in the next call.
                    vtx = last;
                    status_ = closing;
                }
                break;
            } else if (vtx.cmd == SEG_MOVETO) {
                break;
            } else {
                throw std::runtime_error("Unknown vertex command");
            }
        }

        previous_vertex_ = vtx;
        *x = vtx.x;
        *y = vtx.y;
        return vtx.cmd;
    }

    template <typename Iterator>
    bool fit_sleeve(Iterator itr,Iterator end, vertex2d const& v)
    {
        sleeve s(*itr,v,tolerance_);
        ++itr; // skip first vertex
        for (; itr!=end; ++itr)
        {
            if (!s.inside(*itr))
            {
                return false;
            }
        }
        return true;
    }

    unsigned output_vertex_sleeve(double* x, double* y)
    {
        vertex2d vtx(vertex2d::no_init);
        std::size_t min_size = 1;
        while ((vtx.cmd = geom_.vertex(&vtx.x, &vtx.y)) != SEG_END)
        {
            //if ((std::fabs(vtx.x - previous_vertex_.x) < 0.5) &&
            //    (std::fabs(vtx.y - previous_vertex_.y) < 0.5))
            //    continue;

            if (status_ == cache &&
                vertices_.size() >= min_size)
                status_ = process;

            previous_vertex_ = vtx;

            if (vtx.cmd == SEG_MOVETO)
            {
                if (sleeve_cont_.size() > 1)
                {
                    vertices_.push_back(sleeve_cont_.back());
                    sleeve_cont_.clear();
                }
                vertices_.push_back(vtx);
                sleeve_cont_.push_back(vtx);
                if (status_ == process) break;
            }
            else if (vtx.cmd == SEG_LINETO)
            {
                if (sleeve_cont_.size() > 1 && !fit_sleeve(sleeve_cont_.begin(), sleeve_cont_.end(), vtx))
                {
                    vertex2d last = vtx;
                    vtx = sleeve_cont_.back();
                    sleeve_cont_.clear();
                    sleeve_cont_.push_back(vtx);
                    sleeve_cont_.push_back(last);
                    vertices_.push_back(vtx);
                    if (status_ == process) break;
                }
                else
                {
                    sleeve_cont_.push_back(vtx);
                }
            }
            else if (vtx.cmd == SEG_CLOSE)
            {
                if (sleeve_cont_.size() > 1)
                {
                    vertices_.push_back(sleeve_cont_.back());
                    sleeve_cont_.clear();
                }
                vertices_.push_back(vtx);
                if (status_ == process) break;
            }
        }

        if (status_ == cache)
        {
            if (vertices_.size() < min_size)
                return SEG_END;
            status_ = process;
        }

        if (vtx.cmd == SEG_END)
        {
            if (sleeve_cont_.size() > 1)
            {
                vertices_.push_back(sleeve_cont_.back());
            }
            sleeve_cont_.clear();
            vertices_.push_back(vtx);
        }

        if (vertices_.size() > 0)
        {
            vertex2d v = vertices_.front();
            vertices_.pop_front();
            *x = v.x;
            *y = v.y;
            return v.cmd;
        }
        return SEG_END;
    }

    double distance_to_previous(vertex2d const& vtx) {
        double dx = previous_vertex_.x - vtx.x;
        double dy = previous_vertex_.y - vtx.y;
        return dx * dx + dy * dy;
    }

    status init_vertices()
    {
        if (status_ != initial) // already initialized
            return status_;

        reset();

        switch (algorithm_) {
            case visvalingam_whyatt:
                return init_vertices_visvalingam_whyatt();
            case radial_distance:
                // Use
                vertices_.push_back(vertex2d(vertex2d::no_init));
                return status_ = process;
            case zhao_saalfeld:
                return status_ = cache;
            default:
                throw std::runtime_error("simplification algorithm not yet implemented");
        }
    }

    status init_vertices_visvalingam_whyatt()
    {
        typedef std::set<weighted_vertex *, weighted_vertex::ascending_sort> VertexSet;
        typedef std::vector<weighted_vertex *> VertexList;

        std::vector<weighted_vertex *> v_list;
        vertex2d vtx(vertex2d::no_init);
        while ((vtx.cmd = geom_.vertex(&vtx.x, &vtx.y)) != SEG_END)
        {
            v_list.push_back(new weighted_vertex(vtx));
        }

        if (v_list.empty()) {
            return status_ = process;
        }

        // Connect the vertices in a linked list and insert them into the set.
        VertexSet v;
        for (VertexList::iterator i = v_list.begin(); i != v_list.end(); ++i)
        {
            (*i)->prev = i == v_list.begin() ? NULL : *(i - 1);
            (*i)->next = i + 1 == v_list.end() ? NULL : *(i + 1);
            (*i)->weight = (*i)->nominalWeight();
            v.insert(*i);
        }

        // Use Visvalingam-Whyatt algorithm to calculate each point's weight.
        while (v.size() > 0)
        {
            VertexSet::iterator lowest = v.begin();
            weighted_vertex *removed = *lowest;
            if (removed->weight >= tolerance_) {
                break;
            }

            v.erase(lowest);

            // Connect adjacent vertices with each other
            if (removed->prev) removed->prev->next = removed->next;
            if (removed->next) removed->next->prev = removed->prev;
            // Adjust weight and reinsert prev/next to move them to their correct position.
            if (removed->prev) {
                v.erase(removed->prev);
                removed->prev->weight = std::max(removed->weight, removed->prev->nominalWeight());
                v.insert(removed->prev);
            }
            if (removed->next) {
                v.erase(removed->next);
                removed->next->weight = std::max(removed->weight, removed->next->nominalWeight());
                v.insert(removed->next);
            }
        }

        v.clear();

        // Traverse the remaining list and insert them into the vertex cache.
        for (VertexList::iterator i = v_list.begin(); i != v_list.end(); ++i)
        {
            if ((*i)->weight >= tolerance_)
            {
                vertices_.push_back((*i)->coord);
            }
            delete *i;
        }

        // Initialization finished.
        return status_ = process;
    }

    Geometry&                       geom_;
    double                          tolerance_;
    status                          status_;
    simplify_algorithm_e            algorithm_;
    std::deque<vertex2d>            vertices_;
    std::deque<vertex2d>            sleeve_cont_;
    vertex2d                        previous_vertex_;
    mutable size_t                  pos_;
};

}

#endif // MAPNIK_SIMPLIFY_CONVERTER_HPP


#include "CALPHADConcSolverTernary.h"
#include "CALPHADFreeEnergyFunctionsTernary.h"
#include "CALPHADFunctions.h"
#include "CALPHADSpeciesPhaseGibbsEnergy.h"
#include "PhysicalConstants.h"

#include <chrono>

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <iomanip>
#include <iostream>
#include <string>

#include <omp.h>

namespace pt = boost::property_tree;

typedef std::chrono::high_resolution_clock Clock;

int main(int argc, char* argv[])
{
    const int N = 10000000;

#ifdef _OPENMP
    std::cout << "Compiled by an OpenMP-compliant implementation.\n";
    std::cout << "Run test with " << omp_get_max_threads() << " threads"
              << std::endl;
#endif

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadMoNbTa.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    double temperature = 2923.;

    CalphadDataType LmixABPhaseL[4][2];
    CalphadDataType LmixABPhaseA[4][2];

    CalphadDataType LmixACPhaseL[4][2];
    CalphadDataType LmixACPhaseA[4][2];

    CalphadDataType LmixBCPhaseL[4][2];
    CalphadDataType LmixBCPhaseA[4][2];

    CalphadDataType LmixABCPhaseL[3][2];
    CalphadDataType LmixABCPhaseA[3][2];

    {
        std::string dbnamemixL("LmixABCPhaseL");
        if (calphad_db.get_child_optional(dbnamemixL))
        {
            pt::ptree& Lmix0_db = calphad_db.get_child(dbnamemixL);
            Thermo4PFM::readLmixTernaryParameters(Lmix0_db, LmixABCPhaseL);
        }
        else
        {
            for (int j = 0; j < 3; j++)
                for (int i = 0; i < 2; i++)
                {
                    LmixABCPhaseL[j][i] = 0.;
                }
        }
    }
    {
        std::string dbnamemixL("LmixABCPhaseA");
        if (calphad_db.get_child_optional(dbnamemixL))
        {
            pt::ptree& Lmix0_db = calphad_db.get_child(dbnamemixL);
            Thermo4PFM::readLmixTernaryParameters(Lmix0_db, LmixABCPhaseA);
        }
        else
        {
            for (int j = 0; j < 3; j++)
                for (int i = 0; i < 2; i++)
                {
                    LmixABCPhaseA[j][i] = 0.;
                }
        }
    }

    {
        std::string dbnamemixL("LmixABPhaseL");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixL);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixABPhaseL);
    }
    {
        std::string dbnamemixA("LmixABPhaseA");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixA);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixABPhaseA);
    }
    {
        std::string dbnamemixL("LmixACPhaseL");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixL);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixACPhaseL);
    }
    {
        std::string dbnamemixA("LmixACPhaseA");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixA);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixACPhaseA);
    }
    {
        std::string dbnamemixL("LmixBCPhaseL");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixL);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixBCPhaseL);
    }
    {
        std::string dbnamemixA("LmixBCPhaseA");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixA);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixBCPhaseA);
    }

    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy g_species_phaseL[3];
    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy g_species_phaseA[3];

    {
        std::string dbnameL("PhaseL");
        std::string dbnameA("PhaseA");

        pt::ptree& speciesA_db = calphad_db.get_child("SpeciesA");
        g_species_phaseL[0].initialize("L0", speciesA_db.get_child(dbnameL));
        g_species_phaseA[0].initialize("A0", speciesA_db.get_child(dbnameA));

        pt::ptree& speciesB_db = calphad_db.get_child("SpeciesB");
        g_species_phaseL[1].initialize("L1", speciesB_db.get_child(dbnameL));
        g_species_phaseA[1].initialize("A1", speciesB_db.get_child(dbnameA));

        pt::ptree& speciesC_db = calphad_db.get_child("SpeciesC");
        g_species_phaseL[2].initialize("L2", speciesC_db.get_child(dbnameL));
        g_species_phaseA[2].initialize("A2", speciesC_db.get_child(dbnameA));
    }

    CalphadDataType fA[2];
    fA[0] = g_species_phaseL[0].fenergy(temperature);
    fA[1] = g_species_phaseA[0].fenergy(temperature);
    // std::cout<<"fA[0]="<<fA[0]<<", fA[1]="<<fA[1]<<std::endl;

    CalphadDataType fB[2];
    fB[0] = g_species_phaseL[1].fenergy(temperature);
    fB[1] = g_species_phaseA[1].fenergy(temperature);
    // std::cout<<"fB[0]="<<fB[0]<<", fB[1]="<<fB[1]<<std::endl;

    CalphadDataType fC[2];
    fC[0] = g_species_phaseL[2].fenergy(temperature);
    fC[1] = g_species_phaseA[2].fenergy(temperature);

    CalphadDataType L_AB_L[4];
    for (int i = 0; i < 4; i++)
        L_AB_L[i] = LmixABPhaseL[i][0] + temperature * LmixABPhaseL[i][1];

    CalphadDataType L_AB_S[4];
    for (int i = 0; i < 4; i++)
        L_AB_S[i] = LmixABPhaseA[i][0] + temperature * LmixABPhaseA[i][1];

    CalphadDataType L_AC_L[4];
    for (int i = 0; i < 4; i++)
        L_AC_L[i] = LmixACPhaseL[i][0] + temperature * LmixACPhaseL[i][1];

    CalphadDataType L_AC_S[4];
    for (int i = 0; i < 4; i++)
        L_AC_S[i] = LmixACPhaseA[i][0] + temperature * LmixACPhaseA[i][1];

    CalphadDataType L_BC_L[4];
    for (int i = 0; i < 4; i++)
        L_BC_L[i] = LmixBCPhaseL[i][0] + temperature * LmixBCPhaseL[i][1];

    CalphadDataType L_BC_S[4];
    for (int i = 0; i < 4; i++)
        L_BC_S[i] = LmixBCPhaseA[i][0] + temperature * LmixBCPhaseA[i][1];

    CalphadDataType L_ABC_L[3];
    for (int i = 0; i < 3; i++)
        L_ABC_L[i] = LmixABCPhaseL[i][0] + temperature * LmixABCPhaseL[i][1];

    CalphadDataType L_ABC_S[3];
    for (int i = 0; i < 3; i++)
        L_ABC_S[i] = LmixABCPhaseA[i][0] + temperature * LmixABCPhaseA[i][1];

    const double RTinv
        = 1.0 / (Thermo4PFM::gas_constant_R_JpKpmol * temperature);

    double sol[4] = { 0.33, 0.38, 0.32, 0.33 };

    const double deviation = 1.e-4;

#ifndef HAVE_OPENMP_OFFLOAD

    double* xhost = new double[4 * N];
    for (int i = 0; i < 4 * N; i++)
    {
        xhost[i] = -1.;
    }

    // Host solve
    {
        short* nits = new short[N];
        auto t1     = Clock::now();

#pragma omp parallel for
        for (int i = 0; i < N; i++)
        {
#ifdef _OPENMP
            if (!omp_is_initial_device()) abort();
#endif
            xhost[4 * i]     = sol[0];
            xhost[4 * i + 1] = sol[1];
            xhost[4 * i + 2] = sol[2];
            xhost[4 * i + 3] = sol[3];
            double hphi      = 0.5 + (i % 100) * deviation;
            double c0        = 0.33;
            double c1        = 0.33;
            Thermo4PFM::CALPHADConcSolverTernary solver;
            solver.setup(c0, c1, hphi, RTinv, L_AB_L, L_AC_L, L_BC_L, L_AB_S,
                L_AC_S, L_BC_S, L_ABC_L, L_ABC_S, fA, fB, fC);
            nits[i] = solver.ComputeConcentration(&xhost[4 * i], 1.e-8, 50);
        }
        auto t2 = Clock::now();
        long int usec
            = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1)
                  .count();
        std::cout << "Host time/us/solve:   " << (double)usec / (double)N
                  << std::endl;

        std::cout << std::setprecision(12);
        int n = N > 20 ? 20 : N;
        for (int i = 0; i < n; i++)
        {
            std::cout << "Host: x=" << xhost[4 * i] << "," << xhost[4 * i + 1]
                      << "," << xhost[4 * i + 2] << "," << xhost[4 * i + 3]
                      << std::endl;
            std::cout << "nits=" << nits[i] << std::endl;
        }
        delete[] nits;
    }
    delete[] xhost;
#else
    double* xdev = new double[4 * N];
    short* nits  = new short[N];

    // Device solve
    {
        for (int i = 0; i < 4 * N; i++)
        {
            xdev[i] = -1;
        }

        // warm-up GPU with a dummy allocation
        double dummy[N];
#pragma omp target enter data map(alloc : dummy[:N])

        auto t1 = Clock::now();

// clang-format off
#pragma omp target map(from : xdev[:4*N]) \
                   map(from : nits[:N])
{
#pragma omp teams distribute parallel for
            // clang-format on
            for (int i = 0; i < N; i++)
            {
                // if( omp_is_initial_device() ) abort();
                xdev[4 * i]     = sol[0];
                xdev[4 * i + 1] = sol[1];
                xdev[4 * i + 2] = sol[2];
                xdev[4 * i + 3] = sol[3];

                double hphi = 0.5 + (i % 100) * deviation;
                double c0   = 0.33;
                double c1   = 0.33;
                class Thermo4PFM::CALPHADConcSolverTernary solver;
                solver.setup(c0, c1, hphi, RTinv, L_AB_L, L_AC_L, L_BC_L,
                    L_AB_S, L_AC_S, L_BC_S, L_ABC_L, L_ABC_S, fA, fB, fC);
                nits[i] = solver.ComputeConcentration(&xdev[4 * i], 1.e-8, 50);
            }
        }

        auto t2 = Clock::now();

        long int usec
            = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1)
                  .count();
        std::cout << "Device time/us/solve: " << (double)usec / (double)N
                  << std::endl;

#pragma omp target exit data map(delete : dummy[:N])

        // print out some results
        std::cout << std::setprecision(12);
        int n = N > 20 ? 20 : N;
        for (int i = 0; i < n; i++)
        {
            std::cout << "Dev: x=" << xdev[4 * i] << "," << xdev[4 * i + 1]
                      << "," << xdev[4 * i + 2] << "," << xdev[4 * i + 3]
                      << std::endl;
            std::cout << "nits=" << nits[i] << std::endl;
        }

        // verify results
        double tol = 0.03;
        int count  = 0;
        for (int i = 0; i < N; i++)
        {
            if ((xdev[4 * i + 1] != xdev[4 * i + 1])
                || std::abs(xdev[4 * i] - 0.33) > tol
                || std::abs(xdev[4 * i + 1] - 0.33) > tol
                || std::abs(xdev[4 * i + 2] - 0.33) > tol
                || std::abs(xdev[4 * i + 3] - 0.33) > tol)
            {
                std::cout << "Device: x=" << xdev[4 * i] << ","
                          << xdev[4 * i + 1] << "," << xdev[4 * i + 2] << ","
                          << xdev[4 * i + 3] << std::endl;
                std::cout << "Difference: " << xdev[4 * i] - 0.33 << ", "
                          << xdev[4 * i + 1] - 0.33 << ", "
                          << xdev[4 * i + 2] - 0.33 << ", "
                          << xdev[4 * i + 3] - 0.33 << std::endl;
                std::cout << "nits[" << i << "]=" << nits[i] << std::endl;
                count++;
            }
            if (count > 20) break;
        }
    }

    delete[] xdev;
    delete[] nits;
#endif
}