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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// 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_EXTENSIONS_TRIANGULATION_GEOMETRIES_TRIANGULATION_HPP
#define BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_GEOMETRIES_TRIANGULATION_HPP

#include <vector>

#include <boost/range.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/iterator/iterator_categories.hpp>
#include <boost/iterator/indirect_iterator.hpp>

#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/views/detail/points_view.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/geometries/concepts/point_concept.hpp>

namespace boost { namespace geometry
{
template<typename Triangulation>
struct face_range_type {};

template<typename Triangulation>
typename face_range_type<Triangulation>::type
    face_range(Triangulation const& t);

template<typename Triangulation>
struct triangulation_face {};

namespace model
{

template
<
    typename Point,
    bool ClockWise = true,
    template<typename, typename> class VertexContainer = std::vector,
    template<typename, typename> class FaceContainer = std::vector,
    template<typename> class VertexAllocator = std::allocator,
    template<typename> class FaceAllocator = std::allocator
>
class triangulation;

template<typename Triangulation>
struct vertex_ref
{
    typename Triangulation::point_type m_p;
    typename Triangulation::face_iterator m_f;
};

template<typename Triangulation>
struct face_ref
{
    typedef typename Triangulation::point_type point_type;
    typedef typename Triangulation::vertex_iterator vertex_iterator;
    typedef typename Triangulation::face_iterator face_iterator;
    typedef std::array<vertex_iterator, 3> vertex_container;
    vertex_container m_v;
    std::array<face_iterator, 3> m_f;
    std::array<unsigned short, 3> m_o;
public:
    typedef boost::indirect_iterator<typename vertex_container::const_iterator>
        const_iterator;
    typedef boost::indirect_iterator<typename vertex_container::iterator>
        iterator;

    const_iterator begin() const { return const_iterator(m_v.begin()); }
    const_iterator end() const { return const_iterator(m_v.end()); }
};

template
<
    typename Value,
    template<typename, typename> class Container,
    template<typename> class Allocator
>
struct reserve_if_vector{
    static void apply(Container<Value, Allocator<Value>>& c, std::size_t n) {}
};

template<typename Value, template<typename> class Allocator>
struct reserve_if_vector<Value, std::vector, Allocator>{
    static void apply(std::vector<Value, Allocator<Value>>& c,
                      std::size_t n) { c.reserve(n);}
};

template
<
    typename Point,
    bool ClockWise,
    template<typename, typename> class VertexContainer,
    template<typename, typename> class FaceContainer,
    template<typename> class VertexAllocator,
    template<typename> class FaceAllocator
>
class triangulation
{
private:
    BOOST_CONCEPT_ASSERT( (concepts::Point<Point>) );
public:
    typedef Point point_type;
    typedef face_ref<triangulation> face_type;
    typedef vertex_ref<triangulation> vertex_type;
    typedef unsigned short face_vertex_index;
    typedef VertexContainer<vertex_type, VertexAllocator<vertex_type>>
        vertex_container;
    typedef FaceContainer<face_type, FaceAllocator<face_type>> face_container;
    typedef typename face_container::iterator face_iterator;
    typedef typename face_container::const_iterator const_face_iterator;
    typedef typename vertex_container::iterator vertex_iterator;
    typedef typename vertex_container::const_iterator const_vertex_iterator;
    typedef typename coordinate_type<Point>::type coordinate_type;
    typedef typename model::segment<Point> segment_type;

    static const order_selector point_order =
        ClockWise ? clockwise : counterclockwise;
    struct halfedge_index
    {
        halfedge_index(face_iterator f, face_vertex_index v):m_f(f), m_v(v) {}
        face_iterator m_f;
        face_vertex_index m_v;
    };

    struct fulledge_index
    {
        fulledge_index(face_iterator f1,
                       face_vertex_index v1,
                       face_iterator f2,
                       face_vertex_index v2)
            : m_f1(f1),
              m_f2(f2),
              m_v1(v1),
              m_v2(v2) {}
        fulledge_index(halfedge_index e)
            : m_f1(e.m_f),
              m_f2(e.m_f->m_f[ e.m_v ]),
              m_v1(e.m_v),
              m_v2(e.m_f->m_o[ e.m_v ]) {}
        face_iterator m_f1, m_f2;
        face_vertex_index m_v1, m_v2;
    };

    static face_iterator invalid() { return face_iterator(); }
    triangulation(std::size_t points = 3)
    {
        reserve_if_vector<vertex_type, VertexContainer, VertexAllocator>::
            apply(m_vertices, points);
        reserve_if_vector<face_type, FaceContainer, FaceAllocator>::
            apply(m_faces, 2 * points - 5);
    }

    triangulation(std::size_t points, std::size_t faces)
    {
        reserve_if_vector<vertex_type, VertexContainer, VertexAllocator>::
            apply(m_vertices, points);
        reserve_if_vector<face_type, FaceContainer, FaceAllocator>::
            apply(m_faces, faces);
    }

    template <typename InputIt>
    triangulation(InputIt begin, InputIt end)
    {
        m_vertices.assign(begin, end);
        reserve_if_vector<face_type, FaceContainer, FaceAllocator>
            ::apply(m_faces, 2 * m_vertices.size() - 5);
    }

    typename vertex_container::iterator vertices_begin()
    {
        return m_vertices.begin();
    }

    typename vertex_container::const_iterator vertices_begin() const
    {
        return m_vertices.cbegin();
    }

    typename vertex_container::iterator vertices_end()
    {
        return m_vertices.end();
    }

    typename vertex_container::const_iterator vertices_end() const
    {
        return m_vertices.cend();
    }

    vertex_iterator add_vertex(const Point& p)
    {
        return m_vertices.insert(m_vertices.end(), vertex_type{p, invalid()});
    }

    face_container const& face_range() const
    {
        return m_faces;
    }

    vertex_container const& vertex_range() const
    {
        return m_vertices;
    }

    typename face_container::const_iterator faces_cbegin() const
    {
        return m_faces.cbegin();
    }

    typename face_container::iterator faces_begin()
    {
        return m_faces.begin();
    }

    typename face_container::const_iterator faces_cend() const
    {
        return m_faces.cend();
    }

    typename face_container::iterator faces_end()
    {
        return m_faces.end();
    }

    template <typename InputIt>
    void assign_vertices(InputIt begin, InputIt end)
    {
        m_vertices.assign(begin, end);
        m_faces.reserve(2 * m_vertices.size() - 5);
    }

    static Point const& face_vertex(face_iterator f, face_vertex_index v)
    {
        return f -> m_v[ v ] -> m_p;
    }

    static segment_type face_segment(halfedge_index e)
    {
        return segment_type(
            face_vertex(e.m_f, (e.m_v == 2 ? 0 : e.m_v + 1)),
            face_vertex(e.m_f, (e.m_v == 0 ? 2 : e.m_v - 1)) );
    }

    static Point& vertex(vertex_iterator v)
    {
        return v -> m_p;
    }

    static face_iterator neighbour(face_iterator f, unsigned short v)
    {
        return f -> m_f[ v ];
    }

    static const_face_iterator neighbour(const_face_iterator f,
                                         unsigned short v)
    {
        return f -> m_f[ v ];
    }

    static face_vertex_index opposite(face_iterator f, unsigned short v)
    {
        return f -> m_o[ v ];
    }

    static halfedge_index opposite(halfedge_index const& e)
    {
        return halfedge_index{ e.m_f -> m_f[ e.m_v ], e.m_f -> m_o[ e.m_v ] };
    }

    static halfedge_index next(halfedge_index const& e)
    {
        return halfedge_index{
            e.m_f,
            static_cast<unsigned short>(e.m_v == 2 ? 0 : e.m_v + 1) };
    }

    static halfedge_index prev(halfedge_index const& e)
    {
        return halfedge_index{
            e.m_f,
            static_cast<unsigned short>(e.m_v == 0 ? 2 : e.m_v - 1)};
    }

    vertex_iterator boundary_vertex() const
    {
        return m_boundary_vertex;
    }

    static vertex_iterator boundary_next(vertex_iterator v)
    {
        face_iterator fi = v -> m_f;
        if(fi -> m_v[ 0 ] == v )
            return fi -> m_v[ 1 ];
        else if(fi -> m_v[ 1 ] == v)
            return fi -> m_v[ 2 ];
        else
            return fi -> m_v[ 0 ];
    }

    vertex_iterator boundary_prev(vertex_iterator v) const
    {
        face_iterator fi = v -> m_f;
        unsigned short vi;
        if(fi -> m_v[ 0 ] == v) vi = 0;
        else if(fi -> m_v[ 1 ] == v) vi = 1;
        else vi = 2;
        if(m_faces.size() == 1)
            return vi == 0 ? fi -> m_v[ 2 ] : fi -> m_v[ vi - 1 ];
        halfedge_index e = next(halfedge_index{fi, vi});
        while( opposite(e).m_f != invalid() )
        {
            e = prev(opposite(e));
        }
        return e.m_f -> m_v[ e.m_v == 2 ? 0 : e.m_v + 1 ];
    }

    void clear()
    {
        m_vertices.clear();
        m_faces.clear();
    }

    std::size_t vertices() const
    {
        return m_vertices.size();
    }

    std::size_t faces() const
    {
        return m_faces.size();
    }

    static void flip(const halfedge_index& e)
    {
        face_iterator fi1 = e.m_f;
        face_type& f1 = *fi1;
        face_iterator fi2 = f1.m_f[ e.m_v ];
        face_type& f2 = *fi2;
        unsigned short const& v1 = e.m_v;
        unsigned short const v2 = f1.m_o[ v1 ];

        if( f1.m_v[ v1 == 0 ? 2 : v1 - 1 ] -> m_f == fi1 )
            f1.m_v[ v1 == 0 ? 2 : v1 - 1 ] -> m_f = fi2;
        if( f2.m_v[ v2 == 0 ? 2 : v2 - 1  ]-> m_f == fi2)
            f2.m_v[ v2 == 0 ? 2 : v2 - 1  ]-> m_f = fi1;
        f1.m_v[ v1 == 0 ? 2 : v1 - 1 ] = f2.m_v[ v2 ];
        f2.m_v[ v2 == 0 ? 2 : v2 - 1 ] = f1.m_v[ v1 ];

        f1.m_f[v1] = f2.m_f[ v2 == 2 ? 0 : v2 + 1 ];
        f1.m_o[v1] = f2.m_o[ v2 == 2 ? 0 : v2 + 1 ];
        if(f1.m_f[v1] != invalid()) {
            f1.m_f[v1] -> m_f[ f2.m_o[ v2 == 2 ? 0 : v2 + 1 ] ] = fi1;
            f1.m_f[v1] -> m_o[ f2.m_o[ v2 == 2 ? 0 : v2 + 1 ] ] = v1;
        }
        f2.m_f[v2] = f1.m_f[ v1 == 2 ? 0 : v1 + 1 ];
        f2.m_o[v2] = f1.m_o[ v1 == 2 ? 0 : v1 + 1 ];
        if(f2.m_f[v2] != invalid()) {
            f2.m_f[v2] -> m_f[ f1.m_o[ v1 == 2 ? 0 : v1 + 1 ] ] = fi2;
            f2.m_f[v2] -> m_o[ f1.m_o[ v1 == 2 ? 0 : v1 + 1 ] ] = v2;
        }
        f1.m_f[ v1 == 2 ? 0 : v1 + 1 ] = fi2;
        f1.m_o[ v1 == 2 ? 0 : v1 + 1 ] = v2 == 2 ? 0 : v2 + 1;
        f2.m_f[ v2 == 2 ? 0 : v2 + 1 ] = fi1;
        f2.m_o[ v2 == 2 ? 0 : v2 + 1 ] = v1 == 2 ? 0 : v1 + 1;
    }

    face_iterator add_face_on_boundary(halfedge_index e, vertex_iterator v)
    {
        const face_iterator f = e.m_f;
        const face_vertex_index adj = e.m_v;
        f -> m_o[adj] = 0;
        m_boundary_vertex = v;
        face_iterator pos = m_faces.insert(m_faces.end(),
            face_type{ {{v, f->m_v[ adj == 0 ? 2 : adj - 1 ],
                         f->m_v[ adj == 2 ? 0 : adj + 1 ] }},
                {{f, invalid(), invalid()}},
                {{adj, 4, 4}}});
        f -> m_f[adj] = pos;
        v -> m_f = pos;
        m_faces.back().m_v[ 2 ]->m_f = pos;
        return pos;
    }

