/**
 * @file Triangulator.hpp
 * @author bwu
 * @brief Triangulator algrithom for triangulation
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_TRI_TRIANGULATOR_HPP
#define GENERIC_GEOMETRY_TRI_TRIANGULATOR_HPP
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometry.hpp>
#include "TriangulationOperator.hpp"
#include <unordered_map>
#include <unordered_set>
#include <limits>
namespace generic  {
namespace geometry {
namespace tri      {
using generic::common::float_type;

template <typename num_type>
class RuppertRefinement2D;

template <typename num_type>
class ChewSecondRefinement2D;

/**
 * @brief represents a class that make points and edges to trangulation
 * @tparam num_type support integer and floating points number type
 */
template <typename num_type>
class Triangulator2D
{
public:
    using Depth = unsigned short;
    using Point = Point2D<num_type>;
    using Box = Box2D<num_type>;
    using Edge = IndexEdge;
    using Vertex = IndexVertex;
    using Triangle = IndexTriangle;

    using EdgeSet = typename Triangulation<Point>::EdgeSet;
    using TriIdxMap = std::unordered_map<TriIdx, TriIdx>;

    Triangulation<Point> & tri;
    TriangulationOperator<Point> op;
    ///@brief constructs a Triangulator2D that manipulating the triangulation data
    Triangulator2D(Triangulation<Point> & t) : tri(t), op(t) {}

    /**
     * @brief inserts vertices and construct delaunay triangulation
     *
     * @tparam VertexIterator iterator of the sequence of vertices
     * @tparam CoorXGetter functioner type to get coordiante x of input vertex
     * @tparam CoorYGetter functioner type to get coordinate y of input vertex
     * @param begin iterator to the beginning of the vertices sequence
     * @param end iterator to the ending of the vertices sequence
     * @param xGetter functioner object to get coordiante x of input vertex
     * @param yGetter functioner object to get coordiante y of input vertex
     */
    template <typename VertexIterator,
              typename CoorXGetter,
              typename CoorYGetter>
    void InsertVertices(VertexIterator begin, VertexIterator end, CoorXGetter xGetter, CoorYGetter yGetter);

    /**
     * @brief inserts edges and construct constrained delaunay triangulation
     *
     * @tparam EdgeIterator iterator of the sequence of edges
     * @tparam sVerIdxGetter functioner type to get start vertex index of edge
     * @tparam eVerIdxGetter functioner type to get end vertex index of edge
     * @param begin iterator to the beginning of the edges sequence
     * @param end iterator to the ending of the edges sequence
     * @param sGetter functioner object to get start vertex index of edge
     * @param eGetter functioner object to get end vertex index of edge
     */
    template <typename EdgeIterator,
              typename sVerIdxGetter,
              typename eVerIdxGetter>
    void InsertEdges(EdgeIterator begin, EdgeIterator end, sVerIdxGetter sGetter, eVerIdxGetter eGetter);

    ///@brief removes super triangle and produce a convex-hull
    void EraseSuperTriangle();
    ///@brief removes all outer triangles until a boundary defined by constrained edges
    void EraseOuterTriangles();
    /**
     * @brief removes outer triangles and automatically detected holes.
     * Starts from super-triangle and traverses triangles until outer boundary.
     * Triangles outside outer boundary will be removed.
     * Then traversal continues until next boundary.
     * hTriangles between two boundaries will be kept.
     * Traversal to next boundary continues (this time removing triangles).
     * Stops when all triangles are traversed.
     */
    void EraseOuterTrianglesAndHoles();

    ///@brief clears all data in triangulation
    void Clear();
private:
    template <typename VertexIterator,
                typename CoorXGetter,
                typename CoorYGetter>
    Box Extent(VertexIterator begin, VertexIterator end, CoorXGetter xGetter, CoorYGetter yGetter);

    void AddSuperTriangle(const Box & box);

    void InsertOneEdge(const Edge & edge);
    void InsertOneVertex(const Point & pos);

    VerIdx InsertPointInTriangle(const Point & pos, TriIdx i0, std::stack<TriIdx> & out);
    VerIdx InsertPointOnSharedEdge(const Point & pos, TriIdx i1, TriIdx i2, std::stack<TriIdx> & out);
    VerIdx InsertPointOnBoundaryEdge(const Point & pos, TriIdx it, const Edge & e, std::stack<TriIdx> & out);

    void Delaunay(std::stack<TriIdx> & triangls, VerIdx iv);
    bool isNeedFlip(VerIdx iv, TriIdx it, TriIdx itOp) const;
    bool isInCircumCircle(const Point & pos, const Point & v1, const Point & v2, const Point & v3) const;

    std::tuple<TriIdx, VerIdx, VerIdx> IntersectedTriangle(VerIdx iA,
                                                            const TriIdxSet & candidates,
                                                            const Point & a, const Point & b) const;
    void RemoveSuperTriangleVertices();
    template <typename TriIdxIterator>
    void RemoveTriangles(TriIdxIterator begin, TriIdxIterator end);

    TriIdx TriangulatePseudoPolygon(VerIdx ia, VerIdx ib, const std::vector<VerIdx> & points);
    TriIdx PesudoPolyOuterTriangle(VerIdx ia, VerIdx ib) const;
    VerIdx FindDelaunayPoint(VerIdx ia, VerIdx ib, const std::vector<VerIdx> & points) const;
    std::pair<std::vector<VerIdx>, std::vector<VerIdx> >
    SplitPsedoPolygon(VerIdx vi, const std::vector<VerIdx> & points);

    TriIdxSet Grow2Boundary(std::stack<TriIdx> & seeds) const;
    std::vector<Depth> CalculateTriangleDepths(const std::vector<Vertex> & vertices,
                                                const TriangleVec & triangles,
                                                const EdgeSet & fixedEdges);
    TriIdxSet PeelLayer(std::stack<TriIdx> seeds,
                        const TriangleVec & triangles,
                        const EdgeSet & fixedEdges,
                        Depth depth,
                        std::vector<Depth> & triDepths);

    void RebuildRTree();
};

template <typename num_type>
template <typename VertexIterator, typename CoorXGetter, typename CoorYGetter>
inline void Triangulator2D<num_type>::InsertVertices(VertexIterator begin, VertexIterator end, CoorXGetter xGetter, CoorYGetter yGetter)
{
    if(tri.vertices.empty()){
        auto bounds = Extent(begin, end, xGetter, yGetter);
        AddSuperTriangle(bounds);
    }

    tri.vertices.reserve(tri.vertices.size() + std::distance(begin, end));
    for(auto iter = begin; iter != end; ++iter){
        InsertOneVertex(Point(xGetter(*iter), yGetter(*iter)));
    }
}

template <typename num_type>
template <typename EdgeIterator, typename sVerIdxGetter, typename eVerIdxGetter>
inline void Triangulator2D<num_type>::InsertEdges(EdgeIterator begin, EdgeIterator end, sVerIdxGetter sGetter, eVerIdxGetter eGetter)
{
    for(auto iter = begin; iter != end; ++iter){
        InsertOneEdge(Edge(sGetter(*iter) + 3, eGetter(*iter) + 3));
    }
    op.ReallocateTriangulation();
}

template <typename num_type>
inline void Triangulator2D<num_type>::EraseSuperTriangle()
{
    TriIdxVec toErase;
    for(TriIdx it = 0; it < tri.triangles.size(); ++it){
        auto & t = tri.triangles[it];
        if(t.vertices[0] < 3 || t.vertices[1] < 3 || t.vertices[2] < 3)
            toErase.push_back(it);
    }
    RemoveTriangles(toErase.begin(), toErase.end());
    RemoveSuperTriangleVertices();
}

template <typename num_type>
inline void Triangulator2D<num_type>::EraseOuterTriangles()
{
    std::stack<TriIdx> seed(std::deque<TriIdx>(1, *(tri.vertices[0].triangles.begin())));
    TriIdxSet toErase = Grow2Boundary(seed);
    RemoveTriangles(toErase.begin(), toErase.end());
    RemoveSuperTriangleVertices();
}

template <typename num_type>
inline void Triangulator2D<num_type>::EraseOuterTrianglesAndHoles()
{
    auto triDepths = CalculateTriangleDepths(tri.vertices, tri.triangles, tri.fixedEdges);

    TriIdxVec toErase;
    toErase.reserve(tri.triangles.size());
    for(size_t it = 0; it != tri.triangles.size(); ++it){
        if(triDepths[it] % 2 == 0)
            toErase.push_back(it);
    }
    RemoveTriangles(toErase.begin(), toErase.end());
    RemoveSuperTriangleVertices();
}

template <typename num_type>
template <typename VertexIterator, typename CoorXGetter, typename CoorYGetter>
inline Box2D<num_type> Triangulator2D<num_type>::Extent(VertexIterator begin, VertexIterator end, CoorXGetter xGetter, CoorYGetter yGetter)
{
    Box box;
    for(auto iter = begin; iter != end; ++iter)
        box |= Point(xGetter(*iter), yGetter(*iter));
    return box;
}

template <typename num_type>
inline void Triangulator2D<num_type>::AddSuperTriangle(const Box & box)
{
    auto center = box.Center();
    auto l = box.Length();
    auto w = box.Width();
    l *= l; w *= w;
    auto r = 0.5 * 1.1 * std::sqrt(float_type<num_type>(l + w));
    auto sx = r * std::sqrt(3);
    VerIdx iv1 = op.AddOneVertex(Point(center[0] - sx, center[1] - r), {0});
    VerIdx iv2 = op.AddOneVertex(Point(center[0] + sx, center[1] - r), {0});
    VerIdx iv3 = op.AddOneVertex(Point(center[0], center[1] + 2 * r), {0});
    Triangle super{{iv1, iv2, iv3}, {noNeighbor, noNeighbor, noNeighbor}};
    op.AddOneTriangle(super);
}

template <typename num_type>
inline void Triangulator2D<num_type>::InsertOneVertex(const Point & pos)
{
    VerIdx iv;
    std::stack<TriIdx> tris;
    auto [it1, it2] = op.TraversalTriangleAt(pos);
    if(noNeighbor == it2) iv = op.InsertPointInTriangle(pos, it1, tris);
    else iv = op.InsertPointOnSharedEdge(pos, it1, it2, tris);
    Delaunay(tris, iv);
}

template <typename num_type>
inline void Triangulator2D<num_type>::InsertOneEdge(const Edge & edge)
{
    auto is = edge.v1();
    auto ie = edge.v2();
    if(is == ie) return;

    const Vertex & vs = tri.vertices[is];
    const Vertex & ve = tri.vertices[ie];

    if(Vertex::isShareEdge(vs, ve)){
        tri.fixedEdges.insert(Edge(is, ie));
        return;
    }

    TriIdx it;
    VerIdx ivLeft, ivRight;
    std::tie(it, ivLeft, ivRight) = IntersectedTriangle(is, vs.triangles, tri.VertexPoint(vs), tri.VertexPoint(ve));
    if(it == noNeighbor){
        tri.fixedEdges.insert(Edge(is, ivLeft));
        return InsertOneEdge(Edge(ivLeft, ie));
    }

    std::vector<TriIdx> intersected(1, it);
    std::vector<VerIdx> ptsLeft(1, ivLeft);
    std::vector<VerIdx> ptsRight(1, ivRight);
    VerIdx iv = is;
    Triangle t = tri.triangles[it];
    const auto & vrts = t.vertices;
    while(std::find(vrts.begin(), vrts.end(), ie) == vrts.end()){

        auto itOp = t.VeOpTn(iv);
        const Triangle & tOp = tri.triangles[itOp];
        auto ivOp = tOp.TnOpVe(it);
        auto vOp = tri.vertices[ivOp];

        intersected.push_back(itOp);
        it = itOp;
        t = tri.triangles[it];

        PointLineLocation loc = GetPointLineLocation(tri.VertexPoint(vOp), tri.VertexPoint(vs), tri.VertexPoint(ve));
        if(loc == PointLineLocation::Left){
            ptsLeft.push_back(ivOp);
            iv = ivLeft;
            ivLeft = ivOp;
        }
        else if(loc == PointLineLocation::Right){
            ptsRight.push_back(ivOp);
            iv = ivRight;
            ivRight = ivOp;
        }
        else { ie = ivOp; }
    }

    auto iter = intersected.begin();
    for(; iter != intersected.end(); ++iter)
        op.RemoveOneTriangle(*iter);

    auto itLeft = TriangulatePseudoPolygon(is, ie, ptsLeft);
    std::reverse(ptsRight.begin(), ptsRight.end());
    auto itRight = TriangulatePseudoPolygon(ie, is, ptsRight);

    tri.ChangeNeighbor(itLeft, noNeighbor, itRight);
    tri.ChangeNeighbor(itRight, noNeighbor, itLeft);

    tri.fixedEdges.insert(Edge(is, ie));
    if(ie != edge.v2())
        return InsertOneEdge(Edge(ie, edge.v2()));
}