    fulledge_index next_around_vertex(fulledge_index e)
    {
        if(e.m_f2 == invalid()) {
            face_vertex_index left_vi = (e.m_v1 == 0 ? 2 : e.m_v1 - 1);
            vertex_iterator left_v = e.m_f1->m_v[ left_vi ];
            face_iterator next_f = left_v->m_f;
            face_vertex_index next_vi =
                (next_f->m_v[ 0 ] == left_v) ? 2 :
                ((next_f->m_v[ 1 ] == left_v) ? 0 : 1);
            return fulledge_index(invalid(), 4, next_f, next_vi);
        } else {
            return fulledge_index(halfedge_index(e.m_f2,
                                                 e.m_v2 == 0 ? 2 : e.m_v2 - 1));
        }
    }

    static fulledge_index begin_vertex_edge(vertex_iterator vi)
    {
        face_iterator first_f = vi -> m_f;
        face_vertex_index next_vi =
            (first_f->m_v[ 0 ] == vi) ? 2 :
            ((first_f->m_v[ 1 ] == vi) ? 0 : 1);
        return fulledge_index(invalid(), 4, first_f, next_vi);
    }

    face_iterator add_isolated_face(vertex_iterator v1,
                                    vertex_iterator v2,
                                    vertex_iterator v3)
    {
        m_boundary_vertex = v1;
        face_iterator pos = m_faces.insert( m_faces.end(),
            face_type{
                {{ v1, v2, v3 }},
                {{ invalid(), invalid(), invalid() }},
                {{4, 4, 4}} } );
        v1 -> m_f = v2 -> m_f = v3 -> m_f = pos;
        return pos;
    }

    static halfedge_index face_edge(face_iterator f, face_vertex_index v = 0)
    {
        return halfedge_index{f, v};
    }

    static void connect(halfedge_index e1, halfedge_index e2)
    {
        face_iterator f1 = e1.m_f;
        face_iterator f2 = e2.m_f;
        unsigned short& v1 = e1.m_v;
        unsigned short& v2 = e2.m_v;
        f1 -> m_f[ v1 ] = f2;
        f1 -> m_o[ v1 ] = v2;
        f2 -> m_f[ v2 ] = f1;
        f2 -> m_o[ v2 ] = v1;
        if(f1 -> m_v[ (v1 == 2 ? 0 : v1 + 1) ] -> m_f == f1) {
            f1 -> m_v[ (v1 == 2 ? 0 : v1 + 1) ] -> m_f = f2;
        }
        if(f2 -> m_v[ (v2 == 2 ? 0 : v2 + 1) ] -> m_f == f2) {
            f2 -> m_v[ (v2 == 2 ? 0 : v2 + 1) ] -> m_f = f1;
        }
    }

    bool valid() const
    {
        bool valid = true;
        for(const_face_iterator fi = m_faces.begin();
            fi != m_faces.end();
            ++fi)
        {
            face_type const& f = *fi;
            for(unsigned short v = 0 ; v < 3 ; ++v)
            {
                if(f.m_f[ v ] == invalid())
                    continue;
                face_vertex_index const& o = f.m_o[ v ];
                valid = valid && (f.m_f[ v ]->m_o[ o ] == v);
                if(!valid) {
                    return false;
                }
                valid = valid && (f.m_f[ v ]->m_f[ o ] == fi);
                if(!valid) {
                    return false;
                }
                valid = valid
                    && (f.m_v[ (v + 1) % 3 ] == f.m_f[ v ]->m_v[ (o + 2) % 3 ])
                    && (f.m_v[ (v + 2) % 3 ] == f.m_f[ v ]->m_v[ (o + 1) % 3 ]);
                if(!valid) {
                    return false;
                }
                if(f.m_o[v] == 4) {
                    unsigned short next = (v + 1) % 3;
                    valid = valid && f.m_v[ next ]->m_f == fi;
                    if(!valid) {
                        return false;
                    }

                }
            }
        }
        for(const_vertex_iterator vi = m_vertices.cbegin();
            vi != m_vertices.cend();
            ++vi)
        {
            vertex_type const& v = *vi;
            if(v.m_f == invalid()) continue;

            bool found = false;
            for(face_vertex_index vj = 0; vj < 3 ; ++vj)
            {
                found = found || (v.m_f -> m_v[ vj ] == vi);
            }
            valid = valid && found;
            if(!valid) {
                return false;
            }
        }
        return valid;
    }
private:
    vertex_container m_vertices;
    face_container m_faces;
    vertex_iterator m_boundary_vertex;
};

template< typename Point >
struct edge_ref
{
    typename triangulation<Point>::halfedge_index m_e;
    triangulation<Point>& m_t;
};

template< typename Point >
using triangulation_face_range = typename triangulation<Point>::face_container;

template< typename Point >
using triangulation_vertex_range =
    typename triangulation<Point>::vertex_container;

} // namespace model

#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS
namespace traits
{

template<typename Triangulation>
struct tag< model::vertex_ref<Triangulation> >
{ typedef point_tag type; };

template<typename Triangulation>
struct coordinate_type< model::vertex_ref<Triangulation> >
{
    typedef typename coordinate_type<typename Triangulation::point_type>
        ::type type;
};

template<typename Triangulation>
struct dimension< model::vertex_ref<Triangulation> > : boost::mpl::int_<2> {};

template<typename Triangulation>
struct coordinate_system< model::vertex_ref<Triangulation> >
{
    typedef typename coordinate_system<typename Triangulation::point_type>
        ::type type;
};

template<typename Triangulation, std::size_t Dimension>
struct access<model::vertex_ref<Triangulation>, Dimension>
{
    static typename coordinate_type<typename Triangulation::point_type>::type
        get(model::vertex_ref<Triangulation> const& p)
    {
        return boost::geometry::get<Dimension>(p.m_p);
    }
};

template<typename Point> struct tag< model::edge_ref<Point> >
{ typedef segment_tag type; };

template<typename Point> struct point_type< model::edge_ref<Point> >
{ typedef Point type; };

template<typename Point, std::size_t Dimension>
struct indexed_access<model::edge_ref<Point>, 0, Dimension>
{
    static typename coordinate_type<Point>::type
        get(model::edge_ref<Point> const& p)
    {
        return get<Dimension>(
            p.m_t.face_vertex(p.m_e.m_f, (p.m_e.m_v == 2 ? 0 : p.m_e.m_v + 1))
        );
    }
};

template<typename Point, std::size_t Dimension>
struct indexed_access<model::edge_ref<Point>, 1, Dimension>
{
    static typename coordinate_type<Point>::type
        get(model::edge_ref<Point> const& p)
    {
        return get<Dimension>(
            p.m_t.face_vertex(p.m_e.m_f, (p.m_e.m_v == 0 ? 2 : p.m_e.m_v - 1)));
    }
};

template<typename Triangulation>
struct tag<model::face_ref<Triangulation>>
{ typedef ring_tag type; };

template<typename Triangulation>
struct point_order<model::face_ref<Triangulation>>
{ static const order_selector value = Triangulation::point_order; };

template<typename Triangulation>
struct closure<model::face_ref<Triangulation>>
{ static const closure_selector value = open; };

} // namespace traits
#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

template<typename Point>
struct face_range_type<model::triangulation<Point>> {
    typedef typename model::triangulation_face_range<Point> type;
};

template
<
    typename Point,
    bool ClockWise,
    template<typename, typename> class VertexContainer,
    template<typename, typename> class FaceContainer,
    template<typename> class VertexAllocator,
    template<typename> class FaceAllocator
>
inline typename model::triangulation
<
    Point,
    ClockWise,
    VertexContainer,
    FaceContainer,
    VertexAllocator,
    FaceAllocator
>::vertex_container const& vertex_range(
    model::triangulation<Point, ClockWise, VertexContainer, FaceContainer,
        VertexAllocator, FaceAllocator> const& t)
{ return t.vertex_range(); }

template
<
    typename Point,
    bool ClockWise,
    template<typename, typename> class VertexContainer,
    template<typename, typename> class FaceContainer,
    template<typename> class VertexAllocator,
    template<typename> class FaceAllocator
>
inline typename model::triangulation
<
    Point,
    ClockWise,
    VertexContainer,
    FaceContainer,
    VertexAllocator,
    FaceAllocator
>::face_container const& face_range(
    model::triangulation<Point, ClockWise, VertexContainer, FaceContainer,
        VertexAllocator, FaceAllocator> const& t)
{ return t.face_range(); }

template
<
    typename Point,
    bool ClockWise,
    template<typename, typename> class VertexContainer,
    template<typename, typename> class FaceContainer,
    template<typename> class VertexAllocator,
    template<typename> class FaceAllocator,
    typename FaceIterator,
    typename OutputIterator
>
inline void face_adjacent_range(
    model::triangulation<Point, ClockWise, VertexContainer, FaceContainer,
        VertexAllocator, FaceAllocator>& t,
    FaceIterator fi,
    OutputIterator out)
{
    typedef typename model::triangulation<Point, ClockWise, VertexContainer,
        FaceContainer, VertexAllocator, FaceAllocator > triangulation;
    typedef typename triangulation::face_iterator face_iterator;
    face_iterator const invalid = t.invalid();
    if(fi -> m_f[ 0 ] != invalid ) *out++ = fi->m_f[ 0 ];
    if(fi -> m_f[ 1 ] != invalid ) *out++ = fi->m_f[ 1 ];
    if(fi -> m_f[ 2 ] != invalid ) *out++ = fi->m_f[ 2 ];
}

template
<
    typename Point,
    bool ClockWise,
    template<typename, typename> class VertexContainer,
    template<typename, typename> class FaceContainer,
    template<typename> class VertexAllocator,
    template<typename> class FaceAllocator,
    typename FaceIterator,
    typename OutputIterator
>
inline void face_incident_faces(
    model::triangulation<Point, ClockWise, VertexContainer, FaceContainer,
        VertexAllocator, FaceAllocator> & t,
    FaceIterator fi,
    OutputIterator out)
{
    typedef typename model::triangulation<Point, ClockWise, VertexContainer,
        FaceContainer, VertexAllocator, FaceAllocator> triangulation;
    typedef typename triangulation::face_iterator face_iterator;
    typedef typename triangulation::vertex_iterator vertex_iterator;
    typedef typename triangulation::face_vertex_index face_vertex_index;
    typedef typename triangulation::face_type face_type;
    face_iterator const invalid = t.invalid();
    for(face_vertex_index i = 0; i < 3; ++i)
    {
        face_iterator n = fi -> m_f[i];
        face_iterator m = fi -> m_f[i == 2 ? 0 : i + 1];
        face_iterator f_prev = fi;
        face_vertex_index v_prev = i;
        if(n != invalid) {
            f_prev = n;
            v_prev = fi -> m_o[i];
            v_prev = (v_prev == 0 ? 2 : v_prev - 1);
        }
        while(true)
        {
            face_iterator next = f_prev -> m_f[v_prev];
            if(next == invalid) {
                face_vertex_index j = (i == 2 ? 0 : i + 1);
                face_iterator m = fi -> m_f[j];
                if(m == invalid) break;
                face_vertex_index prev_vertex_index = (i == 0 ? 2 : i - 1 );
                vertex_iterator const& prev_vertex_it =
                    fi->m_v[ prev_vertex_index ];
                face_type& first = *prev_vertex_it->m_f;
                *out++ = prev_vertex_it->m_f;
                f_prev = next = prev_vertex_it->m_f;
                if(first.m_v[0] == prev_vertex_it) v_prev = 1;
                else if(first.m_v[1] == prev_vertex_it) v_prev = 2;
                else v_prev = 0;
                continue;
            } else {
                if(&(*next) == &(*fi)) break;
                *out++ = next;
                v_prev = f_prev -> m_o[v_prev];
                v_prev = (v_prev == 0 ? 2 : v_prev - 1);
                f_prev = next;
            }
        }
    }
}

template
<
    typename Point,
    bool ClockWise,
    template<typename, typename> class VertexContainer,
    template<typename, typename> class FaceContainer,
    template<typename> class VertexAllocator,
    template<typename> class FaceAllocator,
    typename OutputIterator
>
inline void vertex_incident_faces(
    model::triangulation<Point, ClockWise, VertexContainer, FaceContainer,
        VertexAllocator, FaceAllocator> & t,
    typename model::triangulation<Point, ClockWise, VertexContainer,
        FaceContainer, VertexAllocator, FaceAllocator>::vertex_iterator vi,
    OutputIterator out)
{
    typedef typename model::triangulation<Point, ClockWise, VertexContainer,
        FaceContainer, VertexAllocator, FaceAllocator> triangulation;
    typedef typename triangulation::face_iterator face_iterator;
    typedef typename triangulation::fulledge_index fulledge_index;
    fulledge_index e = t.begin_vertex_edge(vi);
    face_iterator first_face = e.m_f2;
    *out++ = first_face;
    while(true) {
        e = t.next_around_vertex(e);
        if(e.m_f2 == t.invalid() || e.m_f2 == first_face) break;
        *out++ = e.m_f2;
    }
}

template
<
    typename Point,
    bool ClockWise,
    template<typename, typename> class VertexContainer,
    template<typename, typename> class FaceContainer,
    template<typename> class VertexAllocator,
    template<typename> class FaceAllocator,
    typename OutputIterator
>
inline void vertex_incident_vertices(
    model::triangulation<Point, ClockWise, VertexContainer, FaceContainer,
        VertexAllocator, FaceAllocator> & t,
    typename model::triangulation<Point, ClockWise, VertexContainer,
        FaceContainer, VertexAllocator, FaceAllocator>::vertex_iterator vi,
    OutputIterator out)
{
    typedef model::triangulation<Point, ClockWise, VertexContainer,
        FaceContainer, VertexAllocator, FaceAllocator> triangulation_type;
    typedef typename triangulation_type::face_iterator face_iterator;
    typedef typename triangulation_type::fulledge_index fulledge_index;
    fulledge_index e = t.begin_vertex_edge(vi);
    face_iterator first_face = e.m_f2;
    *out++ = e.m_f2 -> m_v[ e.m_v2 == 0 ? 2 : e.m_v2 - 1 ];
    while(true) {
        e = t.next_around_vertex(e);
        if(e.m_f2 == t.invalid() || e.m_f2 == first_face) break;
        *out++ = e.m_f2 -> m_v[ e.m_v2 == 0 ? 2 : e.m_v2 - 1 ];
    }
}

} // namespace geometry

} // namespace boost