template <typename num_type>
inline void Triangulator2D<num_type>::Delaunay(std::stack<TriIdx> & triangles, VerIdx iv)
{
    while(!triangles.empty()){
        TriIdx it = triangles.top();
        triangles.pop();

        const auto & triangle = tri.triangles[it];
        TriIdx itOp = triangle.VeOpTn(iv);
        if(itOp == noNeighbor) continue;
        if(isNeedFlip(iv, it, itOp)){
            tri.FlipEdge(it, itOp);
            triangles.push(it);
            triangles.push(itOp);
        }
    }
}

template <typename num_type>
inline bool Triangulator2D<num_type>::isNeedFlip(VerIdx iv, TriIdx it, TriIdx itOp) const
{
    const Triangle & triOp = tri.triangles[itOp];
    size_t i = triOp.TnOpiVe(it);
    VerIdx ivOp = triOp.vertices[i];
    if(iv < 3 && ivOp < 3) return false;

    VerIdx ivCw  = triOp.vertices[Triangle:: cw(i)];
    VerIdx ivCcw = triOp.vertices[Triangle::ccw(i)];
    const Point & p0 = tri.VerIdxPoint(iv);
    const Point & p1 = tri.VerIdxPoint(ivCw);
    const Point & p2 = tri.VerIdxPoint(ivOp);
    const Point & p3 = tri.VerIdxPoint(ivCcw);
    if(ivCw < 3) return GetPointLineLocation(p1, p2, p3) == GetPointLineLocation(p0, p2, p3);
    else if(ivCcw < 3) return GetPointLineLocation(p3, p1, p2) == GetPointLineLocation(p0, p1, p2);
    else return isInCircumCircle(p0, p1, p2, p3);
}

template <typename num_type>
inline bool Triangulator2D<num_type>::isInCircumCircle(const Point & p, const Point & p1, const Point & p2, const Point & p3) const
{
    return geometry::isInCircumCircle(p1, p2, p3, p, false);
}

template <typename num_type>
inline std::tuple<TriIdx, VerIdx, VerIdx>
Triangulator2D<num_type>::IntersectedTriangle(VerIdx iA,
                                                const TriIdxSet & candidates,
                                                const Point & a, const Point & b) const
{
    for(TriIdx it : candidates){
        const Triangle & triangle = tri.triangles[it];
        auto i = triangle.iVe(iA);
        auto iv1 = triangle.vertices[Triangle::cw(i)];
        auto iv2 = triangle.vertices[Triangle::ccw(i)];
        auto locP1 = GetPointLineLocation(tri.VerIdxPoint(iv1), a, b);
        auto locP2 = GetPointLineLocation(tri.VerIdxPoint(iv2), a, b);
        if(locP2 == PointLineLocation::Right){
            if(locP1 == PointLineLocation::OnLine)
                return std::make_tuple(noNeighbor, iv1, iv2);
            else if(locP1 == PointLineLocation::Left)
                return std::make_tuple(it, iv1, iv2);
        }
    }
    throw std::runtime_error("Could not find vertex triangle intersected by "
                             "edge. Note: can be caused by duplicate points.");
}

template <typename num_type>
inline void Triangulator2D<num_type>::RemoveSuperTriangleVertices()
{
    for(VerIdx iv = 0; iv < 3; ++iv)
        op.RemoveOneVertex(iv);
    op.ReallocateTriangulation();
}

template <typename num_type>
template <typename TriIdxIterator>
void Triangulator2D<num_type>::RemoveTriangles(TriIdxIterator begin, TriIdxIterator end)
{
    for(auto iter = begin; iter != end; ++iter)
        op.RemoveOneTriangle(*iter);
    op.ReallocateTriangulation();
}

template <typename num_type>
inline TriIdx Triangulator2D<num_type>::TriangulatePseudoPolygon(VerIdx ia, VerIdx ib, const std::vector<VerIdx> & points)
{
    if(points.empty())
        return PesudoPolyOuterTriangle(ia, ib);

    VerIdx ic = FindDelaunayPoint(ia, ib, points);
    auto split = SplitPsedoPolygon(ic, points);
    auto it2 = TriangulatePseudoPolygon(ic, ib, split.second);
    auto it1 = TriangulatePseudoPolygon(ia, ic, split.first);
    Triangle t = {{ia, ib, ic}, {noNeighbor, it2, it1}};
    TriIdx it = op.AddOneTriangle(t);
    if(it1 != noNeighbor){
        if(split.first.empty())
            tri.ChangeNeighbor(it1, ia, ic, it);
        else
            tri.triangles[it1].neighbors[0] = it;
    }
    if(it2 != noNeighbor){
        if(split.second.empty())
            tri.ChangeNeighbor(it2, ic, ib, it);
        else
            tri.triangles[it2].neighbors[0] = it;
    }
    tri.AddAdjacentTriangle(ia, it);
    tri.AddAdjacentTriangle(ib, it);
    tri.AddAdjacentTriangle(ic, it);
    return it;
}

template <typename num_type>
inline TriIdx Triangulator2D<num_type>::PesudoPolyOuterTriangle(VerIdx ia, VerIdx ib) const
{
    const auto & aTris = tri.vertices[ia].triangles;
    const auto & bTris = tri.vertices[ib].triangles;
    for(auto iter = aTris.begin(); iter != aTris.end(); ++iter){
        if(std::find(bTris.begin(), bTris.end(), *iter) != bTris.end())
            return *iter;
    }
    return noNeighbor;
}

template <typename num_type>
inline VerIdx Triangulator2D<num_type>::FindDelaunayPoint(VerIdx ia, VerIdx ib, const std::vector<VerIdx> & vs) const
{
    assert(!vs.empty());
    const auto & a = tri.VerIdxPoint(ia);
    const auto & b = tri.VerIdxPoint(ib);
    VerIdx ic = vs.front();
    auto c = tri.VerIdxPoint(ic);
    for(auto iter = vs.begin(); iter != vs.end(); ++iter){
        const auto & v = tri.VerIdxPoint(*iter);
        if(!isInCircumCircle(v, a, b, c)) continue;
        ic = *iter;
        c = tri.VerIdxPoint(ic);
    }
    return ic;
}

template <typename num_type>
inline std::pair<std::vector<VerIdx>, std::vector<VerIdx> >
Triangulator2D<num_type>::SplitPsedoPolygon(VerIdx vi, const std::vector<VerIdx> & points)
{
    std::pair<std::vector<VerIdx>, std::vector<VerIdx> > out;
    auto iter = points.begin();
    for(; vi != *iter; ++iter)
        out.first.push_back(*iter);
    for(++iter; iter != points.end(); ++iter)
        out.second.push_back(*iter);
    return out;
}

template <typename num_type>
inline TriIdxSet Triangulator2D<num_type>::Grow2Boundary(std::stack<TriIdx> & seeds) const
{
    TriIdxSet traversed;
    while(!seeds.empty()){
        auto it = seeds.top();
        seeds.pop();
        traversed.insert(it);
        const auto & triange = tri.triangles[it];
        for(auto i = 0; i < 3; ++i){
            Edge opEdge = Edge(
                        triange.vertices[Triangle::ccw(i)],
                        triange.vertices[Triangle::cw(i)]);
            if(tri.fixedEdges.count(opEdge)) continue;
            TriIdx in = triange.neighbors[Triangle::iVeOpiTn(i)];
            if(in != noNeighbor && !traversed.count(in))
                seeds.push(in);
        }
    }
    return traversed;
}

template <typename num_type>
inline std::vector<typename Triangulator2D<num_type>::Depth>
Triangulator2D<num_type>::CalculateTriangleDepths(const std::vector<Vertex> & vertices,
                                                    const TriangleVec & triangles,
                                                    const EdgeSet & fixedEdges)
{
    std::vector<Depth> triDepths(triangles.size(), std::numeric_limits<Depth>::max());

    using TriDeque = std::deque<TriIdx>;
    using TriStack = std::stack<TriIdx>;
    TriStack seeds(TriDeque(1, *(tri.vertices[0].triangles.begin())));
    Depth layerDepth = 0;
    do{
        auto newSeeds = PeelLayer(seeds, triangles, fixedEdges, layerDepth++, triDepths);
        seeds = TriStack(TriDeque(newSeeds.begin(), newSeeds.end()));
    } while(!seeds.empty());

    return triDepths;
}

template <typename num_type>
inline TriIdxSet Triangulator2D<num_type>::PeelLayer(std::stack<TriIdx> seeds,
                                        const TriangleVec & triangles,
                                        const EdgeSet & fixedEdges,
                                        Depth depth,
                                        std::vector<Depth> & triDepths)
{
    TriIdxSet behindBoundary;
    while(!seeds.empty()){
        auto it = seeds.top();
        seeds.pop();
        triDepths[it] = depth;
        behindBoundary.erase(it);
        const auto & triangle = triangles[it];
        for(auto i = 0; i < 3; ++i){
            Edge opEdge(triangle.vertices[Triangle::ccw(i)],
                        triangle.vertices[Triangle:: cw(i)]);
            TriIdx in = triangle.neighbors[Triangle::iVeOpiTn(i)];
            if(in == noNeighbor || triDepths[in] <= depth) continue;
            if(fixedEdges.count(opEdge)){
                behindBoundary.insert(in);
                continue;
            }
            seeds.push(in);
        }
    }
    return behindBoundary;
}

template <typename num_type>
inline void Triangulator2D<num_type>::Clear()
{
    op.Clear();
}

template <typename num_type>
class ConvexTriangulator2D
{
    struct DoublyLinkedIndex { VerIdx prev, curr, next; };
public:
    using float_t = float_type<num_type>;
    using Point = Point2D<num_type>;
    using Edge = IndexEdge;
    using Vertex = IndexVertex;
    using Triangle = IndexTriangle;
    using Utility = TriangulationUtility<Point>;
    using EdgeSet = typename Triangulation<Point>::EdgeSet;
    using TriIdxMap = std::unordered_map<TriIdx, TriIdx>;

    Triangulation<Point> & tri;
    TriangulationOperator<Point> op;
    ConvexTriangulator2D(Triangulation<Point> & t) : tri(t), op(t) { op.Clear(); }

    template <typename VertexIterator,
              typename CoorXGetter,
              typename CoorYGetter>
    void Triangulate(VertexIterator begin, VertexIterator end, CoorXGetter xGetter, CoorYGetter yGetter)
    {
        const size_t size = std::distance(begin, end);
        assert(size >= 3);

        size_t i = 0;
        std::vector<DoublyLinkedIndex> vertices(size);
        for(auto iter = begin; iter != end; ++iter){
            vertices[i++].curr = op.AddOneVertex(Point(xGetter(*iter), yGetter(*iter)));
        }

        for(size_t i = 0; i < size; ++i){
            vertices[i].prev = vertices[(i + size - 1) % size].curr;
            vertices[i].next = vertices[(i + 1) % size].curr;
        }

        for(size_t i = size - 1; i > 3; --i){
            vertices[i - 1].next = vertices[i].next;
        }

        TriIdx it = AddTriangle(vertices[0].curr, vertices[1].curr, vertices[2].curr);
        for(size_t i = 3; i < size; ++i)
            ConvexInsertVertex(vertices[i].curr, Edge(vertices[i].prev, vertices[i].next));

        op.ReallocateTriangulation();

    }

private:

    TriIdx AddTriangle(VerIdx iv1, VerIdx iv2, VerIdx iv3)
    {
        return op.AddOneTriangle(iv1, iv2, iv3);
    }

    void ConvexInsertVertex(VerIdx iv, const Edge & e)
    {
        TriIdx it;
        std::tie(it, std::ignore) = tri.GetTriangles(e);
        VerIdx iu = e.v1(), iw = e.v2();
        if(noNeighbor == it){
            AddTriangle(iv, iu, iw);
            return;
        }
        VerIdx ix = tri.triangles[it].EgOpVe(e);
        if(Utility::isInCircumCircle(tri, it, tri.VerIdxPoint(iv))){
            op.RemoveOneTriangle(it);
            ConvexInsertVertex(iv, Edge(iu, ix));
            ConvexInsertVertex(iv, Edge(ix, iw));
        }
        else AddTriangle(iv, iu, iw);
    }
};

struct DuplicatesInfo
{
    std::vector<size_t> mapping;
    std::unordered_set<size_t> duplicates;
};

/**
 * @brief utility functions to find duplicated points within given tolerance
 * @param[in] points point collection
 * @param[in] tolerance treat points distance less than tolerance as duplicated
 * @return DuplicatesInfo duplicate results
 */
template <typename num_type>
inline DuplicatesInfo FindDuplicates(const std::vector<Point2D<num_type> > & points, num_type tolerance)
{
    using Grid = std::pair<size_t, size_t>;
    struct GridHash
    {
        size_t operator() (const Grid & grid) const noexcept
        {
            size_t seed(0);
            boost::hash_combine(seed, grid.first);
            boost::hash_combine(seed, grid.second);
            return seed;
        }
    };

    struct GridCmp
    {
        bool operator() (const Grid & g1, const Grid & g2) const noexcept
        {
            return g1 == g2;
        }
    };

    using GridIdxMap = std::unordered_map<Grid, size_t, GridHash, GridCmp>;
    const size_t shift = std::numeric_limits<size_t>::max() / 2;
    auto toGrid = [tolerance, shift](const Point2D<num_type> & p)
    {
        size_t x = static_cast<size_t>(p[0] / tolerance + shift);
        size_t y = static_cast<size_t>(p[1] / tolerance + shift);
        return std::make_pair(x, y);
    };

    GridIdxMap gridIdxMap;
    DuplicatesInfo dup { std::vector<size_t>(points.size()), std::unordered_set<size_t>() };
    for(size_t iIn = 0, iOut = iIn; iIn < points.size(); ++iIn){
        bool isUnique;
        typename GridIdxMap::const_iterator iter;
        std::tie(iter, isUnique) = gridIdxMap.insert(std::make_pair(toGrid(points[iIn]), iOut));
        if(isUnique){
            dup.mapping[iIn] = iOut++;
            continue;
        }
        dup.mapping[iIn] = iter->second;
        dup.duplicates.insert(iIn);
    }
    return dup;
}

/**
 * @brief remaps edges with duplicated functions
 * @param[in, out] edges input edges
 * @param[in] mapping duplicated info, point n is duplicated with mapping[n]
 */
inline void RemapEdges(std::list<IndexEdge> & edges, const std::vector<size_t> & mapping)
{
    for(auto iter = edges.begin(); iter != edges.end();){
        iter->SetVertices(mapping[iter->v1()], mapping[iter->v2()]);
        if(iter->v1() == iter->v2())
            iter = edges.erase(iter);
        else ++iter;
    }
}

/**
 * @brief removes duplicate points of a point collection
 * @param[in, out] points the given point collection
 * @param duplicates duplicated info, point n is duplicated with mapping[n]
 */
template <typename num_type>
inline void RemoveDuplicates(std::vector<Point2D<num_type> > & points, const std::unordered_set<size_t> & duplicates)
{
    for(size_t i = 0, iNew = i; i < points.size(); ++i){
        if(duplicates.count(i)) continue;
        points[iNew] = points[i];
        iNew++;
    }
    points.erase(points.end() - duplicates.size(), points.end());
}

/**
 * @brief removes duplicate points and remap edges
 * @param[in, out] points the given point collection
 * @param[in, out] edges the given edge collection
 * @param[in] tolerance merge tolerance
 * @return DuplicatesInfo duplicate results
 */
template <typename num_type>
inline DuplicatesInfo RemoveDuplicatesAndRemapEdges(std::vector<Point2D<num_type> > & points, std::list<IndexEdge> & edges, num_type tolerance)
{
    DuplicatesInfo dup = FindDuplicates(points, tolerance);
    RemoveDuplicates(points, dup.duplicates);
    RemapEdges(edges, dup.mapping);
    return dup;
}

}//namespace tri
}//namespace geometry
}//namespace generic
#endif//GENERIC_GEOMETRY_TRI_TRIANGULATOR_HPP


#include <iostream>
#include <time.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/random.hpp>
#include <boost/program_options.hpp>

using namespace std;
using namespace boost::numeric::ublas;
using namespace boost::random;
using namespace boost::program_options;

int main (int argc, char* argv[]) {
    int scale = 2, depth = 2;

    try {
        options_description opts("options");
        opts.add_options()
            ("help,h", "help message")
            ("scale,S", value<int>()->implicit_value(2), "2^scale motif verticies")
            ("depth,D", value<int>()->implicit_value(2), "recursive depth");

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

        if (vm.count("help")) {
            cout << opts << endl;
            return 0;
        }

        if (vm.count("scale")) scale = vm["scale"].as<int>();
        if (vm.count("depth")) depth = vm["depth"].as<int>();
    } catch(exception& e) {
        cout << e.what() << endl;
        cout << "use '-h' to get help." << endl;
        return -1;
    }

    unsigned motif_size  = 1 << scale;
    unsigned result_size = 1 << (scale * (depth + 1));
    unsigned motif_mask  = motif_size - 1;

    cout << result_size << endl;

    boost::mt19937 rng(time(0));
    boost::uniform_int<> binary(0, 1);

    matrix<int> motif(motif_size, motif_size);

    for (unsigned i = 0; i < motif_size; ++ i)
        for (unsigned j = 0; j < motif_size; ++ j)
            motif(i, j) = (j < i) ? binary(rng) : 0;

    motif += trans(motif);

    cout << motif << endl;

    auto kronecker_cell = [=](unsigned i, unsigned j) {
        int cell = 1;
        unsigned motif_i = i, motif_j = j;
        for (unsigned d = 0; d < depth + 1; ++ d) {
            cell *= motif(motif_i & motif_mask, motif_j & motif_mask);
            motif_i >>= scale;
            motif_j >>= scale;
        }
        return cell;
    };

    for (unsigned i = 0; i < result_size; ++ i ) {
        for (unsigned j = 0; j < result_size; ++ j)
           cout << kronecker_cell(i, j) << ' ';
        cout << endl;
    }

    return 0;
}


#include "advent.hpp"
#include "intersect.hpp"

#include <iostream>
#include <iomanip>
#include <fstream>
#include <numeric>
#include <vector>
#include <tuple>
#include <ranges>
#include <string>
#include <scn/scn.h>
#include <fmt/ranges.h>
#include <set>

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

using std::vector;
using std::pair;
using std::string;
using std::ifstream;
using std::ranges::stable_sort;

struct def {
    static constexpr i64 nd{3}; // number of dimensions
    static constexpr i64 ncommon{12}; // number of beacons in common
};

using point = std::tuple<i64, i64, i64>;

struct scanner {
    i64 id;
    std::optional<point> position; // scanner position
    Eigen::MatrixXd beacons; // beacon positions
    Eigen::Matrix<i64, -1, -1> distances; // distance matrix between beacons
    Eigen::Matrix<i64, -1, -1> sorted_distances; // distance matrix between beacons

    explicit scanner(i64 i, Eigen::MatrixXd b)
        : id(i)
        , position(std::nullopt)
        , beacons(std::move(b))
        , distances(compute_distance_matrix(beacons))
        , sorted_distances(distances)
    {
        // sort the values so we can use intersection count
        for (auto col : sorted_distances.colwise()) {
            stable_sort(col);
        }
    }

    static auto compute_distance_matrix(auto const& m) -> Eigen::Matrix<i64, -1, -1>
    {
        Eigen::Matrix<i64, -1, -1> d = decltype(d)::Zero(m.cols(), m.cols());
        for (auto i = 0L; i < m.cols()-1; ++i) {
            for (auto j = i+1; j < m.cols(); ++j) {
                d(i, j) = d(j, i) = static_cast<i64>((m.col(i) - m.col(j)).matrix().squaredNorm());
            }
        }
        return d;
    }

    [[nodiscard]] auto find_common_beacons(scanner const& s) const
    {
        vector<pair<i64, i64>> map;
        for (auto i = 0L; i < distances.cols(); ++i) {
            auto ci = sorted_distances.col(i);
            for (auto j = 0L; j < s.distances.cols(); ++j) {
                auto cj = s.sorted_distances.col(j);
                auto count = detail::count_intersect(ci.data(), ci.size(), cj.data(), cj.size());
                if(count == def::ncommon) {
                    vector<i64> chosen(s.distances.cols(), 0);
                    for (auto k = 0L; k < distances.cols(); ++k) {
                        for (auto l = 0L; l < s.distances.cols(); ++l) {
                            if (!chosen[l] && distances(k, i) == s.distances(l, j)) {
                                chosen[l] = 1;
                                map.emplace_back(l, k);
                            }
                        }
                    }
                    return map;
                }
            }
        }
        return map;
    }

    auto translate(scanner const& dest, vector<pair<i64, i64>> const& map) -> void
    {
        vector<i64> idx; idx.reserve(map.size());
        vector<i64> dest_idx; dest_idx.reserve(map.size());
        for(auto [i, j] : map) {
            idx.push_back(j);
            dest_idx.push_back(i);
        }
        Eigen::MatrixXd tr = Eigen::umeyama(beacons(Eigen::all, idx), dest.beacons(Eigen::all, dest_idx)).array().round();
        auto t = tr.col(tr.cols()-1).template cast<i64>();
        position = {t(0), t(1), t(2)};
        beacons = (tr * beacons.colwise().homogeneous()).block(0, 0, beacons.rows(), beacons.cols());
    }

    friend auto operator<<(std::ostream& os, scanner const& s) -> std::ostream&;
};

auto operator<<(std::ostream& os, scanner const& s) -> std::ostream&
{
    os << "scanner " << s.id << " position: ";
    if (s.position) {
        auto [x, y, z] = *s.position;
        os << "(" << x << "," << y << "," << z << ")\n";
    }
    else { os << "(?,?,?)\n"; }
    os << "beacons:\n";
    os << s.beacons << "\n";
    return os;
}

auto read_input(int argc, char** argv) {
    // read input
    if (argc < 2) {
        throw std::runtime_error("Error: no input.");
    }

    ifstream infile(argv[1]);
    string line;

    vector<scanner> scanners;

    vector<vector<double>> values;
    i64 id{0};
    while (std::getline(infile, line)) {
        if (line[0] == '-' && line[1] == '-') { continue; }
        if (line.empty()) {
            Eigen::MatrixXd beacons(def::nd, values.size());
            for (auto i = 0L; i < std::ssize(values); ++i) {
                beacons.col(i) = Eigen::Map<Eigen::Array<double, -1, 1>>(values[i].data(), std::ssize(values[i]));
            }
            scanners.emplace_back(id++, std::move(beacons));
            values.clear();
            continue;
        }
        values.emplace_back();
        scn::scan_list(line, values.back(), ',');
    }
    scanners[0].position = {0, 0, 0};

    return scanners;
}