#endif // BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_GEOMETRIES_TRIANGULATION_HPP


#include <boost/hana.hpp>
#include <iostream>
#include <cassert>
#include <type_traits>

namespace hana = boost::hana;
using namespace hana::literals;

template <typename T>
constexpr auto is_unique(T x)
{
    return hana::unique(x) == x;
}

static_assert(is_unique(hana::tuple_t<>), "");
static_assert(is_unique(hana::tuple_t<int>), "");
static_assert(!is_unique(hana::tuple_t<int, int>), "");

enum class setting0 : int
{
    red = 0,
    blue = 1,
    green = 2
};

enum class setting1 : int
{
    single = 0,
    multiple = 1,
    mixed = 2
};

enum class setting2 : int
{
    on = 0,
    off = 1
};

template <typename... Ts>
struct settings_set : hana::tuple<Ts...>
{
    template <typename T>
    constexpr auto get()
    {
        return static_cast<T>(decltype(hana::make_map(

            hana::make_pair(hana::type_c<typename Ts::enum_type>,
                typename Ts::ic_value{})...)[hana::type_c<T>])::value);
        /*
        return hana::find_if(hana::tuple<Ts...>{}, [](auto a)
            {
                return hana::bool_c<
                    std::is_same<typename decltype(a)::enum_type, T>{}>;
            });
*/
    }
};

template <typename... Ts>
constexpr auto make_settings(Ts...)
{
    static_assert(is_unique(hana::tuple_t<Ts...>), "");
    return settings_set<Ts...>{};
}

template <typename T>
constexpr auto val(T t)
{
    return static_cast<std::underlying_type_t<T>>(t);
}

template <typename T, T TV>
struct enum_w
{
    using enum_type = T;
    using ic_value = std::integral_constant<std::underlying_type_t<T>,
        (std::underlying_type_t<T>)TV>;
};

int main()
{
// static_assert(setting1::single == setting1::single, "");
// static_assert((int)setting1::single == 0, "");
// static_assert(get_inner_value<setting1, setting1::single>() == 0, "");
// static_assert(
//    get_inner_value_helper(setting1::single) == 0, "");
/*
    static_assert(std::is_same<decltype(setting1::single), setting1>{}, "");

    using test_w = enum_w<setting1, setting1::single>;
    static_assert(std::is_same<test_w::enum_type, setting1>{}, "");
    static_assert(
        std::is_same<test_w::ic_value, std::integral_constant<int, 0>>{},
   "");
*/

#define FCK(x)             \
    enum_w<decltype(x), x> \
    {                      \
    }

    auto s = make_settings(
        FCK(setting1::single), FCK(setting0::red), FCK(setting2::off));

    static_assert(s.get<setting1>() == setting1::single, "");
    static_assert(s.get<setting1>() != setting1::mixed, "");
    // static_assert(s.get<setting1>() == hana::nothing, "");

    // using f2 = enum_ic2<setting1::single>;

    // auto s = make_settings(setting1::single, setting0::red,
    // setting2::off);
}


#include "CALPHADFreeEnergyFunctionsTernary.h"
#include "CALPHADConcSolverTernary.h"
#include "CALPHADEqConcSolverTernary.h"
#include "CALPHADFunctions.h"
#include "CALPHADTieLineConcSolverTernary.h"
#include "PhysicalConstants.h"
#include "datatypes.h"
#include "functions.h"
#include "well_functions.h"

#include <boost/property_tree/json_parser.hpp>

#include <cmath>
#include <iomanip>
#include <string>

namespace pt = boost::property_tree;