auto day19(int argc, char** argv) -> int
{
    auto scanners = read_input(argc, argv);
    vector<scanner> known{scanners[0]};

    while (known.size() < scanners.size()) {
        auto sz = std::ssize(known);
        for (auto i = 0L; i < sz; ++i) {
            auto const& s = known[i];
            for (auto& p : scanners) {
                if (p.position) { continue; }
                if (auto map = p.find_common_beacons(s); std::ssize(map) == def::ncommon) {
                    p.translate(s, map);
                    known.push_back(p);
                    break;
                }
            }
        }
    }

    std::set<point> unique_beacons;
    auto manhattan = 0L;
    for (auto const& s : scanners) {
        for (auto col : s.beacons.colwise()) {
            auto t = col.template cast<i64>();
            unique_beacons.insert({t(0), t(1), t(2)});
        }
        auto [x1, y1, z1] = *s.position;
        Eigen::Array<i64, 3, 1> v{x1, y1, z1};
        for (auto const& p : scanners) {
            auto [x2, y2, z2] = *p.position;
            Eigen::Array<i64, 3, 1> u{x2, y2, z2};
            manhattan = std::max(manhattan, (u - v).abs().sum());
        }
    }
    fmt::print("part 1: {}\n", unique_beacons.size());
    fmt::print("part 2: {}\n", manhattan);

    return 0;
}


/*
    This file is part of utils-lib.

    Copyright (c) 2020, 2021, 2022 Bernardo Fichera <bernardo.fichera@gmail.com>

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef utils_lib_GRADIENT_CHECKER_HPP
#define utils_lib_GRADIENT_CHECKER_HPP

#include <Eigen/Core>
#include <iostream>

namespace utils_lib {
    template <typename Precision = double>
    class DerivativeChecker {
    public:
        DerivativeChecker(const size_t& dim = 1, const size_t& res = 51) : _dim(dim), _res(res) {}

        DerivativeChecker& setResolution(const size_t& res)
        {
            _res = res;
            return *this;
        }

        DerivativeChecker& setDimension(const size_t& dim)
        {
            _dim = dim;
            return *this;
        }

        Eigen::Matrix<Precision, Eigen::Dynamic, Eigen::Dynamic> numericalGradient()
        {
            Eigen::Matrix<Precision, Eigen::Dynamic, Eigen::Dynamic> M(_t.rows(), 2);
            M << _t, _G;

            return M;
        }

        Eigen::Matrix<Precision, Eigen::Dynamic, Eigen::Dynamic> numericalHessian()
        {
            Eigen::Matrix<Precision, Eigen::Dynamic, Eigen::Dynamic> M(_t.rows(), 2);
            M << _t, _H;

            return M;
        }

        template <typename Function, typename Gradient>
        bool checkGradient(Function f, Gradient g, const Eigen::VectorXd& x, const Eigen::VectorXd& v)
        {
            // Init G
            _G.setZero(_res);

            // Generate log spaced perturbation intensities
            _t = Eigen::Matrix<Precision, Eigen::Dynamic, 1>::LinSpaced(_res, -8, 0);
            for (size_t i = 0; i < _t.rows(); i++)
                _t(i) = std::pow(10, _t(i));

            // Calculate first order Taylor expansion
            for (size_t i = 0; i < _t.rows(); i++)
                _G(i) = std::abs(f(x + _t(i) * v) - f(x) - _t(i) * g(x).dot(v));

            // Transform to log space
            _t = _t.array().log();
            _G = _G.array().log();

            // Calculate mean slope for central values
            Precision mean_slope = 0, num_points = 0;

            for (size_t i = 20; i < _res - 20; i++) {
                mean_slope = mean_slope + (_G(i + 1) - _G(i)) / (_t(i + 1) - _t(i));
                num_points++;
            }

            mean_slope /= num_points;

            std::cout << "First order Taylor expansion slope: " << mean_slope << " - It should be approximately equal to 2.0" << std::endl;

            return (std::abs(mean_slope - 2.0) <= 1e-3) ? true : false;
        }

        template <typename Function, typename Gradient>
        bool checkGradient(Function f, Gradient g)
        {
            // Generate random test point and perturbation direction
            Eigen::VectorXd x = Eigen::VectorXd::Random(_dim), v = Eigen::VectorXd::Random(_dim);

            // Init G
            _G.setZero(_res);

            // Generate log spaced perturbation intensities
            _t = Eigen::Matrix<Precision, Eigen::Dynamic, 1>::LinSpaced(_res, -8, 0);
            for (size_t i = 0; i < _t.rows(); i++)
                _t(i) = std::pow(10, _t(i));

            // Calculate first order Taylor expansion
            for (size_t i = 0; i < _t.rows(); i++)
                _G(i) = std::abs(f(x + _t(i) * v) - f(x) - _t(i) * g(x).dot(v));

            // Transform to log space
            _t = _t.array().log();
            _G = _G.array().log();

            // Calculate mean slope for central values
            Precision mean_slope = 0, num_points = 0;

            for (size_t i = 20; i < _res - 20; i++) {
                mean_slope = mean_slope + (_G(i + 1) - _G(i)) / (_t(i + 1) - _t(i));
                num_points++;
            }

            mean_slope /= num_points;

            std::cout << "First order Taylor expansion slope: " << mean_slope << " - It should be approximately equal to 2.0" << std::endl;

            return (std::abs(mean_slope - 2.0) <= 1e-3) ? true : false;
        }

        template <typename Function, typename Gradient, typename Hessian>
        bool checkHessian(Function f, Gradient g, Hessian h, const Eigen::VectorXd& x, const Eigen::VectorXd& v)
        {
            // Init H
            _H.setZero(_res);

            // Generate log spaced perturbation intensities
            _t = Eigen::Matrix<Precision, Eigen::Dynamic, 1>::LinSpaced(_res, -8, 0);
            for (size_t i = 0; i < _t.rows(); i++)
                _t(i) = std::pow(10, _t(i));

            // Calculate second order Taylor expansion
            for (size_t i = 0; i < _t.rows(); i++)
                _H(i) = std::abs(f(x + _t(i) * v) - f(x) - _t(i) * g(x).dot(v) - 0.5 * std::pow(_t(i), 2) * h(x, v).dot(v));

            // Transform to log space
            _t = _t.array().log();
            _H = _H.array().log();

            // Calculate mean slope for central values
            Precision mean_slope = 0, num_points = 0;

            for (size_t i = 30; i < _res - 10; i++) {
                mean_slope = mean_slope + (_H(i + 1) - _H(i)) / (_t(i + 1) - _t(i));
                num_points++;
            }

            mean_slope /= num_points;

            std::cout << "Second order Taylor expansion slope: " << mean_slope << " - It should be approximately equal to 3.0" << std::endl;

            return (std::abs(mean_slope - 3.0) <= 1e-3) ? true : false;
        }

        template <typename Function, typename Gradient, typename Hessian>
        bool checkHessian(Function f, Gradient g, Hessian h)
        {
            // Generate random test point and perturbation direction
            Eigen::VectorXd x = Eigen::VectorXd::Random(_dim), v = Eigen::VectorXd::Random(_dim);

            // Init H
            _H.setZero(_res);

            // Generate log spaced perturbation intensities
            _t = Eigen::Matrix<Precision, Eigen::Dynamic, 1>::LinSpaced(_res, -8, 0);
            for (size_t i = 0; i < _t.rows(); i++)
                _t(i) = std::pow(10, _t(i));

            // Calculate second order Taylor expansion
            for (size_t i = 0; i < _t.rows(); i++)
                _H(i) = std::abs(f(x + _t(i) * v) - f(x) - _t(i) * g(x).dot(v) - 0.5 * std::pow(_t(i), 2) * h(x, v).dot(v));

            // Transform to log space
            _t = _t.array().log();
            _H = _H.array().log();

            // Calculate mean slope for central values
            Precision mean_slope = 0, num_points = 0;

            for (size_t i = 30; i < _res - 10; i++) {
                mean_slope = mean_slope + (_H(i + 1) - _H(i)) / (_t(i + 1) - _t(i));
                num_points++;
            }

            mean_slope /= num_points;

            std::cout << "Second order Taylor expansion slope: " << mean_slope << " - It should be approximately equal to 3.0" << std::endl;

            return (std::abs(mean_slope - 3.0) <= 1e-3) ? true : false;
        }

    protected:
        size_t _dim, _res;

        Eigen::Matrix<Precision, Eigen::Dynamic, 1> _t, _G, _H;
    };
} // namespace utils_lib

#endif // utils_lib_GRADIENT_CHECKER_HPP


#pragma once
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <ceres/cubic_interpolation.h>
#include <superpixel_mesh/image.hpp>

namespace superpixel_mesh {

typedef ceres::Grid2D<ImageDataType> Grid;
typedef ceres::BiCubicInterpolator<Grid> Interpolator;

template <typename T> class Homography {
public:
  typedef Eigen::Matrix<T, 2, 4> MatX;
  typedef Eigen::Matrix<T, 2, 4> MatB;
  typedef Eigen::Matrix<T, 3, 3> MatH;
  typedef Eigen::Matrix<T, 8, 8> Mat8;
  typedef Eigen::Matrix<T, 8, 1> Vec8;

  static void ComputeHomography(const MatX &x1, const MatX &x2, Vec8 *H) {
    Mat8 L = Mat8::Zero();
    Vec8 b = Vec8::Zero();
    /**
     *
     *  Add 2 rows for each point. Parametrization uses h9 = 1
     *
     *    0       0       0   -x1    -y1    -1    x1*y2  y1*y2    hi   =  -y2
     *   -x1     -y1     -1     0      0     0    x1*x2  y1*x2    hi+1 =   -x2
     */

    for (int i = 0; i < 4; ++i) {
      int r = i * 2;
      L(r, 3) = x1(0, i);
      L(r, 4) = x1(1, i);
      L(r, 5) = T(1.0);
      L(r, 6) = -x1(0, i) * x2(1, i);
      L(r, 7) = -x1(1, i) * x2(1, i);
      b(r) = x2(1, i);

      L(r + 1, 0) = x1(0, i);
      L(r + 1, 1) = x1(1, i);
      L(r + 1, 2) = T(1.0);
      L(r + 1, 6) = -x1(0, i) * x2(0, i);
      L(r + 1, 7) = -x1(1, i) * x2(0, i);
      b(r + 1) = x2(0, i);
    }
    // Solve Ax=B
    Eigen::PartialPivLU<Mat8> dec(L);
    (*H) = dec.solve(b);
  }
};

template <int SIZE, int PADDING = 2> struct PixelDissimilarityCost {

  PixelDissimilarityCost(const Interpolator &inteporlator)
      : interpolator(inteporlator) {}

  template <typename T>
  bool operator()(const T *const v1, const T *const v2, const T *const v3,
                  const T *const v4, T *residuals) const {

    // Compute the homography
    typename Homography<T>::MatX x1, x2;
    x1 << T(-PADDING), T(SIZE + PADDING), T(SIZE + PADDING), T(-PADDING),
        T(-PADDING), T(-PADDING), T(SIZE + PADDING), T(SIZE + PADDING);
    x2 << v1[0], v2[0], v3[0], v4[0], v1[1], v2[1], v3[1], v4[1];

    typename Homography<T>::Vec8 h;
    Homography<T>::ComputeHomography(x1, x2, &h);

    T intensities[SIZE * SIZE];
    T mean = T(0);
    int intensity_index = 0;
    for (int row = 0; row < SIZE; row++) {
      for (int col = 0; col < SIZE; col++, intensity_index++) {
        T x, y, s;
        x = T(col);
        y = T(row);
        s = T(1.0) / (h(6) * x + h(7) * y + T(1.0));
        interpolator.Evaluate(s * (h(3) * x + h(4) * y + h(5)),
                              s * (h(0) * x + h(1) * y + h(2)),
                              &intensities[intensity_index]);
        mean += intensities[intensity_index];
      }
    }
    const T normalization = T(1.0 / double(SIZE * SIZE));
    mean *= normalization;
    for (int intensity_index = 0; intensity_index < SIZE * SIZE;
         intensity_index++) {
      residuals[intensity_index] = intensities[intensity_index] - mean;
    }

    return true;
  }

  const Interpolator &interpolator;
};

struct AreaRegularizationCost {

  AreaRegularizationCost(const double target_area) : target_area(target_area) {}