namespace Thermo4PFM
{

void readLmixTernaryParameters(
    pt::ptree& Lmix_db, CalphadDataType LmixABC[3][2])
{
    // L0
    {
        auto child = Lmix_db.get_child_optional("L0");
        if (child)
        {
            int i = 0;
            for (pt::ptree::value_type& v : Lmix_db.get_child("L0"))
            {
                LmixABC[0][i] = v.second.get_value<CalphadDataType>();
                i++;
            }
        }
    }
    // L1
    {
        auto child = Lmix_db.get_child_optional("L1");
        if (child)
        {
            int i = 0;
            for (pt::ptree::value_type& v : Lmix_db.get_child("L1"))
            {
                LmixABC[1][i] = v.second.get_value<CalphadDataType>();
                i++;
            }
        }
    }
    // L2
    {
        auto child = Lmix_db.get_child_optional("L2");
        if (child)
        {
            int i = 0;
            for (pt::ptree::value_type& v : Lmix_db.get_child("L2"))
            {
                LmixABC[2][i] = v.second.get_value<CalphadDataType>();
                i++;
            }
        }
    }
}

CALPHADFreeEnergyFunctionsTernary::CALPHADFreeEnergyFunctionsTernary(
    pt::ptree& calphad_db, boost::optional<pt::ptree&> newton_db,
    const EnergyInterpolationType energy_interp_func_type,
    const ConcInterpolationType conc_interp_func_type)
    : energy_interp_func_type_(energy_interp_func_type),
      conc_interp_func_type_(conc_interp_func_type),
      newton_tol_(1.e-8),
      newton_alpha_(1.),
      newton_maxits_(20),
      newton_verbose_(false)

{
    std::string fenergy_diag_filename("energy.vtk");
    fenergy_diag_filename_ = new char[fenergy_diag_filename.length() + 1];
    strcpy(fenergy_diag_filename_, fenergy_diag_filename.c_str());

    readParameters(calphad_db);

    if (newton_db) readNewtonparameters(newton_db.get());
}

//=======================================================================

void CALPHADFreeEnergyFunctionsTernary::readNewtonparameters(
    pt::ptree& newton_db)
{
    newton_tol_     = newton_db.get<double>("tol", newton_tol_);
    newton_alpha_   = newton_db.get<double>("alpha", newton_alpha_);
    newton_maxits_  = newton_db.get<int>("max_its", newton_maxits_);
    newton_verbose_ = newton_db.get<bool>("verbose", newton_verbose_);
}

//=======================================================================

void CALPHADFreeEnergyFunctionsTernary::readParameters(pt::ptree& calphad_db)
{
    pt::ptree& species0_db = calphad_db.get_child("SpeciesA");
    std::string name       = species0_db.get<std::string>("name", "unknown");
    std::string dbnameL("PhaseL");
    g_species_phaseL_[0].initialize(name, species0_db.get_child(dbnameL));
    std::string dbnameA("PhaseA");
    g_species_phaseA_[0].initialize(name, species0_db.get_child(dbnameA));

    pt::ptree& speciesB_db = calphad_db.get_child("SpeciesB");
    name                   = speciesB_db.get<std::string>("name", "unknown");
    g_species_phaseL_[1].initialize(name, speciesB_db.get_child(dbnameL));
    g_species_phaseA_[1].initialize(name, speciesB_db.get_child(dbnameA));

    pt::ptree& speciesC_db = calphad_db.get_child("SpeciesC");
    name                   = speciesC_db.get<std::string>("name", "unknown");
    g_species_phaseL_[2].initialize(name, speciesC_db.get_child(dbnameL));
    g_species_phaseA_[2].initialize(name, speciesC_db.get_child(dbnameA));

    // read Lmix coefficients

    // AB
    {
        pt::ptree& Lmix0_db = calphad_db.get_child("LmixABPhaseL");
        readLmixBinary(Lmix0_db, LmixABPhaseL_);

        pt::ptree& Lmix1_db = calphad_db.get_child("LmixABPhaseA");
        readLmixBinary(Lmix1_db, LmixABPhaseA_);
    }

    // AC
    {
        pt::ptree& Lmix0_db = calphad_db.get_child("LmixACPhaseL");
        readLmixBinary(Lmix0_db, LmixACPhaseL_);

        pt::ptree& Lmix1_db = calphad_db.get_child("LmixACPhaseA");
        readLmixBinary(Lmix1_db, LmixACPhaseA_);
    }

    // BC
    {
        pt::ptree& Lmix0_db = calphad_db.get_child("LmixBCPhaseL");
        readLmixBinary(Lmix0_db, LmixBCPhaseL_);

        pt::ptree& Lmix1_db = calphad_db.get_child("LmixBCPhaseA");
        readLmixBinary(Lmix1_db, LmixBCPhaseA_);
    }

    // ABC
    {
        // default values
        LmixABCPhaseL_[0][0] = 0.0;
        LmixABCPhaseL_[0][1] = 0.0;
        LmixABCPhaseL_[1][0] = 0.0;
        LmixABCPhaseL_[1][1] = 0.0;
        LmixABCPhaseL_[2][0] = 0.0;
        LmixABCPhaseL_[2][1] = 0.0;

        std::string dbnamemixL("LmixABCPhaseL");
        if (calphad_db.get_child_optional(dbnamemixL))
        {
            pt::ptree& Lmix0_db = calphad_db.get_child(dbnamemixL);
            readLmixTernaryParameters(Lmix0_db, LmixABCPhaseL_);
        }

        assert(LmixABCPhaseL_[0][0] == LmixABCPhaseL_[0][0]);
        assert(LmixABCPhaseL_[0][1] == LmixABCPhaseL_[0][1]);
        assert(LmixABCPhaseL_[1][0] == LmixABCPhaseL_[1][0]);
        assert(LmixABCPhaseL_[1][1] == LmixABCPhaseL_[1][1]);
        assert(LmixABCPhaseL_[2][0] == LmixABCPhaseL_[2][0]);
        assert(LmixABCPhaseL_[2][1] == LmixABCPhaseL_[2][1]);

        std::string dbnamemixA("LmixABCPhaseA");
        // default values
        LmixABCPhaseA_[0][0] = 0.0;
        LmixABCPhaseA_[0][1] = 0.0;
        LmixABCPhaseA_[1][0] = 0.0;
        LmixABCPhaseA_[1][1] = 0.0;
        LmixABCPhaseA_[2][0] = 0.0;
        LmixABCPhaseA_[2][1] = 0.0;
        if (calphad_db.get_child_optional(dbnamemixA))
        {
            pt::ptree& Lmix1_db = calphad_db.get_child(dbnamemixA);
            readLmixTernaryParameters(Lmix1_db, LmixABCPhaseA_);
        }

        assert(LmixABCPhaseA_[0][0] == LmixABCPhaseA_[0][0]);
        assert(LmixABCPhaseA_[0][1] == LmixABCPhaseA_[0][1]);
        assert(LmixABCPhaseA_[1][0] == LmixABCPhaseA_[1][0]);
        assert(LmixABCPhaseA_[1][1] == LmixABCPhaseA_[1][1]);
        assert(LmixABCPhaseA_[2][0] == LmixABCPhaseA_[2][0]);
        assert(LmixABCPhaseA_[2][1] == LmixABCPhaseA_[2][1]);
    }

    // print database just read
    // std::clog << "CALPHAD database..." << std::endl;
    // pt::write_json(std::clog, calphad_db);
}

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

#ifdef HAVE_OPENMP_OFFLOAD
#pragma omp declare target
#endif

double CALPHADFreeEnergyFunctionsTernary::computeFreeEnergy(
    const double temperature, const double* conc, const PhaseIndex pi,
    const bool gp)
{
    const double conc0 = conc[0];
    const double conc1 = conc[1];

    CalphadDataType lAB[4]
        = { lmix0ABPhase(pi, temperature), lmix1ABPhase(pi, temperature),
              lmix2ABPhase(pi, temperature), lmix3ABPhase(pi, temperature) };
    CalphadDataType lAC[4]
        = { lmix0ACPhase(pi, temperature), lmix1ACPhase(pi, temperature),
              lmix2ACPhase(pi, temperature), lmix3ACPhase(pi, temperature) };
    CalphadDataType lBC[4]
        = { lmix0BCPhase(pi, temperature), lmix1BCPhase(pi, temperature),
              lmix2BCPhase(pi, temperature), lmix3BCPhase(pi, temperature) };

    CalphadDataType lABC[3] = { lmix0ABCPhase(pi, temperature),
        lmix1ABCPhase(pi, temperature), lmix2ABCPhase(pi, temperature) };

    CALPHADSpeciesPhaseGibbsEnergy* g_species;

    switch (pi)
    {
        case PhaseIndex::phaseL:
            g_species = &g_species_phaseL_[0];
            break;
        case PhaseIndex::phaseA:
            g_species = &g_species_phaseA_[0];
            break;
        default:
            //            std::cout << "CALPHADFreeEnergyFunctionsTernary::"
            //                         "computeFreeEnergy(), undefined phase!!!"
            //                      << std::endl;
            // abort();
            return 0.;
    }

    double conc2 = 1. - conc0 - conc1;
    double fe    = conc0 * g_species[0].fenergy(temperature)
                + conc1 * g_species[1].fenergy(temperature)
                + conc2 * g_species[2].fenergy(temperature)
                + CALPHADcomputeFMixTernary(lAB, lAC, lBC, lABC, conc0, conc1)
                + CALPHADcomputeFIdealMixTernary(
                      gas_constant_R_JpKpmol * temperature, conc0, conc1);

    // subtract -mu*c to get grand potential
    if (gp)
    {
        double deriv[2];
        computeDerivFreeEnergy(temperature, conc, pi, deriv);
        fe -= deriv[0] * conc0;
        fe -= deriv[1] * conc1;
    }

    return fe;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsTernary::computeDerivFreeEnergy(
    const double temperature, const double* const conc, const PhaseIndex pi,
    double* deriv)
{
    CalphadDataType lAB[4]
        = { lmix0ABPhase(pi, temperature), lmix1ABPhase(pi, temperature),
              lmix2ABPhase(pi, temperature), lmix3ABPhase(pi, temperature) };
    CalphadDataType lAC[4]
        = { lmix0ACPhase(pi, temperature), lmix1ACPhase(pi, temperature),
              lmix2ACPhase(pi, temperature), lmix3ACPhase(pi, temperature) };
    CalphadDataType lBC[4]
        = { lmix0BCPhase(pi, temperature), lmix1BCPhase(pi, temperature),
              lmix2BCPhase(pi, temperature), lmix3BCPhase(pi, temperature) };

    CalphadDataType lABC[3] = { lmix0ABCPhase(pi, temperature),
        lmix1ABCPhase(pi, temperature), lmix2ABCPhase(pi, temperature) };

    CALPHADSpeciesPhaseGibbsEnergy* g_species;

    switch (pi)
    {
        case PhaseIndex::phaseL:
            g_species = &g_species_phaseL_[0];
            break;
        case PhaseIndex::phaseA:
            g_species = &g_species_phaseA_[0];
            break;
        default:
            //            std::cout << "CALPHADFreeEnergyFunctionsTernary::"
            //                         "computeFreeEnergy(), undefined phase!!!"
            //                      << std::endl;
            // abort();
            return;
    }

    CALPHADcomputeFMix_derivTernary(
        lAB, lAC, lBC, lABC, conc[0], conc[1], deriv);

    deriv[0] += g_species[0].fenergy(temperature);
    deriv[0] -= g_species[2].fenergy(temperature);

    deriv[1] += g_species[1].fenergy(temperature);
    deriv[1] -= g_species[2].fenergy(temperature);

    double tmp[2];
    CALPHADcomputeFIdealMix_derivTernary(
        gas_constant_R_JpKpmol * temperature, conc[0], conc[1], tmp);
    deriv[0] += tmp[0];
    deriv[1] += tmp[1];
}

//=======================================================================

void CALPHADFreeEnergyFunctionsTernary::computeSecondDerivativeFreeEnergy(
    const double temp, const double* const conc, const PhaseIndex pi,
    double* d2fdc2)
{
    // assert(conc[0] >= 0.);
    // assert(conc[0] <= 1.);
    // assert(conc[1] >= 0.);
    // assert(conc[1] <= 1.);

    CalphadDataType lAB[4]  = { lmix0ABPhase(pi, temp), lmix1ABPhase(pi, temp),
        lmix2ABPhase(pi, temp), lmix3ABPhase(pi, temp) };
    CalphadDataType lAC[4]  = { lmix0ACPhase(pi, temp), lmix1ACPhase(pi, temp),
        lmix2ACPhase(pi, temp), lmix3ACPhase(pi, temp) };
    CalphadDataType lBC[4]  = { lmix0BCPhase(pi, temp), lmix1BCPhase(pi, temp),
        lmix2BCPhase(pi, temp), lmix3BCPhase(pi, temp) };
    CalphadDataType lABC[3] = { lmix0ABCPhase(pi, temp),
        lmix1ABCPhase(pi, temp), lmix2ABCPhase(pi, temp) };
    const double rt         = gas_constant_R_JpKpmol * temp;

    double deriv1[4];
    CALPHADcomputeFIdealMix_deriv2Ternary(rt, conc[0], conc[1], &deriv1[0]);

    double deriv2[4];
    CALPHADcomputeFMix_deriv2Ternary(
        lAB, lAC, lBC, lABC, conc[0], conc[1], &deriv2[0]);

    d2fdc2[0] = deriv1[0] + deriv2[0];
    d2fdc2[1] = deriv1[1] + deriv2[1];
    d2fdc2[2] = deriv1[2] + deriv2[2];
    d2fdc2[3] = deriv1[3] + deriv2[3];
}

//=======================================================================

void CALPHADFreeEnergyFunctionsTernary::computeTdependentParameters(
    const double temperature, CalphadDataType* L_AB_L, CalphadDataType* L_AC_L,
    CalphadDataType* L_BC_L, CalphadDataType* L_ABC_L, CalphadDataType* L_AB_S,
    CalphadDataType* L_AC_S, CalphadDataType* L_BC_S, CalphadDataType* L_ABC_S,
    CalphadDataType* fA, CalphadDataType* fB, CalphadDataType* fC)
{
    fA[0] = g_species_phaseL_[0].fenergy(temperature);
    fB[0] = g_species_phaseL_[1].fenergy(temperature);
    fC[0] = g_species_phaseL_[2].fenergy(temperature);

    fA[1] = g_species_phaseA_[0].fenergy(temperature);
    fB[1] = g_species_phaseA_[1].fenergy(temperature);
    fC[1] = g_species_phaseA_[2].fenergy(temperature);

    L_AB_L[0]  = lmix0ABPhaseL(temperature);
    L_AB_L[1]  = lmix1ABPhaseL(temperature);
    L_AB_L[2]  = lmix2ABPhaseL(temperature);
    L_AB_L[3]  = lmix3ABPhaseL(temperature);
    L_AC_L[0]  = lmix0ACPhaseL(temperature);
    L_AC_L[1]  = lmix1ACPhaseL(temperature);
    L_AC_L[2]  = lmix2ACPhaseL(temperature);
    L_AC_L[3]  = lmix3ACPhaseL(temperature);
    L_BC_L[0]  = lmix0BCPhaseL(temperature);
    L_BC_L[1]  = lmix1BCPhaseL(temperature);
    L_BC_L[2]  = lmix2BCPhaseL(temperature);
    L_BC_L[3]  = lmix3BCPhaseL(temperature);
    L_ABC_L[0] = lmix0ABCPhaseL(temperature);
    L_ABC_L[1] = lmix1ABCPhaseL(temperature);
    L_ABC_L[2] = lmix2ABCPhaseL(temperature);

    L_AB_S[0]  = lmix0ABPhaseA(temperature);
    L_AB_S[1]  = lmix1ABPhaseA(temperature);
    L_AB_S[2]  = lmix2ABPhaseA(temperature);
    L_AB_S[3]  = lmix3ABPhaseA(temperature);
    L_AC_S[0]  = lmix0ACPhaseA(temperature);
    L_AC_S[1]  = lmix1ACPhaseA(temperature);
    L_AC_S[2]  = lmix2ACPhaseA(temperature);
    L_AC_S[3]  = lmix3ACPhaseA(temperature);
    L_BC_S[0]  = lmix0BCPhaseA(temperature);
    L_BC_S[1]  = lmix1BCPhaseA(temperature);
    L_BC_S[2]  = lmix2BCPhaseA(temperature);
    L_BC_S[3]  = lmix3BCPhaseA(temperature);
    L_ABC_S[0] = lmix0ABCPhaseA(temperature);
    L_ABC_S[1] = lmix1ABCPhaseA(temperature);
    L_ABC_S[2] = lmix2ABCPhaseA(temperature);

    // assert(L_ABC_L[0] == L_ABC_L[0]);
    // assert(fC[0] == fC[0]);
}

//=======================================================================

// compute equilibrium concentrations in various phases for given temperature
bool CALPHADFreeEnergyFunctionsTernary::computeCeqT(
    const double temperature, double* ceq, const int maxits, const bool verbose)
{
    // if (verbose)
    //    std::cout << "CALPHADFreeEnergyFunctionsTernary::computeCeqT()"
    //              << std::endl;
    // assert(temperature > 0.);

    CalphadDataType L_AB_L[4];
    CalphadDataType L_AC_L[4];
    CalphadDataType L_BC_L[4];

    CalphadDataType L_ABC_L[3];

    CalphadDataType L_AB_S[4];
    CalphadDataType L_AC_S[4];
    CalphadDataType L_BC_S[4];

    CalphadDataType L_ABC_S[3];

    CalphadDataType fA[2];
    CalphadDataType fB[2];
    CalphadDataType fC[2];

    computeTdependentParameters(temperature, L_AB_L, L_AC_L, L_BC_L, L_ABC_L,
        L_AB_S, L_AC_S, L_BC_S, L_ABC_S, fA, fB, fC);

    double RTinv = 1.0 / (gas_constant_R_JpKpmol * temperature);

    CALPHADEqConcSolverTernary eq_solver;
    eq_solver.setup(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);
    int ret = eq_solver.ComputeConcentration(ceq, newton_tol_, maxits);
#ifndef HAVE_OPENMP_OFFLOAD
    if (ret >= 0)
    {
        if (verbose)
        {
            std::cout << "CALPHAD, c0 phase0=" << ceq[0] << std::endl;
            std::cout << "CALPHAD, c1 phase0=" << ceq[1] << std::endl;
            std::cout << "CALPHAD, c0 phase1=" << ceq[2] << std::endl;
            std::cout << "CALPHAD, c1 phase1=" << ceq[3] << std::endl;
        }
    }
    else
    {
        std::cout << "CALPHADFreeEnergyFunctionsTernary, WARNING: ceq "
                     "computation did not converge"
                  << std::endl;
    }
#endif
    return (ret >= 0);
}

//=======================================================================

bool CALPHADFreeEnergyFunctionsTernary::computeTieLine(const double temperature,
    const double c0, const double c1, double* ceq, const int maxits,
    const bool verbose)
{
    CalphadDataType L_AB_L[4];
    CalphadDataType L_AC_L[4];
    CalphadDataType L_BC_L[4];

    CalphadDataType L_ABC_L[3];

    CalphadDataType L_AB_S[4];
    CalphadDataType L_AC_S[4];
    CalphadDataType L_BC_S[4];

    CalphadDataType L_ABC_S[3];

    CalphadDataType fA[2];
    CalphadDataType fB[2];
    CalphadDataType fC[2];

    computeTdependentParameters(temperature, L_AB_L, L_AC_L, L_BC_L, L_ABC_L,
        L_AB_S, L_AC_S, L_BC_S, L_ABC_S, fA, fB, fC);

    double RTinv = 1.0 / (gas_constant_R_JpKpmol * temperature);

    CALPHADTieLineConcSolverTernary eq_solver;
    eq_solver.setup(c0, c1, 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);
    int ret = eq_solver.ComputeConcentration(ceq, newton_tol_, newton_maxits_);
#ifndef HAVE_OPENMP_OFFLOAD
    if (ret >= 0)
    {
        if (verbose)
        {
            std::cout << "CALPHAD, c0 phase0=" << ceq[0] << std::endl;
            std::cout << "CALPHAD, c1 phase0=" << ceq[1] << std::endl;
            std::cout << "CALPHAD, c0 phase1=" << ceq[2] << std::endl;
            std::cout << "CALPHAD, c1 phase1=" << ceq[3] << std::endl;
        }
    }
    else
    {
        std::cout << "CALPHADFreeEnergyFunctionsTernary, WARNING: ceq "
                     "computation did not converge"
                  << std::endl;
    }
#endif
    return (ret >= 0);
}

//=======================================================================

void CALPHADFreeEnergyFunctionsTernary::computePhasesFreeEnergies(
    const double temperature, const double* const hphi, const double conc0,
    const double conc1, double& fl, double& fa)
{
    // std::cout<<"CALPHADFreeEnergyFunctionsTernary::computePhasesFreeEnergies()"<<endl;

    double cauxilliary[4] = { conc0, conc1, conc0, conc1 };

    CalphadDataType L_AB_L[4];
    CalphadDataType L_AC_L[4];
    CalphadDataType L_BC_L[4];

    CalphadDataType L_ABC_L[3];

    CalphadDataType L_AB_S[4];
    CalphadDataType L_AC_S[4];
    CalphadDataType L_BC_S[4];

    CalphadDataType L_ABC_S[3];

    CalphadDataType fA[2];
    CalphadDataType fB[2];
    CalphadDataType fC[2];

    computeTdependentParameters(temperature, L_AB_L, L_AC_L, L_BC_L, L_ABC_L,
        L_AB_S, L_AC_S, L_BC_S, L_ABC_S, fA, fB, fC);

    double RTinv = 1.0 / (gas_constant_R_JpKpmol * temperature);
    CALPHADConcSolverTernary solver;
    solver.setup(conc0, conc1, hphi[0], 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);
    int ret
        = solver.ComputeConcentration(cauxilliary, newton_tol_, newton_maxits_);
#ifndef HAVE_OPENMP_OFFLOAD
    if (ret < 0)
    {
        std::cerr << "ERROR in "
                     "CALPHADFreeEnergyFunctionsTernary::"
                     "computePhasesFreeEnergies() "
                     "---"
                  << "conc0=" << conc0 << ", conc1=" << conc1
                  << ", hphi=" << hphi[0] << std::endl;
        abort();
    }

    assert(conc0 >= 0.);
#endif
    double concl[2] = { cauxilliary[0], cauxilliary[1] };
    fl = computeFreeEnergy(temperature, &concl[0], PhaseIndex::phaseL, false);

    double conca[2] = { cauxilliary[2], cauxilliary[3] };
    fa = computeFreeEnergy(temperature, &conca[0], PhaseIndex::phaseA, false);
}

//-----------------------------------------------------------------------
// output: x
int CALPHADFreeEnergyFunctionsTernary::computePhaseConcentrations(
    const double temperature, const double* const conc, const double* const phi,
    double* x)
{
    // assert(conc[0] == conc[0]);
    // assert(conc[1] == conc[1]);
    // assert(x[0] >= 0.);
    // assert(x[1] >= 0.);
    // assert(x[0] <= 1.);
    // assert(x[1] <= 1.);

    const double conc0 = conc[0];
    const double conc1 = conc[1];

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

    CalphadDataType L_AB_L[4];
    CalphadDataType L_AC_L[4];
    CalphadDataType L_BC_L[4];

    CalphadDataType L_ABC_L[3];

    CalphadDataType L_AB_S[4];
    CalphadDataType L_AC_S[4];
    CalphadDataType L_BC_S[4];

    CalphadDataType L_ABC_S[3];

    CalphadDataType fA[2];
    CalphadDataType fB[2];
    CalphadDataType fC[2];

    computeTdependentParameters(temperature, L_AB_L, L_AC_L, L_BC_L, L_ABC_L,
        L_AB_S, L_AC_S, L_BC_S, L_ABC_S, fA, fB, fC);
    // assert(fC[0] == fC[0]);

    const double hphi = interp_func(conc_interp_func_type_, phi[0]);

    // conc could be outside of [0.,1.] in a trial step
    double c0 = conc0 >= 0. ? conc0 : 0.;
    c0        = c0 <= 1. ? c0 : 1.;
    double c1 = conc1 >= 0. ? conc1 : 0.;
    c1        = c1 <= 1. ? c1 : 1.;

    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);
    int ret = solver.ComputeConcentration(x, newton_tol_, newton_maxits_);
#ifndef HAVE_OPENMP_OFFLOAD
    if (ret == -1)
    {
        std::cerr << "ERROR, "
                     "CALPHADFreeEnergyFunctionsTernary::"
                     "computePhaseConcentrations() "
                     "failed for conc0="
                  << conc0 << ", conc1=" << conc1 << ", hphi=" << hphi
                  << std::endl;
    }
#endif
    return ret;
}

#ifdef HAVE_OPENMP_OFFLOAD
#pragma omp end declare target
#endif

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

void CALPHADFreeEnergyFunctionsTernary::energyVsPhiAndC(
    const double temperature, const double* const ceq, const bool found_ceq,
    const double phi_well_scale, const int npts_phi, const int npts_c)
{
    std::clog << "CALPHADFreeEnergyFunctionsTernary::energyVsPhiAndC()..."
              << std::endl;

    const double* const ceqL = &ceq[0];
    const double* const ceqS = &ceq[2];
    // std::clog<<"Input:
    // "<<ceq[0]<<","<<ceq[1]<<","<<ceq[2]<<","<<ceq[3]<<endl;

    double slopec = 0.;
    double fc0    = 0.;
    double fc1    = 0.;
    if (found_ceq)
    {
        // compute slope of f between equilibrium concentrations
        // to add slopec*conc to energy later on

        fc0    = computeFreeEnergy(temperature, &ceqL[0], PhaseIndex::phaseL);
        fc1    = computeFreeEnergy(temperature, &ceqS[0], PhaseIndex::phaseA);
        slopec = -(fc1 - fc0) / (ceqL[1] - ceqL[0]);
    }
    std::clog << std::setprecision(8) << "fc0: " << fc0 << "..."
              << ", fc1: " << fc1 << "..." << std::endl;
    std::clog << "CALPHADFreeEnergyFunctionsTernary: Use slope: " << slopec
              << "..." << std::endl;

    {
        // reset cmin, cmax, deltac
        double c0min = std::min(ceqL[0], ceqS[0]);
        double c0max = std::max(ceqL[0], ceqS[0]);

        double dc0     = c0max - c0min;
        c0min          = std::max(0.25 * c0min, c0min - 0.25 * dc0);
        c0max          = std::min(1. - 0.25 * (1. - c0max), c0max + 0.25 * dc0);
        c0max          = std::max(c0max, c0min + dc0);
        double deltac0 = (c0max - c0min) / (npts_c - 1);

        double c1min = std::min(ceqL[1], ceqS[1]);
        double c1max = std::max(ceqL[1], ceqS[1]);

        double dc1     = c1max - c1min;
        c1min          = std::max(0.25 * c1min, c1min - 0.25 * dc1);
        c1max          = std::min(1. - 0.25 * (1. - c1max), c1max + 0.25 * dc1);
        c1max          = std::max(c1max, c1min + dc1);
        double deltac1 = (c1max - c1min) / (npts_c - 1);

        std::clog << "Range for c0: " << c0min << " to " << c0max << std::endl;
        std::clog << "Range for c1: " << c1min << " to " << c1max << std::endl;

        std::ofstream tfile(fenergy_diag_filename_, std::ios::out);

        printEnergyVsPhiHeader(temperature, npts_phi, npts_c, npts_c, c0min,
            c0max, c1min, c1max, tfile);

        for (int i0 = 0; i0 < npts_c; i0++)
        {
            int i1    = (1. - c0min - deltac0 * i0 - c1min) / deltac1;
            int i1max = i1 < npts_c ? i1 : npts_c;
            for (int i1 = 0; i1 < i1max; i1++)
            {
                double c[2] = { c0min + deltac0 * i0, c1min + deltac1 * i1 };
                printEnergyVsPhi(
                    c, temperature, phi_well_scale, npts_phi, tfile);
            }
        }
    }
}

// Print out free energy as a function of phase
// for given composition and temperature
// File format: ASCII VTK, readble with Visit
void CALPHADFreeEnergyFunctionsTernary::printEnergyVsPhiHeader(
    const double temperature, const int nphi, const int nc0, const int nc1,
    const double c0min, const double c0max, const double c1min,
    const double c1max, std::ostream& os) const
{
    os << "# vtk DataFile Version 2.0" << std::endl;
    os << "Free energy [J/mol] at T=" << temperature << std::endl;
    os << "ASCII" << std::endl;
    os << "DATASET STRUCTURED_POINTS" << std::endl;

    os << "DIMENSIONS   " << nphi << " " << nc0 << " " << nc1 << std::endl;
    double asp_ratio_c0 = (nc0 > 1) ? (c0max - c0min) / (nc0 - 1) : 1.;
    double asp_ratio_c1 = (nc1 > 1) ? (c1max - c1min) / (nc1 - 1) : 1.;
    os << "ASPECT_RATIO " << 1. / (nphi - 1) << " " << asp_ratio_c0 << " "
       << asp_ratio_c1 << std::endl;
    os << "ORIGIN        0. " << c0min << " " << c1min << std::endl;
    os << "POINT_DATA   " << nphi * nc0 * nc1 << std::endl;
    os << "SCALARS energy float 1" << std::endl;
    os << "LOOKUP_TABLE default" << std::endl;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsTernary::printEnergyVsPhi(const double* conc,
    const double temperature, const double phi_well_scale, const int npts,
    std::ostream& os)
{
    // std::cout << "CALPHADFreeEnergyFunctionsTernary::printEnergyVsPhi()..."
    // << std::endl;
    const double dphi = 1.0 / (double)(npts - 1);

    // os << "# phi     f(phi)     for c=" << conc
    //           << " and T=" << temperature << std::endl;
    for (int i = 0; i < npts; i++)
    {
        const double phi = i * dphi;

        double e       = fchem(&phi, conc, temperature);
        const double w = phi_well_scale * well_func(phi);

        os << e + w << std::endl;
    }
    // os << std::endl;
}

//=======================================================================
// compute free energy in [J/mol]
double CALPHADFreeEnergyFunctionsTernary::fchem(
    const double* const phi, const double* const conc, const double temperature)
{
    const double conc0 = conc[0];
    const double conc1 = conc[1];

    const double hcphi = interp_func(conc_interp_func_type_, phi[0]);

    const double tol = 1.e-8;
    double fl        = 0.;
    double fa        = 0.;
    if ((phi[0] > tol) & (phi[0] < 1. - tol))
    {
        computePhasesFreeEnergies(temperature, &hcphi, conc0, conc1, fl, fa);
    }
    else
    {
        // don't solve for phases concentrations, just compute energy
        // in either phase
        double conc[2] = { conc0, conc1 };
        if (phi[0] <= tol)
        {
            fl = computeFreeEnergy(temperature, &conc[0], PhaseIndex::phaseL);
        }
        else
        {
            fa = computeFreeEnergy(temperature, &conc[0], PhaseIndex::phaseA);
        }
    }

    const double hfphi = interp_func(energy_interp_func_type_, phi[0]);

    return (1.0 - hfphi) * fl + hfphi * fa;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsTernary::printEnergyVsComposition(
    const double temperature, std::ostream& os, const int npts)
{
    const double dc = 1.0 / (double)(npts - 1);

    std::string name1("Fl");
    name1 += g_species_phaseL_[0].name();
    name1 += g_species_phaseL_[2].name();
    os << "#" << name1 << std::endl;
    os << "#phi=0, c1=0, temperature=" << temperature << std::endl;
    for (int i = 0; i < npts; i++)
    {
        double conc[2];
        conc[0] = i * dc;
        conc[1] = 0.;

        const double phi = 0.;

        double e = fchem(&phi, conc, temperature);
        os << conc[0] << "\t" << e << std::endl;
    }
    os << std::endl;

    std::string name2("Fs");
    name2 += g_species_phaseA_[0].name();
    name2 += g_species_phaseA_[2].name();
    os << "#" << name2 << std::endl;
    os << "#phi=1, c1=0, temperature=" << temperature << std::endl;
    for (int i = 0; i < npts; i++)
    {
        double conc[2];
        conc[0] = i * dc;
        conc[1] = 0.;

        const double phi = 1.;

        double e = fchem(&phi, conc, temperature);
        os << conc[0] << "\t" << e << std::endl;
    }
    os << std::endl;

    std::string name3("Fl");
    name3 += g_species_phaseL_[1].name();
    name3 += g_species_phaseL_[2].name();
    os << "#" << name3 << std::endl;
    os << "#phi=0, c0=0, temperature=" << temperature << std::endl;
    for (int i = 0; i < npts; i++)
    {
        double conc[2];
        conc[0] = 0.;
        conc[1] = i * dc;

        const double phi = 0.;

        double e = fchem(&phi, conc, temperature);
        os << conc[1] << "\t" << e << std::endl;
    }
    os << std::endl;

    std::string name4("Fs");
    name4 += g_species_phaseA_[1].name();
    name4 += g_species_phaseA_[2].name();
    os << "#" << name4 << std::endl;
    os << "#phi=1, c0=0, temperature=" << temperature << std::endl;
    for (int i = 0; i < npts; i++)
    {
        double conc[2];
        conc[0] = 0.;
        conc[1] = i * dc;

        const double phi = 1.;

        double e = fchem(&phi, conc, temperature);
        os << conc[1] << "\t" << e << std::endl;
    }
    os << std::endl;

    std::string name5("Fl");
    name5 += g_species_phaseL_[0].name();
    name5 += g_species_phaseL_[1].name();
    os << "#" << name5 << std::endl;
    os << "#phi=0, temperature=" << temperature << std::endl;
    for (int i = 0; i < npts; i++)
    {
        double conc[2];
        conc[0] = i * dc;
        conc[1] = 1. - i * dc;

        const double phi = 0.;

        double e = fchem(&phi, conc, temperature);
        os << conc[0] << "\t" << e << std::endl;
    }
    os << std::endl;

    std::string name6("Fs");
    name6 += g_species_phaseA_[0].name();
    name6 += g_species_phaseA_[1].name();
    os << "#" << name6 << std::endl;
    os << "#phi=1, temperature=" << temperature << std::endl;
    for (int i = 0; i < npts; i++)
    {
        double conc[2];
        conc[0] = i * dc;
        conc[1] = 1. - i * dc;

        const double phi = 1.;

        double e = fchem(&phi, conc, temperature);
        os << conc[0] << "\t" << e << std::endl;
    }
    os << std::endl;
}

void CALPHADFreeEnergyFunctionsTernary::preRunDiagnostics(
    const double T0, const double T1)
{
    std::ofstream os1("FlC0vsT.dat", std::ios::out);
    os1 << "#Species 0, Phase L" << std::endl;
    g_species_phaseL_[0].plotFofT(os1, T0, T1);

    std::ofstream os2("FlC1vsT.dat", std::ios::out);
    os2 << "#Species 1, Phase L" << std::endl;
    g_species_phaseL_[1].plotFofT(os2, T0, T1);

    std::ofstream os3("FlC2vsT.dat", std::ios::out);
    os3 << "#Species 2, Phase L" << std::endl;
    g_species_phaseL_[2].plotFofT(os3, T0, T1);

    std::ofstream os4("FsC0vsT.dat", std::ios::out);
    os4 << "#Species 0, Phase A" << std::endl;
    g_species_phaseA_[0].plotFofT(os4, T0, T1);

    std::ofstream os5("FsC1vsT.dat", std::ios::out);
    os5 << "#Species 1, Phase A" << std::endl;
    g_species_phaseA_[1].plotFofT(os5, T0, T1);

    std::ofstream os6("FsC2vsT.dat", std::ios::out);
    os6 << "#Species 2, Phase A" << std::endl;
    g_species_phaseA_[2].plotFofT(os6, T0, T1);
}
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include "graph_augment.hh"
#include <boost/graph/stoer_wagner_min_cut.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_min_cut
{
    template <class Graph, class EdgeWeight, class PartMap>
    void operator()(Graph& g, EdgeWeight eweight, PartMap part_map, double& mc) const
    {
        try
        {
            mc = stoer_wagner_min_cut(g, eweight, parity_map(part_map));
        }
        catch (bad_graph&)
        {
            throw ValueException("Graph has less than 2 vertices.");
        }
    }

};

double min_cut(GraphInterface& gi, boost::any weight, boost::any part_map)
{
    double mc = 0;

    typedef ConstantPropertyMap<size_t,GraphInterface::edge_t> cweight_t;

    if (weight.empty())
        weight = cweight_t(1);

    typedef boost::mpl::push_back<writable_edge_scalar_properties, cweight_t>::type
        weight_maps;

    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_min_cut(),  placeholders::_1,  placeholders::_2,
                       placeholders::_3, std::ref(mc)),
         weight_maps(), writable_vertex_scalar_properties())(weight, part_map);
    return mc;
}

struct do_get_residual_graph
{
    template <class Graph, class CapacityMap, class ResidualMap,
              class AugmentedMap>
    void operator()(Graph& g, CapacityMap capacity, ResidualMap res,
                    AugmentedMap augmented) const
    {
        residual_graph(g, capacity, res, augmented);
    }
};

void get_residual_graph(GraphInterface& gi, boost::any capacity,
                        boost::any res, boost::any oaugment)
{
    typedef property_map_type::apply<uint8_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    emap_t augment = boost::any_cast<emap_t>(oaugment);
    run_action<>()
        (gi, std::bind(do_get_residual_graph(), placeholders::_1,
                       placeholders::_2, placeholders::_3, augment),
         edge_scalar_properties(), edge_scalar_properties())(capacity, res);
}


#ifndef CPPID_GM3D_DETAIL_BASIC_POINT_VECTOR_HPP
#define CPPID_GM3D_DETAIL_BASIC_POINT_VECTOR_HPP

#include <cstddef>
#include <type_traits>
#include <utility>

#include <boost/qvm/all.hpp>

#include "type_traits.hpp"

namespace cppid::gm3d::detail {

template<typename TDerived, typename T, std::size_t N>
struct basic_point_vector {
  using scalar_type = T;

  static constexpr auto dimensions = N;

  scalar_type elems[dimensions];

  constexpr basic_point_vector() noexcept = default;

  template<
   typename... Us,
   typename = std::enable_if_t<((sizeof...(Us) == dimensions) && ... &&
                                std::is_convertible_v<Us, scalar_type>)>>
  constexpr basic_point_vector(Us... args) noexcept
  : elems{scalar_type(args)...}
  {
  }

  explicit constexpr basic_point_vector(scalar_type s) noexcept
  : basic_point_vector{s, std::make_index_sequence<dimensions>()}
  {
  }

  template<std::size_t... Ns,
           typename = std::enable_if_t<(sizeof...(Ns) == dimensions)>>
  constexpr basic_point_vector(scalar_type s,
                               std::index_sequence<Ns...>) noexcept
  : basic_point_vector{(static_cast<void>(Ns), s)...}
  {
  }

  constexpr void x(scalar_type val) noexcept
  {
    elems[0] = val;
  }

  constexpr void y(scalar_type val) noexcept
  {
    elems[1] = val;
  }

  template<auto VN = dimensions, std::enable_if_t<(VN >= 3)>* = nullptr>
  constexpr void z(scalar_type val) noexcept
  {
    elems[2] = val;
  }

  template<auto VN = dimensions, std::enable_if_t<(VN >= 4)>* = nullptr>
  constexpr void w(scalar_type val) noexcept
  {
    elems[3] = val;
  }