  template <typename T>
  T TriangleArea(const T *const v1, const T *const v2,
                 const T *const v3) const {
    /**
     * Shoelace formula:
     * (1/2) | x1y2 + x2y3 + x3y1 - x2y1 - x3y2 - x1y3 |
     */
    const auto &x1 = v1[0];
    const auto &y1 = v1[1];
    const auto &x2 = v2[0];
    const auto &y2 = v2[1];
    const auto &x3 = v3[0];
    const auto &y3 = v3[1];

    return 0.5 * ceres::abs(x1 * y2 + x2 * y3 + x3 * y1 - x2 * y1 - x3 * y2 -
                            x1 * y3);
  }

  template <typename T>
  bool operator()(const T *const v1, const T *const v2, const T *const v3,
                  const T *const v4, T *residuals) const {
    T triangle_areas[4];
    triangle_areas[0] = TriangleArea(v4, v3, v2);
    triangle_areas[1] = TriangleArea(v3, v2, v1);
    triangle_areas[2] = TriangleArea(v2, v1, v4);
    triangle_areas[3] = TriangleArea(v1, v4, v3);

    const T target_triangle_area = T(0.5 * target_area);
    for (int triangle_index = 0; triangle_index < 4; triangle_index++)
      residuals[triangle_index] =
          target_triangle_area - triangle_areas[triangle_index];

    return true;
  }

  const double target_area;
};

} // namespace superpixel_mesh


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config.hpp>

#if BOOST_METAPARSE_STD < 2011
#include <iostream>

int main()
{
  std::cout << "Please use a compiler that supports constexpr" << std::endl;
}
#else

#define BOOST_MPL_LIMIT_STRING_SIZE 64
#define BOOST_METAPARSE_LIMIT_STRING_SIZE BOOST_MPL_LIMIT_STRING_SIZE

#include <boost/metaparse/grammar.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/build_parser.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/util/digit_to_int.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/char.hpp>
#include <boost/mpl/int.hpp>

using boost::metaparse::build_parser;
using boost::metaparse::entire_input;
using boost::metaparse::token;
using boost::metaparse::grammar;

using boost::metaparse::util::digit_to_int;

using boost::mpl::apply_wrap1;
using boost::mpl::fold;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::times;
using boost::mpl::divides;
using boost::mpl::eval_if;
using boost::mpl::equal_to;
using boost::mpl::_1;
using boost::mpl::_2;
using boost::mpl::char_;
using boost::mpl::lambda;
using boost::mpl::int_;

#ifdef _STR
  #error _STR already defined
#endif
#define _STR BOOST_METAPARSE_STRING

template <class A, class B>
struct lazy_plus : plus<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_minus : minus<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_times : times<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_divides : divides<typename A::type, typename B::type> {};

template <class C, class T, class F>
struct lazy_eval_if : eval_if<typename C::type, T, F> {};

template <class A, class B>
struct lazy_equal_to : equal_to<typename A::type, typename B::type> {};

template <class Sequence, class State, class ForwardOp>
struct lazy_fold :
  fold<typename Sequence::type, typename State::type, typename ForwardOp::type>
{};

typedef
  lazy_fold<
    back<_1>,
    front<_1>,
    lambda<
      lazy_eval_if<
        lazy_equal_to<front<_2>, char_<'*'>>,
        lazy_times<_1, back<_2>>,
        lazy_divides<_1, back<_2>>
      >
    >::type
  >
  prod_action;

typedef
  lazy_fold<
    back<_1>,
    front<_1>,
    lambda<
      lazy_eval_if<
        lazy_equal_to<front<_2>, char_<'+'>>,
        lazy_plus<_1, back<_2>>,
        lazy_minus<_1, back<_2>>
      >
    >::type
  >
  plus_action;

typedef
  lambda<
    lazy_fold<
      _1,
      int_<0>,
      lambda<
        lazy_plus<lazy_times<_1, int_<10>>, apply_wrap1<digit_to_int<>, _2>>
      >::type
    >
  >::type
  int_action;

typedef
  grammar<_STR("plus_exp")>

    ::rule<_STR("int ::= ('0'|'1'|'2'|'3'|'4'|'5'|'6'|'7'|'8'|'9')+"), int_action>::type
    ::rule<_STR("ws ::= (' ' | '\n' | '\r' | '\t')*")>::type
    ::rule<_STR("int_token ::= int ws"), front<_1>>::type
    ::rule<_STR("plus_token ::= '+' ws"), front<_1>>::type
    ::rule<_STR("minus_token ::= '-' ws"), front<_1>>::type
    ::rule<_STR("mult_token ::= '*' ws"), front<_1>>::type
    ::rule<_STR("div_token ::= '/' ws"), front<_1>>::type
    ::rule<_STR("plus_token ::= '+' ws")>::type
    ::rule<_STR("plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*"), plus_action>::type
    ::rule<_STR("prod_exp ::= int_token ((mult_token | div_token) int_token)*"), prod_action>::type
  expression;

typedef build_parser<entire_input<expression>> calculator_parser;

int main()
{
  using std::cout;
  using std::endl;

  cout
    << apply_wrap1<calculator_parser, _STR("13")>::type::value << endl
    << apply_wrap1<calculator_parser, _STR("1+ 2*4-6/2")>::type::value << endl
    ;
}
#endif


#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <boost/filesystem.hpp>
#include <Open3D/Open3D.h>
#include <glog/logging.h>

#include "FileSystemTools.h"
#include "YamlFileIO.h"

// fitness threshold
double min_fitness_thresh = 0.05;

struct RegistrationResult{
    // transform matrix
    Eigen::Matrix4d T;
    // fitness score
    double fitness;
    // inlier rms
    double inlier_rms;
};

// create init extrinsic file
void createMultiLidarExtFile(const std::string& filename)
{
    // new backpack structural value
    Eigen::Matrix4d T_l0_l1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd l0_l1_vec1(-30 * M_PI / 180.0, Eigen::Vector3d(0, 0, 1));
    Eigen::AngleAxisd l0_l1_vec2(-73.5 * M_PI / 180.0, Eigen::Vector3d(0, 1, 0));
    Eigen::Matrix3d l0_l1_vec = l0_l1_vec1.matrix() * l0_l1_vec2.matrix();
    Eigen::Vector3d t_l0_l1(-0.31405, 0, -0.39803);
    T_l0_l1_gt.block<3, 3>(0, 0) = l0_l1_vec;
    T_l0_l1_gt.block<3, 1>(0, 3) = l0_l1_vec1.matrix() * t_l0_l1;
    std::cout << "T_l0_l1_gt:\n" << T_l0_l1_gt << "\n";

    common::saveExtFileOpencv(filename, T_l0_l1_gt);
}

bool alignTwoPointClouds(const std::string &src_pcl_file, const std::string &target_pcl_file, const Eigen::Matrix4d &T_init,
                RegistrationResult &result){
    std::string path, file_name;
    common::splitPathAndFilename(src_pcl_file, &path, &file_name);

    auto src_pcd_ptr = open3d::io::CreatePointCloudFromFile(src_pcl_file);
    auto target_pcd_ptr = open3d::io::CreatePointCloudFromFile(target_pcl_file);

    if( src_pcd_ptr == nullptr || src_pcd_ptr == nullptr){
        LOG(ERROR) << "Fail to load src and target pointcloud file!";
        return false;
    }

    auto src_pcd_down_ptr = src_pcd_ptr->VoxelDownSample(0.05);
    src_pcd_ptr->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(0.1, 30));

    auto target_pcd_down_ptr = target_pcd_ptr->VoxelDownSample(0.05);
    target_pcd_ptr->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(0.1, 30));

    src_pcd_ptr->Transform(T_init);
    src_pcd_down_ptr->Transform(T_init);

    open3d::registration::RegistrationResult icp_result;

    const double max_corresp_dis = 0.03; // meter
    open3d::registration::ICPConvergenceCriteria icp_criteria(1e-6, 1e-6, 100);
    // icp_result = open3d::registration::RegistrationICP(*src_pcd_ptr, *target_pcd_ptr, max_corresp_dis, Eigen::Matrix4d::Identity(),
    //                                                         open3d::registration::TransformationEstimationPointToPlane(), icp_criteria);
    icp_result = open3d::registration::RegistrationICP(*src_pcd_ptr, *target_pcd_ptr, max_corresp_dis, Eigen::Matrix4d::Identity(),
                                                    open3d::registration::TransformationEstimationPointToPoint(false), icp_criteria);

    // transform source pointcloud with icp_transformation
    Eigen::Matrix4d T_icp = icp_result.transformation_;

    LOG(INFO) << "#############" << path << " ################";
    result.fitness = icp_result.fitness_;
    LOG(INFO) << "icp_result.fitness: " << icp_result.fitness_;
    result.inlier_rms = icp_result.inlier_rmse_;
    LOG(INFO) << "icp_result.inlier_rmse_ : " << icp_result.inlier_rmse_;

    if(result.fitness < min_fitness_thresh){
        LOG(ERROR) << "fitness score < " << min_fitness_thresh;
        return false;
    }

    // final transformation between source pointcloud and target pointcloud
    result.T = T_icp * T_init;
    std::cout << "Final transformation:\n"
              << result.T << "\n";

    src_pcd_down_ptr->Transform(T_icp);

    std::string save_file_path;

    save_file_path = common::concatenateFolderAndFileName(path, "transformed_source_pointcloud.ply");
    open3d::io::WritePointCloudToPLY(save_file_path, *src_pcd_down_ptr, true);
    std::shared_ptr<open3d::geometry::PointCloud> merge_points(new open3d::geometry::PointCloud);
    *merge_points = *src_pcd_down_ptr + *target_pcd_down_ptr;

    save_file_path = common::concatenateFolderAndFileName(path, "merged_pointcloud.ply");
    open3d::io::WritePointCloudToPLY(save_file_path, *merge_points, true);

    return true;
}

void evaluateExtrinsics(const std::vector<RegistrationResult> & v_results, const Eigen::Matrix4d &T_baseline){
    assert(v_results.size() > 1);

    std::vector<double> v_inlier_rms = {v_results[0].inlier_rms};
    // rotation sigma
    std::vector<double> v_sigma_r;
    // translation sigma
    std::vector<double> v_sigma_t;

    int T_num = v_results.size();
    for(int i = 0; i < T_num; ++i){
        Eigen::Matrix4d T = v_results[i].T;
        Eigen::Matrix4d T_inv = T.inverse();

        Eigen::Matrix4d T_delt = T_inv * T_baseline;
        Eigen::AngleAxisd rot_vec(T_delt.block<3, 3>(0, 0));
        double delt_r = rot_vec.angle() * 180.0 / M_PI;
        Eigen::Vector3d t_delt = T_delt.block<3, 1>(0, 3);
        double delt_t = t_delt.norm() * 100;
        LOG(INFO) << "T_baseline and  T_" << i << " Rotation delta: "
                <<  delt_r << " °\n";
        LOG(INFO) << "T_baseline and  T_" << i << " Translation delta: " << delt_t
                << " cm\n";

        v_sigma_r.push_back(delt_r * delt_r);
        v_sigma_t.push_back(delt_t * delt_t);
        v_inlier_rms.push_back(v_results[i].inlier_rms);
    }

    double sigma_r = std::accumulate(v_sigma_r.begin(), v_sigma_r.end(), 0.0) / T_num;
    double sigma_t = std::accumulate(v_sigma_t.begin(), v_sigma_t.end(), 0.0) / T_num;
    LOG(INFO) << "Avg rotation sigma: " << sigma_r << ", Avg trans sigma: " << sigma_t;
    LOG(INFO) << "Final sigma_r" << std::sqrt(sigma_r) << ", sigma_t: " << std::sqrt(sigma_t);
    // LOG(INFO) << "Avg inlier rms: " << std::accumulate(v_inlier_rms.begin(), v_inlier_rms.end(), 0.0) / v_inlier_rms.size();

}