  constexpr auto x() const noexcept -> scalar_type
  {
    return elems[0];
  }

  constexpr auto y() const noexcept -> scalar_type
  {
    return elems[1];
  }

  template<auto VN = dimensions, std::enable_if_t<(VN >= 3)>* = nullptr>
  constexpr auto z() const noexcept -> scalar_type
  {
    return elems[2];
  }

  template<auto VN = dimensions, std::enable_if_t<(VN >= 4)>* = nullptr>
  constexpr auto w() const noexcept -> scalar_type
  {
    return elems[3];
  }
};

} // namespace cppid::gm3d::detail

namespace boost::qvm {
template<typename TVec, typename T, std::size_t N>
struct vec_traits<cppid::gm3d::detail::basic_point_vector<TVec, T, N>> {
  using point_vector_type = cppid::gm3d::detail::basic_point_vector<TVec, T, N>;

  using scalar_type = typename point_vector_type::scalar_type;

  static constexpr int dim = point_vector_type::dimensions;

  template<int I>
  static inline constexpr auto write_element(point_vector_type& v) noexcept
   -> scalar_type&
  {
    return v.elems[I];
  }

  template<int I>
  static inline constexpr auto read_element(point_vector_type const& v) noexcept
   -> scalar_type
  {
    return v.elems[I];
  }

  static inline constexpr auto write_element_idx(int i,
                                                 point_vector_type& v) noexcept
   -> scalar_type&
  {
    return v.elems[i];
  }