int main(int argc, char **argv){
    if (argc < 3){
        LOG(FATAL) << "Usage: test_lidar2lidar_calibration [T_l0_l1_init.yaml] [input_dataset_folder] [output_folder] [lidar0_id] [lidar1_id]";
        return -1;
    }

    std::string init_ext_filepath(argv[1]);
    // dataset folder e.g. /path/data0/lidar_lidar
    std::string dataset_folder(argv[2]);
    std::string output_folder(argv[3]);
    int lidar0_id = 0, lidar1_id = 1;
    if (argc == 6){
        lidar0_id = std::stoi (argv[4]);
        lidar1_id = std::stoi (argv[5]);
    }

    // if initial extrinsic isn't here, create it
    if(!common::fileExists(init_ext_filepath)){
        createMultiLidarExtFile(init_ext_filepath);
    }
    if(!common::pathExists(dataset_folder)){
        LOG(FATAL) << "Input folder doesnot exist!";
        return -1;
    }
    if(!common::pathExists(output_folder)){
        if(!common::createPath(output_folder)){
            LOG(FATAL) << "Fail to create " << output_folder;
            return -1;
        }
    }

    // Eigen::Matrix4d T_base_l0 = horizontal_lidar_ptr->extrinsics();
    // Eigen::Matrix4d T_base_l1 = vertical_lidar_ptr->extrinsics();
    // initial extrinsic between two lidar
    Eigen::Matrix4d T_l0_l1_init = Eigen::Matrix4d::Identity();
    if (!common::loadExtFileOpencv(init_ext_filepath, T_l0_l1_init)){
        LOG(FATAL) << "Fail to load " << init_ext_filepath;
        return -1;
    }

    std::vector<RegistrationResult> v_extrinsics;

    // for (const auto & entry : boost::filesystem::directory_iterator(dataset_folder)){
    //     if(!boost::filesystem::is_directory(entry))
    //         continue;

    //     std::string scan_folder_path = entry.path().string() + "/lidar_lidar";
        std::string lidar0_scan_folder = common::concatenateFolderAndFileName(dataset_folder, "lidar0");
        std::string lidar1_scan_folder = common::concatenateFolderAndFileName(dataset_folder, "lidar1");
        std::vector<std::string> v_lidar0_pcl_paths,  v_lidar1_pcl_paths;
        std::vector<std::string> paths = {lidar0_scan_folder};
        common::getFileLists(paths, true, "ply", &v_lidar0_pcl_paths);
        paths = {lidar1_scan_folder};
        common::getFileLists(paths, true, "ply", &v_lidar1_pcl_paths);

        if (v_lidar0_pcl_paths.empty() || v_lidar1_pcl_paths.empty()){
            LOG(ERROR) << "lidar scan folder is empty!\n";
            return -1;
        }
        // only choose 1 scan pair for calibration
        std::string src_pcl_file_path, target_pcl_file_path;
        {
            std::string file_path = v_lidar0_pcl_paths[0];
            std::string path, file_name;
            common::splitPathAndFilename(file_path, &path, &file_name);
            if(std::atoi(&file_name[0]) == lidar0_id)
                target_pcl_file_path = file_path;
        }
        {
            std::string file_path = v_lidar1_pcl_paths[0];
            std::string path, file_name;
            common::splitPathAndFilename(file_path, &path, &file_name);
            if(std::atoi(&file_name[0]) == lidar1_id)
                src_pcl_file_path = file_path;
        }

        RegistrationResult regist_result;
        bool sts = alignTwoPointClouds(src_pcl_file_path, target_pcl_file_path, T_l0_l1_init, regist_result);
        if(!sts){
            return -1;
        }

        std::string save_file_path = common::concatenateFolderAndFileName(output_folder, "lidar0_to_lidar1.yml");
        common::saveExtFileOpencv(save_file_path, regist_result.T);
        v_extrinsics.emplace_back(regist_result);
    // }

    // sort result by fitness
    // std::sort(v_extrinsics.begin(), v_extrinsics.end(), [&](RegistrationResult &res_a, RegistrationResult &res_b){
    //     return res_a.fitness > res_b.fitness;
    // });

    // evaluateExtrinsics(v_extrinsics, v_extrinsics[0].T);

    return 0;
}


/**
 * \file CachedCosinusGeneratorFilter.cpp
 */

#include "CachedCosinusGeneratorFilter.h"

#include <cmath>
#include <cstdint>
#include <cstring>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename DataType_>
  CachedCosinusGeneratorFilter<DataType_>::CachedCosinusGeneratorFilter(int periods, int seconds)
  :Parent(0, 1), indice(1), periods(periods), seconds(seconds), volume(1), offset(0)
  {
  }

  template<typename DataType_>
  CachedCosinusGeneratorFilter<DataType_>::~CachedCosinusGeneratorFilter()
  {
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_frequency(int periods, int seconds)
  {
    this->periods = periods;
    this->seconds = seconds;
    setup();
  }

  template<typename DataType_>
  std::pair<int, int> CachedCosinusGeneratorFilter<DataType_>::get_frequency() const
  {
    return std::make_pair(periods, seconds);
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
  }

  template<typename DataType_>
  DataType_ CachedCosinusGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
  }

  template<typename DataType_>
  DataType_ CachedCosinusGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::setup()
  {
    indice = 0;
    cache.resize(output_sampling_rate * seconds);
    for(int i = 0; i < cache.size(); ++i)
    {
      cache[i] = static_cast<DataType>(std::cos(2 * boost::math::constants::pi<double>() * (i+1) * periods / seconds / output_sampling_rate));
    }
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::process_impl(int64_t size) const
  {
    DataType* ATK_RESTRICT output = outputs[0];
    int64_t processed = 0;
    while(processed < size)
    {
      int64_t to_copy = std::min(size - processed, int64_t(cache.size()) - indice);
      memcpy(reinterpret_cast<void*>(output + processed), reinterpret_cast<const void*>(cache.data() + indice), to_copy * sizeof(DataType_));
      indice += to_copy;
      processed += to_copy;
      if(indice >= static_cast<int64_t>(cache.size()))
      {
        indice = 0;
      }
    }
    for(int64_t i = 0; i < size; ++i)
    {
      output[i] = static_cast<DataType>(offset + volume * output[i]);
    }
  }

  template class CachedCosinusGeneratorFilter<float>;
  template class CachedCosinusGeneratorFilter<double>;
}


/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <inttypes.h>
#include <stdint.h>
#include <time.h>
#include "dSFMText.hpp"
#include "dSFMT-calc-jump.hpp"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

using namespace dsfmt;
using namespace NTL;
using namespace std;

void calc_minimal(GF2X& minimal, int maxdegree, w128_t outseq[], int pos)
{
    uint64_t mask = 0;
    vec_GF2 seq;
    seq.SetLength(2 * maxdegree);
    int idx = 0;

    if (pos >= 104 || pos < 0) {
	cerr << "pos error:" << dec << pos << endl;
	exit(1);
    } else if (pos >= 52) {
	idx = 1;
	mask = UINT64_C(1) << (pos - 52);
    } else {
	idx = 0;
	mask = UINT64_C(1) << pos;
    }
    if (idx < 0 || idx >= 2) {
	cerr << "idx error:" << dec << idx << endl;
	exit(1);
    }
    for (int i = 0; i < 2 * maxdegree; i++) {
	if ((outseq[i].u[idx] & mask) != 0) {
	    seq[i] = 1;
	} else {
	    seq[i] = 0;
	}
    }
    MinPolySeq(minimal, seq, maxdegree);
#if defined(DEBUG)
    cout << "deg(minimal) = " << dec << deg(minimal) << endl;
#endif
}

void LCM(GF2X& lcm, const GF2X& x, const GF2X& y) {
    GF2X gcd;
    mul(lcm, x, y);
    GCD(gcd, x, y);
//void DivRem(GF2X& q, GF2X& r, const GF2X& a, const GF2X& b);
// q = a/b, r = a%b
    GF2X r;
    DivRem(lcm, r, lcm, gcd);
    if (deg(r) != -1) {
	cerr << "LCM error: deg(r) = " << dec << deg(r);
	cerr << "r:" << r << endl;
	exit(1);
    }
}

static void get_lcm_sub(GF2X& lcmpoly, dSFMText& dsfmt) {
    GF2X tmp(1,1);
    int maxdegree = dsfmt.get_maxdegree();
    w128_t out_seq[2 * maxdegree];

    for (int i = 0; i < 2 * maxdegree; i++) {
	out_seq[i] = dsfmt.next();
    }
    GF2X minimal;
    GF2X t1;
    SetCoeff(t1, 0, 1);
    SetCoeff(t1, 1, 1);
    for (int pos = 0; pos < 104; pos++) {
	calc_minimal(minimal, maxdegree, out_seq, pos);
	LCM(tmp, lcmpoly, minimal);
	lcmpoly = tmp;
	if (divide(lcmpoly, t1)) {
	    return;
	}
    }
#if defined(DEBUG)
    cout << "deg(lcmpoly) = " << dec << deg(lcmpoly) << endl;
#endif
}

void get_lcm(GF2X& lcmpoly, dSFMText& dsfmt) {
    int maxdegree = dsfmt.get_maxdegree();
    GF2X t1;
    SetCoeff(t1, 0, 1);
    SetCoeff(t1, 1, 1);

    //uint32_t time = (uint32_t)clock();
    dsfmt.seeding(1234);
    get_lcm_sub(lcmpoly, dsfmt);
    if (deg(lcmpoly) >= maxdegree) {
	return;
    }
    if (divide(lcmpoly, t1)) {
	return;
    }
    for(int i = 0; i < maxdegree; i++) {
	dsfmt.init_basis(i);
	get_lcm_sub(lcmpoly, dsfmt);
	if (deg(lcmpoly) >= maxdegree) {
	    return;
	}
	if (divide(lcmpoly, t1)) {
	    return;
	}
    }
}

static int has_large_irreducible(GF2X& fpoly, int degree) {
    static const GF2X t2(2, 1);
    static const GF2X t1(1, 1);
    GF2X t2m;
    GF2X t;
    GF2X alpha;
    int m;

    t2m = t2;
    if (deg(fpoly) < degree) {
	return 0;
    }
    t = t1;
    t += t2m;

    for (m = 1; deg(fpoly) > degree; m++) {
	for(;;) {
	    GCD(alpha, fpoly, t);
	    if (IsOne(alpha)) {
		break;
	    }
	    fpoly /= alpha;
	    if (deg(fpoly) < degree) {
		return 0;
	    }
	}
	t2m *= t2m;
	t2m %= fpoly;
	add(t, t2m, t1);
    }
    if (deg(fpoly) != degree) {
	return 0;
    }
    return IterIrredTest(fpoly);
}

void check_fix(dSFMText & fix)
{
    fix.setup_high();
    cout << "before:";
    fix.print(cout);
    fix.next();
    cout << "after 1 step:";
    fix.print(cout);
    fix.next();
    cout << "after 2 step:";
    fix.print(cout);
    fix.next();
    cout << "after 3 step:";
    fix.print(cout);
}

void check_fix2(dSFMText & fix)
{
    cout << "before:";
    fix.print(cout);
    fix.next_add();
    cout << "after 1 step:";
    fix.print(cout);
    fix.next_add();
    cout << "after 2 step:";
    fix.print(cout);
    fix.next_add();
    cout << "after 3 step:";
    fix.print(cout);
}

void calc_fix(GF2X& inv, int mexp, int pos1, int sl1,
	      uint64_t mask1, uint64_t mask2)
{
    dSFMText fix(mexp, pos1, sl1, mask1, mask2);
    //fix.setup_high();
    //fix.next();
    //cout << "setup0:";
    //fix.print(cout);
    fix.setup_constants();
#if defined(DEBUG)
    cout << "setup:";
    fix.print(cout);
#endif
    dSFMText work(mexp, pos1, sl1, mask1, mask2);
#if defined(DEBUG)
    cout << "zero:";
    work.print(cout);
#endif
    for (long i = 0; i <= deg(inv); i++) {
	if (IsOne(coeff(inv, i))) {
	    work.add(fix);
#if defined(DEBUG) && 0
    cout << "work:";
    work.print(cout);
#endif
	}
	fix.next();
    }
    cout << "fix:";
    work.print(cout);
    check_fix(work);
}

void check_const(GF2X& lcm, int mexp, int pos1, int sl1,
	      uint64_t mask1, uint64_t mask2)
{
    dSFMText normal(mexp, pos1, sl1, mask1, mask2);
    dSFMText add(mexp, pos1, sl1, mask1, mask2);
    normal.seeding(123);
    add.seeding(123);

    normal.setup_high();
    normal.next();
    add.next_add();
    cout << "normal:";
    normal.print(cout);
    cout << "add:";
    add.print(cout);
    for (int i = 0; i < 5; i++) {
	normal.next();
	add.next_add();
    }
    cout << "normal:";
    normal.print(cout);
    cout << "add:";
    add.print(cout);
}

void check_gcd(GF2X& d, GF2X& s, GF2X& inv, GF2X& work, GF2X& t1)
{
//    XGCD(d, s, inv, work, t1);
//    check_gcd(d, s, inv, work, t1);
//void XGCD(GF2X& d, GF2X& s, GF2X& t, const GF2X& a, const GF2X& b);
// d = gcd(a,b), a s + b t = d
    if (deg(d) != 0) {
	cout << "check_gcd d != 1:" << dec << deg(d) << endl;
	return;
    }
    GF2X p;
    GF2X q;
    mul(p, s, work);
    mul(q, inv, t1);
    p += q;
    if (p != d) {
	cout << "check_gcd p != d, p = " << p << endl;
	cout << "check_gcd p != d, d = " << d << endl;
    } else {
	cout << "check_gcd OK d:" << d << endl;
    }
}
int main(int argc, char *argv[]) {
    if (argc < 6) {
	cout << argv[0] << " mexp pos1 sl1 mask1 mask2" << endl;
	return -1;
    }
    int mexp = strtol(argv[1], NULL, 10);
    int pos1 = strtol(argv[2], NULL, 10);
    int sl1 = strtol(argv[3], NULL, 10);
    uint64_t mask[2];
    mask[0] = strtoull(argv[4], NULL, 16);
    mask[1] = strtoull(argv[5], NULL, 16);
#if defined(DEBUG)
    cout << "mexp:" << dec << mexp << endl;
    cout << "pos1:" << dec << pos1 << endl;
    cout << "sl1:" << dec << sl1 << endl;
    cout << "mask1:" << hex<< mask[0] << endl;
    cout << "mask2:" << hex << mask[1] << endl;
#endif
    dSFMText dsfmt(mexp, pos1, sl1, mask[0], mask[1]);
    GF2X characteristic(0,1);
    get_lcm(characteristic, dsfmt);
#if defined(DEBUG)
    cout << "degree:" << dec << deg(characteristic) << endl;
    cout << "maxdegree = " << dec << dsfmt.get_mamaxdegree() << endl;
    cout << characteristic << endl;
#endif
    GF2X work;
    work = characteristic;
#if defined(DEBUG)
    cout << "degree:" << deg(characteristic) << endl;
    cout << characteristic << endl;
#endif
    if (!has_large_irreducible(characteristic, mexp)) {
        cout << "error?" << endl;
        return -1;
    }
#if 0
    GF2X remain = work / characteristic;
    vec_pair_GF2X_long factors;
    CanZass(factors, remain);
    cout << "degree of work:" << dec << deg(work) << endl;
    cout << "degree of remain:" << dec << deg(remain) << endl;
    cout << "=== factor of remain ===" << endl;
    for (int i = 0; i < factors.length(); i++) {
	cout << factors[i].a;
	cout << ":";
	cout << dec << deg(factors[i].a);
	cout << ":";
	cout << dec << factors[i].b << endl;
    }
#endif
    GF2X d, s, inv;
    GF2X t1;
    SetCoeff(t1, 0, 1);
    SetCoeff(t1, 1, 1);
    XGCD(d, s, inv, work, t1);
    check_gcd(d, s, inv, work, t1);
// d = gcd(a,b), a s + b t = d
//    MulMod(s, inv, t1, work);
#if defined(DEBUG)
    cout << "deg work:" << dec << deg(work) << endl;
    cout << "deg inv:" << dec << deg(inv) << endl;
#endif
    string x;
//    cout << "# deg = " << dec << deg(work) << endl;
    polytostring(x, work);
    cout << "#" << dec << mexp;
    cout << "," << dec << pos1;
    cout << "," << dec << sl1;
    cout << "," << hex << mask[0];
    cout << "," << hex << mask[1];
    cout << dec << endl;
    cout << x << endl;
    cout << dec << flush;
    if (deg(d) != 0) {
	cout << "doesn't have fixpoint." << endl;
	return 0;
    }
#if 0
    string y;
    polytostring(y, inv);
    if (deg(d) == 0) {
	cout << y << endl;
    } else {
	cout << "can't finf inv d=" << d << endl;
    }
#endif
    calc_fix(inv, mexp, pos1, sl1, mask[0], mask[1]);
    //check_const(inv, mexp, pos1, sl1, mask[0], mask[1]);
}


#define DEBUG 1
/**
 * File    : A.cpp
 * Author  : Kazune Takahashi
 * Created : 2020/1/25 11:32:12
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

class Sieve
{
  static constexpr ll MAX_SIZE{10000010LL};
  ll N;
  vector<ll> f;
  vector<ll> prime_nums;

public:
  Sieve(ll N = MAX_SIZE) : N{N}, f(N, 0), prime_nums{}
  {
    f[0] = f[1] = -1;
    for (auto i = 2; i < N; i++)
    {
      if (f[i])
      {
        continue;
      }
      prime_nums.push_back(i);
      f[i] = i;
      for (auto j = 2 * i; j < N; j += i)
      {
        if (!f[j])
        {
          f[j] = i;
        }
      }
    }
  }

  bool is_prime(ll x) const
  { // 2 \leq x \leq MAX_SIZE^2
    if (x < N)
    {
      return f[x];
    }
    for (auto e : prime_nums)
    {
      if (x % e == 0)
      {
        return false;
      }
    }
    return true;
  }

  vector<ll> const &primes() const
  {
    return prime_nums;
  }

  vector<ll> factor_list(ll x) const
  {
    if (x < 2)
    {
      return {};
    }
    vector<ll> res;
    auto it{prime_nums.begin()};
    if (x < N)
    {
      while (x != 1)
      {
        res.push_back(f[x]);
        x /= f[x];
      }
    }
    else
    {
      while (x != 1 && it != prime_nums.end())
      {
        if (x % *it == 0)
        {
          res.push_back(*it);
          x /= *it;
        }
        else
        {
          ++it;
        }
      }
      if (x != 1)
      {
        res.push_back(x);
      }
    }
    return res;
  }

  vector<tuple<ll, ll>> factor(ll x) const
  {
    if (x < 2)
    {
      return {};
    }
    auto factors{factor_list(x)};
    vector<tuple<ll, ll>> res{make_tuple(factors[0], 0)};
    for (auto x : factors)
    {
      if (x == get<0>(res.back()))
      {
        get<1>(res.back())++;
      }
      else
      {
        res.emplace_back(x, 1);
      }
    }
    return res;
  }
};

// ----- main() -----

using ld = long double;
using point = complex<ld>;
constexpr ld PI{3.14159265358979323846};

class Solve
{
  ld La, Lb, Lc, Na, Nb, Nc, Ne, Ma, Mb, Mc, Tc;
  int alpha;
  Sieve sieve;
  ld Ta = 0;
  ld Tb;
  point Oa{0, 900}, Ob{900, 0}, Oc{900, 900}, Oe{90, 50};
  point Aa{1, 0}, Ab{0, 1}, Ac{-1, 0};
  ld S{500}, A{3000}, D{60};

public:
  Solve(ld La, ld Lb, ld Lc, ld Na, ld Nb, ld Nc, ld Ne, ld Ma, ld Mb, ld Mc, ld Tc) : La{La}, Lb{Lb}, Lc{Lc}, Na{Na}, Nb{Nb}, Nc{Nc}, Ne{Ne}, Ma{Ma}, Mb{Mb}, Mc{Mc}, Tc{Tc} {}

  void flush()
  {
    for (auto i = 0; i < 20; ++i)
    {
      ld now{0};
      ld W{Ma};
      if (now + Ma * Na > 100)
      {
        W = 500;
      }
      flush_A(W);
      now += Na * W;
      W = Mb;
      if (now + Mb * Nb > 100)
      {
        W = 500;
      }
      flush_B(W);
      now += Nb * W;
      W = Mc;
      if (now + Mc * Nc > 100)
      {
        W = 500;
      }
      flush_C(W);
      now += Nc * W;
      flush_E(now / Ne);
    }
  }

private:
  void update_Tb()
  {
    if (alpha == 0)
    {
      Tb = 0;
    }
    else
    {
      ll p{sieve.primes()[alpha + 1]};
      ll r{p % 180};
      Tb = (r <= 90 ? r : 180 - r);
    }
  }

  point arg(ld theta) { return theta / 180 * PI * point(0, 1); }
  point Pa() { return exp(-arg(Ta)) * La + Oa; }
  point Pb() { return exp(arg(Tb)) * Lb + Ob; }
  point Pc() { return exp(arg(Tc)) * Lc + Oc; }

  void flush_simple(ld X, ld Sx, ld Ax, ld Dx, ld Y, ld Sy, ld Ay, ld Dy, ld Ta, ld Wa, ld Wb, ld Wc, ld We)
  {
    cout << fixed << setprecision(0) << X << ", " << Sx << ", " << Ax << ", " << Dx << ", " << Y << ", " << Sy << ", " << Ay << ", " << Dy << ", " << Ta << ", " << Wa << ", " << Wb << ", " << Wc << ", " << We << endl;
  }

  void flush_A(ld W)
  {
    flush_simple(Pa().real(), S, A, D, Pa().imag(), S, A, D, Ta, W, 0, 0, 0);
    alpha += W;
  }

  void flush_B(ld W)
  {
    flush_simple(Pb().real(), S, A, D, Pb().imag(), S, A, D, Ta, 0, W, 0, 0);
  }

  void flush_C(ld W)
  {
    flush_simple(Pc().real(), S, A, D, Pc().imag(), S, A, D, Ta, 0, 0, W, 0);
  }

  void flush_E(ld W)
  {
    flush_simple(Oe.real(), S, A, D, Oe.imag(), S, A, D, Ta, 0, 0, 0, W);
  }
};

int main()
{
  ld La, Lb, Lc, Na, Nb, Nc, Ne, Ma, Mb, Mc, Tc;
  cin >> La >> Lb >> Lc >> Na >> Nb >> Nc >> Ne >> Ma >> Mb >> Mc >> Tc;
  Solve solve(La, Lb, Lc, Na / 1000, Nb / 1000, Nc / 1000, Ne / 1000, Ma, Mb, Mc, Tc);
  solve.flush();
}


#include <iostream>
#include <limits>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>>
    graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

class edge_adder
{
    graph &G;

public:
    explicit edge_adder(graph &G) : G(G) {}

    void add_edge(int from, int to, long capacity)
    {
        auto c_map = boost::get(boost::edge_capacity, G);
        auto r_map = boost::get(boost::edge_reverse, G);
        const auto e = boost::add_edge(from, to, G).first;
        const auto rev_e = boost::add_edge(to, from, G).first;
        c_map[e] = capacity;
        c_map[rev_e] = 0; // reverse edge has no capacity!
        r_map[e] = rev_e;
        r_map[rev_e] = e;
    }
};

using namespace std;

void solve()
{
    int n, m;
    cin >> n >> m;

    graph G(n);
    edge_adder adder(G);

    int u, v, c;
    for (int i = 0; i < m; ++i) {
        cin >> u >> v >> c;
        adder.add_edge(u, v, c);
    }

    long minCut = numeric_limits<long>::max();

    int source = 0;
    for (int target = 1; target < n; ++target) {
        long maxFlow = boost::push_relabel_max_flow(G, source, target);
        minCut = min(minCut, maxFlow);
        maxFlow = boost::push_relabel_max_flow(G, target, source);
        minCut = min(minCut, maxFlow);
    }
    cout << minCut << endl;
}

int main()
{
    ios_base::sync_with_stdio(false);
    int t;
    cin >> t;
    for (int i = 0; i < t; ++i)
    {
        solve();
    }
    return 0;
}


#include <algorithm>
#include <numeric>
#include <cmath>
#include <iostream>
#include <iomanip>
#include <limits>

#include <boost/timer.hpp>

#include "CompositeMinimization.h"

namespace Grante {

double CompositeMinimization::FISTAMinimize(
	CompositeMinimizationProblem& prob, std::vector<double>& x_opt,
	double conv_tol, unsigned int max_iter, bool verbose) {
	double Lub = 0.25;	// Lipschitz upper bound estimate for the gradient
	double eta = 2.0;	// Lipschitz bound scale factor

	// Initialize iterates
	unsigned int dim = prob.Dimensions();
	x_opt.resize(dim);
	prob.ProvideStartingPoint(x_opt);
	std::vector<double> xprev(x_opt);
	std::vector<double> x_fgrad_dummy;
	std::vector<double> y(x_opt);
	std::vector<double> y_fgrad(dim, 0.0);
	std::vector<double> u(dim, 0.0);

	// Convergence criterion
	double conv = std::numeric_limits<double>::infinity();
	double t = 1.0;

	// Objective F(x)=f(x)+g(x)
	double Fval = std::numeric_limits<double>::signaling_NaN();
	boost::timer total_timer;
	unsigned int lip_iter = 0;
	for (unsigned int iter = 0; max_iter == 0 || iter < max_iter; ++iter) {
		double y_fval = prob.EvalF(y, y_fgrad);
		double obj = y_fval + prob.EvalG(y, x_fgrad_dummy);

		// Verbose output
		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective        conv  lipiter"
				<< std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Convergence criterion
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << conv << "   ";
			// Lipschitz iterations
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << lip_iter;

			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		// Backtracking
		lip_iter = 0;
		do {
			lip_iter += 1;

			// Solve (2.6) in Beck and Teboulle
			std::transform(y.begin(), y.end(), y_fgrad.begin(), u.begin(),
				[Lub](double y_e, double y_fgrad_e) -> double {
					return (y_e - y_fgrad_e/Lub); });
			// update x_opt
			prob.EvalGProximalOperator(u, Lub, x_opt);

			// Evaluate F(x)
			Fval = prob.EvalF(x_opt, x_fgrad_dummy);
			Fval += prob.EvalG(x_opt, x_fgrad_dummy);

			// Evaluate Q_L(x,y)
			double Qval = y_fval + prob.EvalG(x_opt, x_fgrad_dummy);
			for (unsigned int di = 0; di < dim; ++di) {
				double xsuby = x_opt[di] - y[di];
				Qval += xsuby*y_fgrad[di];
				Qval += 0.5*Lub*xsuby*xsuby;
			}

			// Sufficient upper bound on the Lipschitz constant?
			if (Fval <= Qval)
				break;

			Lub *= eta;	// Increase upper bound estimate
		} while (true);

		// Update t sequence
		double tprev = t;
		t = 0.5*(1.0 + std::sqrt(1.0 + 4.0*tprev*tprev));

		// Perform averaged step: update y
		std::transform(x_opt.begin(), x_opt.end(), xprev.begin(),
			y.begin(), [t,tprev](double xe, double xle) -> double {
				return (xe + ((tprev-1.0)/t)*(xe-xle)); });
		// conv=norm(x_opt-xprev)
		conv = 0.0;
		for (unsigned int di = 0; di < dim; ++di)
			conv += (x_opt[di]-xprev[di]) * (x_opt[di]-xprev[di]);
		conv = std::sqrt(conv);
		if (conv <= conv_tol) {
			if (verbose) {
				std::cout << "Converged with tolerance " << conv
					<< " (<= " << conv_tol << ")." << std::endl;
			}
			return (Fval);
		}

		std::copy(x_opt.begin(), x_opt.end(), xprev.begin());
	}
	return (Fval);	// objective of x_opt
}

}


#include <iostream>
#include <Eigen/Dense>

Eigen::VectorXd dipoleval(const Eigen::VectorXd & x, const Eigen::VectorXd & t, const Eigen::VectorXd & y){
	int x_size= x.size();
	int w=1;
	int n= t.size();
	Eigen::VectorXd p=y;
	Eigen::VectorXd dP=Eigen::MatrixXd::Zero(1,n);
	for (int i=1; i<n; i++){//im
		for (int j=i-1; j>0;j--){//i0
			//% compute dp(i)'s
            dP(j) = p(j+1) + (x(w)-t(j))*dP(j+1) - p(j) - (x(w)-t(i))*dP(j);
            dP(j) = dP(j) / ( t(i) - t(j) );
            //% compute p(i)'s
            p(j)  = (x(w)-t(j)*p(j+1) - (x(w)-t(i))*p(j));
            p(j)  = p(j) / ( t(i) - t(j) );
            }
			std::cout <<"test\n";

	}
	return p.row(0);
}

int main(){
	int n=6;
	Eigen::VectorXd x=Eigen::MatrixXd::Random(n,1);
	Eigen::VectorXd y=Eigen::MatrixXd::Random(n,1);
	Eigen::VectorXd t=Eigen::MatrixXd::Random(n,1);

	Eigen::VectorXd v;
	v=dipoleval(x,t,y);
	std::cout << v << std::endl;
}

Nummerical Methods for CSE/PS6/dipoleval.cpp


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef NORMS_HH
#define NORMS_HH

/**
 * @file
 * @brief  Some norms for FunctionSpaceElement s or FunctionViews
 * @author Anton Schiela
 */