  static inline constexpr auto
  read_element_idx(int i, point_vector_type const& v) noexcept -> scalar_type
  {
    return v.elems[i];
  }
};

} // namespace boost::qvm

#endif


/// @file  embed_ref.cpp
/// @brief Choose and embed a reference subset.

#include <boost/program_options.hpp>
#include <ogt/ogt.hpp>
#include <chrono>
#include <future>
#include <iomanip>
#include <iostream>
#include <vector>
#include <cmath>

using namespace boost::program_options;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::SparseMatrix;
using ogt::core::AB_LT_AC;
using ogt::core::Collection;
using ogt::core::createDistOracle;
using ogt::core::Oracle;
using ogt::embed::CmpConstraint;
using ogt::embed::embedCmpWithSOE;
using ogt::embed::EmbedConfig;
using ogt::embed::EmbedResult;
using ogt::io::IoErr;
using ogt::io::isSparseFile;
using ogt::io::loadDenseMatrix;
using ogt::io::loadSparseMatrix;
using ogt::io::openSink;
using ogt::io::saveEmbedding;
using ogt::linalg::DenseNZIterator;
using ogt::linalg::normalizePos;
using ogt::linalg::posToCosineDist;
using ogt::linalg::posToDist;
using ogt::linalg::posToJaccardDist;
using ogt::linalg::projectOntoUnitSphere;
using ogt::linalg::selectRows;
using ogt::util::random_nchoosek;
using std::async;
using std::cerr;
using std::cout;
using std::endl;
using std::future;
using std::make_shared;
using std::map;
using std::min;
using std::shared_ptr;
using std::string;
using std::stringstream;
using std::vector;

const char * DIST_EUCLIDEAN = "EUCLIDEAN";
const char * DIST_COSINE = "COSINE";
const char * DIST_JACCARD = "JACCARD";

std::ostream& log() {
	std::time_t t = std::time(nullptr);
    std::tm tm = *std::localtime(&t);
    cerr << std::put_time(&tm, "%c %Z") << ' ';
    return cerr;
}

/// Run a single embedding, perhaps in a thread pool.
shared_ptr<EmbedResult> embed(size_t nObj,
	const vector<CmpConstraint>& cmps, const EmbedConfig& config) {
	auto result = make_shared<EmbedResult>();
	result->X = MatrixXd::Random(nObj, config.nDim);
	*result = embedCmpWithSOE(cmps, result->X, config);
	return result;
}

/// Run the program.
int run_embed_ref(string posPath, string distType, bool projectOntoSphere,
	size_t subsetSize, size_t numRankings, size_t nDim, size_t batchSize, size_t bestOf,
	double target, bool doubleOnSuccess, double doublingTimeLimit,
	string embPath, string triplesPath) {

	// Load the true position matrix and create a distance oraclelog() "Loading position matrix from " << posPath << endl;
	MatrixXd truePos, subsetPos;
	Eigen::SparseMatrix<double, Eigen::RowMajor> spTruePos, spSubsetPos;
	size_t nObj;
	if (isSparseFile(posPath)) {
		try {
			spTruePos = loadSparseMatrix(posPath);
		} catch (const IoErr& e) {
			log() << "Could not load position matrix: " << e.what() << endl;
			return 0;
		}
		nObj = spTruePos.rows();
	} else {
		truePos = loadDenseMatrix(posPath);
		nObj = truePos.rows();
	}

	size_t numTries = 0;
	bool isDoubling = false;
	shared_ptr<EmbedResult> best;
	vector<size_t> subset;
	double savedLoss = INFINITY;
	auto startedSize = std::chrono::steady_clock::now();
	while (!best || (best->loss > target && numTries < bestOf)) {

		// Choose a subset at random
		log() << "Choosing subset from " << nObj << " total items" << endl;
		auto ss = random_nchoosek(nObj, subsetSize);
		subset.clear();
		subset.insert(subset.begin(), ss.begin(), ss.end());
		if (spTruePos.size() > 0) {
			spSubsetPos = selectRows(spTruePos, subset);
			log() << "Subset pos is " << spSubsetPos.rows() << " x " << spSubsetPos.cols() << endl;
		} else {
			subsetPos = selectRows(truePos, subset);
			log() << "Subset pos is " << subsetPos.rows() << " x " << subsetPos.cols() << endl;
		}

		// Construct the collection
		log() << "Finding all pairwise distances" << endl;
		MatrixXd dist;
		if (distType == DIST_EUCLIDEAN) {
			if (spSubsetPos.size() > 0) {
				dist = posToDist(spSubsetPos);
			} else {
				dist = posToDist(subsetPos);
			}
		} else if (distType == DIST_COSINE) {
			if (spSubsetPos.size() > 0) {
				dist = posToCosineDist(spSubsetPos);
			} else {
				dist = posToCosineDist(subsetPos);
			}
		} else if (distType == DIST_JACCARD) {
			if (spSubsetPos.size() > 0) {
				dist = posToJaccardDist<SparseMatrix<double, Eigen::RowMajor>,
					Eigen::SparseVector<double>,
					Eigen::SparseVector<double>::InnerIterator>(
						spSubsetPos);
			} else {
				dist = posToJaccardDist<MatrixXd, VectorXd, DenseNZIterator<VectorXd>>(
						subsetPos);
			}
		} else {
			log() << "Invalid distance function: " << distType << endl;
			return 0;
		}
		log() << "Dist is " << dist.rows() << " x " << dist.cols() << endl;

		auto oracle = createDistOracle(dist);
		auto coll = Collection::Create(oracle, subset.size());

		// Choose comparisons for the subset.
		numRankings = std::min<size_t>(numRankings, subset.size());
		log() << "Finding comparisons for " << numRankings << " rankings" << endl;
		vector<CmpConstraint> cmps;
		for (size_t pt : coll->frftTraversal().stopAfter(numRankings)) {
			coll->sort(pt, cmps);
		}
		auto outTriples = openSink(triplesPath);
		if (!outTriples) {
			log() << "Could not open " << triplesPath << endl;
			return 1;
		}
		for (const auto& cmp : cmps) {
			*outTriples << subset[cmp.a] << ','
				<< subset[cmp.b] << ','
				<< subset[cmp.c] << endl;
		}
		outTriples.reset();
		log() << "Embedding from " << cmps.size() << " triples" << endl;

		// Embed the subset.
		log() << "Embedding subset into R^" << nDim << " from " << cmps.size() << " triples" << endl;
		EmbedConfig config;
		config.nDim = nDim;
		config.minDelta = 1e-12;
		config.maxIter = 1e4;
		config.verbose = false;
		config.margin = 1e-2;

		vector<future<shared_ptr<EmbedResult>>> futures;
		batchSize = min(batchSize, 1 + bestOf - numTries);
		if (!batchSize) {
			break;
		}
		auto started = std::chrono::steady_clock::now();
		for (size_t i = 0; i < batchSize; i++) {
			futures.push_back(async(embed, subsetSize, cmps, config));
		}
		for (auto& future : futures) {
			numTries++;
			auto result = future.get();
			if (!best || result->loss < best->loss) {
				best = result;
			}
			log() << "Got embedding loss " << result->loss << " in " << numTries << " tries; min is " << best->loss << endl;
			if (best->loss == 0) {
				break;
			} else if (isDoubling && doublingTimeLimit) {
				auto endedSize = std::chrono::steady_clock::now();
				std::chrono::duration<double> diff = endedSize - startedSize;
				if (diff.count() > doublingTimeLimit) {
					break;
				}
			}
		}
		auto ended = std::chrono::steady_clock::now();
		std::chrono::duration<double> diff = ended - started;
		log() << "Batch took " << diff.count() << " seconds" << endl;

		// Save the embedding.
		if (best->loss <= savedLoss) {
			savedLoss = best->loss;
			log() << "Saving embedding after " << numTries << " tries with loss "
				<< best->loss << endl;
			if (projectOntoSphere) {
				best->X = projectOntoUnitSphere(best->X);
			} else {
				best->X = normalizePos(best->X);
			}
			saveEmbedding(best->X, subset, embPath);
		}

		// Double on success
		if (isDoubling) {
			auto endedSize = std::chrono::steady_clock::now();
			std::chrono::duration<double> diff = endedSize - startedSize;
			log() << "Subset size took " << diff.count() << " seconds so far" << endl;
			if (isDoubling && doublingTimeLimit && diff.count() > doublingTimeLimit) {
				log() << "Stopping due to long size duration" << endl;
				break;
			}
		}
		if (doubleOnSuccess && best->loss <= savedLoss) {
			savedLoss = target;
			subsetSize = min(nObj, 2 * subsetSize);
			isDoubling = true;
			startedSize = std::chrono::steady_clock::now();
			log() << "Doubling subset size to " << subsetSize << endl;
			best.reset();
		}
	}

	log() << "Done" << endl;
	return 0;
}

/// Program entry point.
int main(int argc, char * argv[]) {
	try {
		stringstream distDesc;
		distDesc << "Distance function: " << DIST_EUCLIDEAN << ", " << DIST_COSINE << ", or " << DIST_JACCARD;
		options_description desc{"eval_embedding options"};
		desc.add_options()
			("help,h", "Show this usage message")
			("posfile", value<string>(), "Input file containing dataset")
			("dist", value<string>()->default_value("EUCLIDEAN"), distDesc.str().c_str())
			("project_onto_sphere", value<bool>()->default_value(false), "Project embedding onto centered sphere")
			("size", value<size_t>(), "Subset size")
			("num_rankings", value<size_t>(), "Number of rankings to embed with")
			("dim", value<size_t>(), "Embedding dimensionality")
			("embedding", value<string>(), "Output file for the reference embedding")
			("ref_triples", value<string>(), "Output file for the reference triples")
			("batch", value<size_t>()->default_value(1), "Re-roll after n tries")
			("best_of", value<size_t>()->default_value(1), "Return best of n runs")
			("ref_double", value<bool>()->default_value(false), "On success, attempt twice the subset size")
			("double_time_limit", value<double>()->default_value(0), "When attempting larger subsets, stop if a batch takes more than this many seconds")
			("target", value<double>()->default_value(0), "Stop when a loss at most this large is obtained")
			;

		variables_map vm;
		store(parse_command_line(argc, argv, desc), vm);
		notify(vm);

		bool valid = true;
		if (vm.count("help")) {
			cerr << desc << endl;
			valid = false;
		}
		if (!vm.count("posfile")) {
			cerr << "--posfile is required" << endl;
			valid = false;
		}
		if (!vm.count("embedding")) {
			cerr << "--embedding is required" << endl;
			valid = false;
		}
		if (!vm.count("size")) {
			cerr << "--size is required" << endl;
			valid = false;
		}
		if (!vm.count("dim")) {
			cerr << "--dim is required" << endl;
			valid = false;
		}
		if (valid) {
			return run_embed_ref(
				vm["posfile"].as<string>(),
				vm["dist"].as<string>(),
				vm["project_onto_sphere"].as<bool>(),
				vm["size"].as<size_t>(),
				vm["num_rankings"].as<size_t>(),
				vm["dim"].as<size_t>(),
				vm["batch"].as<size_t>(),
				vm["best_of"].as<size_t>(),
				vm["target"].as<double>(),
				vm["ref_double"].as<bool>(),
				vm["double_time_limit"].as<double>(),
				vm["embedding"].as<string>(),
				vm["ref_triples"].as<string>());
		}
	} catch (const error &ex) {
		cerr << ex.what() << endl;
	}
	return 1;
}


// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2017 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef OPENMVG_NUMERIC_EIGEN_ALIAS_DEFINITION_HPP
#define OPENMVG_NUMERIC_EIGEN_ALIAS_DEFINITION_HPP

//--
// Eigen
// http://eigen.tuxfamily.org/dox-devel/QuickRefPage.html
//--

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

#include <initializer_list>
#include <memory>
#include <vector>

// Extend EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION with initializer list support.
#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(...)       \
namespace std {                                                            \
  template <>                                                              \
  class vector<__VA_ARGS__, std::allocator<__VA_ARGS__>>                   \
      : public vector<__VA_ARGS__, Eigen::aligned_allocator<__VA_ARGS__>> { \
    typedef vector<__VA_ARGS__, Eigen::aligned_allocator<__VA_ARGS__>>      \
        vector_base;                                                       \
                                                                           \
   public:                                                                 \
    typedef __VA_ARGS__ value_type;                                        \
    typedef vector_base::allocator_type allocator_type;                    \
    typedef vector_base::size_type size_type;                              \
    typedef vector_base::iterator iterator;                                \
    explicit vector(const allocator_type& a = allocator_type())            \
        : vector_base(a) {}                                                \
    template <typename InputIterator>                                      \
    explicit vector(InputIterator first, InputIterator last,                        \
           const allocator_type& a = allocator_type())                     \
        : vector_base(first, last, a) {}                                   \
    vector(const vector& c) = default;                                     \
    explicit vector(size_type num, const value_type& val = value_type())   \
        : vector_base(num, val) {}                                         \
    explicit vector(iterator start, iterator end) : vector_base(start, end) {}      \
    vector& operator=(const vector& x) = default;                          \
    /* Add initializer list constructor support*/                          \
    vector(std::initializer_list<__VA_ARGS__> list)                             \
        : vector_base(list.begin(), list.end()) {}                         \
  };                                                                       \
}  // namespace std

namespace openMVG
{
  using Eigen::Map;

  /// Trait used for double type
  using EigenDoubleTraits = Eigen::NumTraits<double>;

  /// 3d vector using double internal format
  using Vec3 = Eigen::Vector3d;

  /// 2d vector using int internal format
  using Vec2i = Eigen::Vector2i;

  /// 2d vector using float internal format
  using Vec2f = Eigen::Vector2f;

  /// 3d vector using float internal format
  using Vec3f =Eigen::Vector3f;

  /// 9d vector using double internal format
  using Vec9 = Eigen::Matrix<double, 9, 1>;

  /// Quaternion type
  using Quaternion = Eigen::Quaternion<double>;

  /// 3x3 matrix using double internal format
  using Mat3 = Eigen::Matrix<double, 3, 3>;

  /// 3x4 matrix using double internal format
  using Mat34 = Eigen::Matrix<double, 3, 4>;

  /// 2d vector using double internal format
  using Vec2 = Eigen::Vector2d;

  /// 4d vector using double internal format
  using Vec4 = Eigen::Vector4d;

  /// 6d vector using double internal format
  using Vec6 = Eigen::Matrix<double, 6, 1>;

  /// 4x4 matrix using double internal format
  using Mat4 = Eigen::Matrix<double, 4, 4>;

  /// generic matrix using unsigned int internal format
  using Matu = Eigen::Matrix<unsigned int, Eigen::Dynamic, Eigen::Dynamic>;

  /// 3x3 matrix using double internal format with RowMajor storage
  using RMat3 = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>;

  //-- General purpose Matrix and Vector
  /// Unconstrained matrix using double internal format
  using Mat = Eigen::MatrixXd;

  /// Unconstrained vector using double internal format
  using Vec = Eigen::VectorXd;

  /// Unconstrained vector using unsigned int internal format
  using Vecu = Eigen::Matrix<unsigned int, Eigen::Dynamic, 1>;

  /// Unconstrained matrix using float internal format
  using Matf = Eigen::MatrixXf;

  /// Unconstrained vector using float internal format
  using Vecf = Eigen::VectorXf;

  /// 2xN matrix using double internal format
  using Mat2X = Eigen::Matrix<double, 2, Eigen::Dynamic>;

  /// 3xN matrix using double internal format
  using Mat3X = Eigen::Matrix<double, 3, Eigen::Dynamic>;

  /// 4xN matrix using double internal format
  using Mat4X = Eigen::Matrix<double, 4, Eigen::Dynamic>;

  /// Nx9 matrix using double internal format
  using MatX9 = Eigen::Matrix<double, Eigen::Dynamic, 9>;

  //-- Sparse Matrix (Column major, and row major)
  /// Sparse unconstrained matrix using double internal format
  using sMat = Eigen::SparseMatrix<double>;

  /// Sparse unconstrained matrix using double internal format and Row Major storage
  using sRMat = Eigen::SparseMatrix<double, Eigen::RowMajor>;

} // namespace openMVG

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Vec2)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Vec3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Vec4)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Vec6)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Vec9)

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Vec2i)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Vec2f)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Vec3f)

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Quaternion)

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Mat3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::RMat3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Mat4)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_INITIALIZER_LIST(openMVG::Mat34)

#endif  // OPENMVG_NUMERIC_EIGEN_ALIAS_DEFINITION_HPP


#ifndef HELPERS_HPP
#define HELPERS_HPP

#include <iostream>
#include <algorithm>
#include <chrono>
// Point Cloud Library
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
// Eigen
#include <Eigen/Dense>
#include <Eigen/Core>

#if defined(__SSE2__)
#include <xmmintrin.h>
#endif

#if defined(__AVX__)
#include <immintrin.h>
#endif

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <vector>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

/** A helper struct to apply an SO3 or SE3 transform to a 3D point.
 * Supports single and double precision transform matrices. */
template<typename Scalar>
struct Transformer
{
    const Eigen::Matrix<Scalar, 4, 4>& tf;

    /** Construct a transformer object.
     * The transform matrix is captured by const reference. Make sure that it does not go out of scope before this
     * object does. */
    Transformer (const Eigen::Matrix<Scalar, 4, 4>& transform) : tf (transform) { };

    /** Apply SO3 transform (top-left corner of the transform matrix).
     * \param[in] src input 3D point (pointer to 3 floats)
     * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 0. */
    void so3 (const float* src, float* tgt) const
    {
        const Scalar p[3] = { src[0], src[1], src[2] };  // need this when src == tgt
        tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2]);
        tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2]);
        tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2]);
        tgt[3] = 0;
    }

    /** Apply SE3 transform.
     * \param[in] src input 3D point (pointer to 3 floats)
     * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 1. */
    void se3 (const float* src, float* tgt) const
    {
        const Scalar p[3] = { src[0], src[1], src[2] };  // need this when src == tgt
        tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2] + tf (0, 3));
        tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2] + tf (1, 3));
        tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2] + tf (2, 3));
        tgt[3] = 1;
    }
};

#if defined(__SSE2__)

/** Optimized version for single-precision transforms using SSE2 intrinsics. */
template<>
struct Transformer<float>
{
    /// Columns of the transform matrix stored in XMM registers.
    __m128 c[4];

    Transformer(const Eigen::Matrix4f& tf)
    {
        for (std::size_t i = 0; i < 4; ++i)
            c[i] = _mm_load_ps (tf.col (i).data ());
    }

    void so3 (const float* src, float* tgt) const
    {
        __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
        __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
        __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
        _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, p2)));
    }

    void se3 (const float* src, float* tgt) const
    {
        __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
        __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
        __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
        _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, _mm_add_ps(p2, c[3]))));
    }
};

#if !defined(__AVX__)

/** Optimized version for double-precision transform using SSE2 intrinsics. */
template<>
struct Transformer<double>
{
    /// Columns of the transform matrix stored in XMM registers.
    __m128d c[4][2];

    Transformer(const Eigen::Matrix4d& tf)
    {
        for (std::size_t i = 0; i < 4; ++i)
        {
            c[i][0] = _mm_load_pd (tf.col (i).data () + 0);
            c[i][1] = _mm_load_pd (tf.col (i).data () + 2);
        }
    }

    void so3 (const float* src, float* tgt) const
    {
        __m128d xx = _mm_cvtps_pd (_mm_load_ps1 (&src[0]));
        __m128d p0 = _mm_mul_pd (xx, c[0][0]);
        __m128d p1 = _mm_mul_pd (xx, c[0][1]);

        for (std::size_t i = 1; i < 3; ++i)
        {
            __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
            p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
            p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
        }

        _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
    }

    void se3 (const float* src, float* tgt) const
    {
        __m128d p0 = c[3][0];
        __m128d p1 = c[3][1];

        for (std::size_t i = 0; i < 3; ++i)
        {
            __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
            p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
            p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
        }

        _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
    }
};

#else

/** Optimized version for double-precision transform using AVX intrinsics. */
template<>
struct Transformer<double>
{
    __m256d c[4];

    Transformer(const Eigen::Matrix4d& tf)
    {
        for (std::size_t i = 0; i < 4; ++i)
            c[i] = _mm256_load_pd (tf.col (i).data ());
    }

    void so3 (const float* src, float* tgt) const
    {
        __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
        __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
        __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
        _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, p2))));
    }

    void se3 (const float* src, float* tgt) const
    {
        __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
        __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
        __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
        _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, _mm256_add_pd(p2, c[3])))));
    }
};

#endif // !defined(__AVX__)
#endif // defined(__SSE2__)

using namespace std;
using namespace Eigen;

inline void apply_transformation_optimized(float* data,float* output,Eigen::Affine3d& transformation)
{
    Transformer<double> tf(transformation.matrix());
    tf.se3(data,output);
}

/*****************************************************************/
//Color Definitions
/****************************************************************/
#define _NORMAL_    "\x1b[0m"
#define _BLACK_     "\x1b[30;47m"
#define _RED_       "\x1b[31;40m"
#define _GREEN_     "\x1b[32;40m"
#define _YELLOW_    "\x1b[33;40m"
#define _BLUE_      "\x1b[34;40m"
#define _MAGENTA_   "\x1b[35;40m"
#define _CYAN_      "\x1b[36;40m"
#define _WHITE_     "\x1b[37;40m"
#define _BRED_      "\x1b[1;31;40m"
#define _BGREEN_    "\x1b[1;32;40m"
#define _BYELLOW_   "\x1b[1;33;40m"
#define _BBLUE_     "\x1b[1;34;40m"
#define _BMAGENTA_  "\x1b[1;35;40m"
#define _BCYAN_     "\x1b[1;36;40m"
#define _BWHITE_    "\x1b[1;37;40m"
/******************************************************************/
//Print Utilities for debugging.
/*******************************************************************/
#define dbg_color(...) do { printf(__VA_ARGS__); } while(0)

#define TIC() std::chrono::high_resolution_clock::time_point END_TIME,START_TIME=std::chrono::high_resolution_clock::now()

#define RESET() START_TIME=std::chrono::high_resolution_clock::now()

#define TOC()                                                   \
    do                                                              \
{                                                               \
    dbg_color(_GREEN_);                                         \
    END_TIME=std::chrono::high_resolution_clock::now();         \
    std::cout << " Time elapsed in "<<__func__<<"() at line "<<__LINE__<<": "         \
    << std::chrono::duration_cast<std::chrono::microseconds>(END_TIME - START_TIME).count() << " microseconds.\n"; \
    dbg_color(_NORMAL_);                                        \
}while(0)

namespace TransformationUtilities
{
    inline Eigen::MatrixXd apply_transformation(Eigen::MatrixXd data, Eigen::Matrix4d T_mat);
    template<typename PointT> void transformPointCloud(typename pcl::PointCloud<PointT>::Ptr cloud_input,typename pcl::PointCloud<PointT>::Ptr cloud_output,Eigen::MatrixXd& a_T_b)
    {
        for(auto point:cloud_input->points)
        {
            Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
            pts(0,0)=point.x;
            pts(0,1)=point.y;
            pts(0,2)=point.z;
            Eigen::MatrixXd pts_trans=apply_transformation(pts,a_T_b);
            auto pt = point;
            pt.x=pts_trans(0,0);
            pt.y=pts_trans(0,1);
            pt.z=pts_trans(0,2);
            cloud_output->points.push_back(pt);
        }
    }
    template<typename PointT> void transformPointCloud(typename pcl::PointCloud<PointT>::Ptr cloud_input,Eigen::MatrixXd& a_T_b)
    {
        for(auto& point:cloud_input->points)
        {
            Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
            pts(0,0)=point.x;
            pts(0,1)=point.y;
            pts(0,2)=point.z;
            Eigen::MatrixXd pts_trans=apply_transformation(pts,a_T_b);
            point.x=pts_trans(0,0);
            point.y=pts_trans(0,1);
            point.z=pts_trans(0,2);
        }
    }
    std::string validate_seq(std::string seq);
    //Default : ZYX
    Eigen::Matrix3d eul2rot(Eigen::MatrixXd eul_angles, std::string seq="");

    inline Eigen::MatrixXd vectorToTransformationMatrix(vector<double>& a_T_b_static);

    constexpr double degreeToRadian(int angle)
    {
        return angle*3.141592/180;
    }

    constexpr double radTodeg(double rad)
    {
        return (rad/3.14159)*180;
    }

    Eigen::Affine3f getAffineMatrix(Eigen::MatrixXd &transformation);
    Eigen::MatrixXd rot2eul(Eigen::Matrix3d rot_mat, std::string seq);
};

namespace InputUtilities
{
    void readPointCloud(string filename, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,string metric="m");
    vector<MatrixXd> readTransformations(string filename,bool affine=false,string metric="m");
    vector<double> getTransVector(boost::property_tree::ptree &pt,string s);
    vector<double> getPlaneEquation(boost::property_tree::ptree &pt,string s);
};

namespace InterfaceUtilities
{
    void drawGrid(pcl::visualization::PCLVisualizer::Ptr viewer,double xmin=-3.0,double xmax=3.0,double ymin=-3.0,double ymax=3.0,double gridsize=0.5);
    void addClouds(vector<PointCloudT::Ptr>& clouds,vector<PointCloudT::Ptr>& cloud_outputs,vector<MatrixXd>& iks,vector<double> & flange_transformation,pcl::visualization::PCLVisualizer::Ptr viewer);
    void addObjectToSpace(PointCloudT::Ptr cloud,PointCloudT::Ptr output,vector<double>& transformation,pcl::visualization::PCLVisualizer::Ptr viewer);
    void updateObjectToSpace(PointCloudT::Ptr cloud,PointCloudT::Ptr output,vector<double>& transformation,pcl::visualization::PCLVisualizer::Ptr viewer,bool selected=true);
};
#endif