#include <boost/fusion/include/at_c.hpp>
#include <fem/integration.hh>
#include <fem/functionviews.hh>

namespace Kaskade
{
/// L_2-norms
struct L2Norm
{
/// Evaluation of square norm
  template<typename Function>
  double square(Function const& f) const
  {
    Integral<typename Function::Space> integral;
    FunctionViews::AbsSquare<Function> asf(f);
    return integral(asf);
  }

/// Evaluation of norm
  template<typename Function>
  double operator()(Function const& f) const
  {
    return std::sqrt(square(f));
  }
};

/// L_2-norms
// struct L2ScalarProduct
// {
// /// Evaluation of square norm
//   template<typename Function>
//   double operator()(Function f,Function g)
//   {
//     Integral<typename Function::Space> integral;
//     FunctionViews::Dot<Function> dotfg(f,g);
//     return integral(dotfg);
//   }
//
// /// Evaluation of norm
//   template<typename Function>
//   double operator()(Function f)
//   {
//     return std::sqrt(this->operator()(f,f));
//   }
// };

/// H1-semi-norms
struct H1SemiNorm
{
/// Evaluation of square norm
  template<typename Function>
  double square(Function const& f) const
  {
    Integral<typename Function::Space> integral;
    FunctionViews::GradientAbsSquare<Function> asf(f);
    return integral(asf);
//    FunctionViews::Gradient<Function> gf(f);
//    L2Norm l2Norm;
//    return l2Norm.square(gf);
  }

/// Evaluation of norm
  template<typename Function>
  double operator()(Function const& f) const
  {
    return std::sqrt(square(f));
  }
};

// THE FOLLOWING CODE IS UNDOCUMENTED, APPEARS TO BE RATHER PROJECT-SPECIFIC, AND IS NOWHERE USED IN
// THE SVN REPOSITORY. UNLESS SOME UNEXPECTED PROBLEMS ARISE, THIS CODE WILL BE DELETED AS OF
// 2016-03-31.
//
// template <int val>
// struct Decrement
// {
//   static constexpr int value = val-1;
// };
//
//
//
// template <int variableId, int numberOfVariables>
// struct MyNormEvaluator
// {
//   template <class DomainElement, class Norm>
//   static double square(DomainElement const& x, Norm const& norm)
//   {
//     return norm.square( boost::fusion::at_c<variableId>(x.data) ) + MyNormEvaluator<variableId-1,numberOfVariables>::square(x,norm);
//   }
// };
//
// template <int numberOfVariables>
// struct MyNormEvaluator<0,numberOfVariables>
// {
//   template <class DomainElement, class Norm>
//   static double square(DomainElement const& x, Norm const& norm)
//   {
//     return norm.square( boost::fusion::at_c<0>(x.data) );
//   }
// };
//
// struct MyH1SemiNorm
// {
//   template <typename DomainElement>
//   static double square(DomainElement const& x)
//   {
//     return MyNormEvaluator<DomainElement::Descriptions::noOfVariables-1, DomainElement::Descriptions::noOfVariables>::square(x,H1SemiNorm());
//   }
//
//   template <typename DomainElement>
//   double operator()(DomainElement const& x) const
//   {
//     return sqrt(square(x));
//   }
// };

// struct MyL2Norm
// {
//   template <typename DomainElement>
//   static double square(DomainElement const& x)
//   {
//     return MyNormEvaluator<DomainElement::Descriptions::noOfVariables-1, DomainElement::Descriptions::noOfVariables>::square(x,L2Norm());
//   }
//
//   template <typename DomainElement>
//   double operator()(DomainElement const& x) const
//   {
//     return sqrt(square(x));
//   }
// };

/// H1-norms
struct H1Norm
{
/// Evaluation of square norm
  template<typename Function>
  double square(Function f)
  {
    H1SemiNorm h1Norm;
    L2Norm l2Norm;
    return h1Norm.square(f)+l2Norm.square(f);
  }

/// Evaluation of norm
  template<typename Function>
  double operator()(Function f)
  {
    return std::sqrt(square(f));
  }
};

template<class Space> class LocalIntegral;
template<class Grid, class T> class CellData;

/// local (cellwise) H1-semi-norms
template<class Function>
typename CellData<typename Function::Space::Grid, typename Function::ValueType>::CellDataVector localH1SemiNorm(Function const& f)
{
  typedef typename Function::Space Space;
  typedef typename Space::Grid Grid;
  LocalIntegral<Space> localIntegral;
  typename CellData<Grid,typename Function::ValueType>::CellDataVector
    errorIndicator(localIntegral(
                     makeView<FunctionViews::AbsSquare>(makeView<FunctionViews::Gradient>(f))));
  return errorIndicator;
}

/// local (cellwise) L2-norms
template<class Function>
typename CellData<typename Function::Space::Grid, typename Function::ValueType>::CellDataVector localL2Norm(Function const& f)
{
  typedef typename Function::Space Space;
  typedef typename Space::Grid Grid;
  LocalIntegral<Space> localIntegral;
  typename CellData<Grid,typename Function::ValueType>::CellDataVector
    errorIndicator(localIntegral(
                     makeView<FunctionViews::AbsSquare>(f)));
  return errorIndicator;
}

/**
 * @brief boundaryL2Norm computes the L2-norm of an FE function on the whole boundary of the underlying grid.
 *
 * @param function is the FE function to be integrated.
 * @tparam FEFunction is the type of the integrand.
 * @return value of integral
 *
 */
template <class FEFunction>
typename FEFunction::Space::Scalar boundaryL2Norm(FEFunction const& function) {
  FunctionViews::AbsSquare<FEFunction> functionSquared(function);
  return std::sqrt(integrateOverBoundary(functionSquared));
}
} // end of namespace Kaskade
#endif


/*
 * Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   SideScanGeoreferencing.hpp
 * Author: Jordan McManus <jordan.mcmanus@cidco.ca>
 *
 * Created on March 11, 2020, 12:40 PM
 */

#ifndef SIDESCANGEOREFERENCING_HPP
#define SIDESCANGEOREFERENCING_HPP

#include <Eigen/Dense>
#include "../Position.hpp"
#include "../math/CoordinateTransform.hpp"

class SideScanGeoreferencing {
public:

    static void georeferenceSideScanEcef(
            Eigen::Vector3d & antennaEcef,
            Eigen::Vector3d & antenna2TowPointLeverArmEcef,
            Eigen::Vector3d & layBackEcef,
            Eigen::Vector3d & sideDistanceEcef,
            Position & georeferencedPosition
            ) {

        Eigen::Vector3d objectPositionEcef =
                antennaEcef +
                antenna2TowPointLeverArmEcef +
                layBackEcef +
                sideDistanceEcef;

        CoordinateTransform::convertECEFToLongitudeLatitudeElevation(objectPositionEcef, georeferencedPosition);
    }
};

#endif /* SIDESCANGEOREFERENCING_HPP */