/**
 * @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;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

int main()
{
    using mtl::srange; using mtl::imax;
    // For a more realistic example set sz to 1000 or larger
    const int size = 3, N = size * size;

    typedef mtl::compressed2D<double>  	   matrix_type;
    typedef itl::pc::ic_0<matrix_type> 	   ic_type;
    typedef itl::pc::diagonal<matrix_type> dia_type;

    mtl::compressed2D<double>          A;
    laplacian_setup(A, size, size);
    // mtl::io::tout << "A is\n" << A << '\n';

    itl::pc::concat<ic_type, dia_type, matrix_type> L(A);
    // ic_type                                         L(A);
    itl::pc::identity<matrix_type>                  R(A);

    mtl::dense_vector<double>          x(N, 1.0), b(N);

    b = A * x;
    x= 0;

    itl::cyclic_iteration<double> iter(b, N, 1.e-6, 0.0, 3);
    cg(A, x, b, L, iter);

    // Test if adjoint works (do not try bicg, it convergences poorly)
    x= 0;
    itl::cyclic_iteration<double> iter2(b, N, 1.e-6, 0.0, 5);
    qmr(A, x, b, L, R, iter2);

    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


// solving A * X = B
// using driver function gesv()

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <boost/numeric/bindings/blas/level1/set.hpp>
#include <boost/numeric/bindings/blas/level3/gemm.hpp>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
#endif

int main() {

  cout << endl;

  size_t n = 5;
  m_t a (n, n);   // system matrix

  size_t nrhs = 2;
  m_t x (n, nrhs), bb (n, nrhs);
  // b -- right-hand side matrix, see below

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  m_t const aa (a); // copy of a, because a is `lost' after gesv()

  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  blas::set( 1., xc0 );
  blas::set (2., xc1);
  blas::gemm ( 1., a, x, 0.0, bb);  // bb = a x, so we know the result ;o)

  print_m (a, "A");
  cout << endl;
  print_m (bb, "B");
  cout << endl;

  // see leading comments for `gesv()' in clapack.hpp
#ifndef F_ROW_MAJOR
  m_t b (bb);
#else
  m_t b (ublas::trans (bb));
#endif
  print_m (b, "B for gesv()");
  cout << endl;

  std::vector< int > pivot( bindings::size1( a ) );
  lapack::gesv (a, pivot,  b);  // solving the system, b contains x

#ifndef F_ROW_MAJOR
  print_m (b, "X");
  cout << endl;
  blas::gemm (1.0, aa, b, 0.0, x);
#else
  print_m (b, "X^T");
  cout << endl;
  blas::gemm ( 1.0, aa, bindings::trans(b), 0.0, x);
#endif
  print_m (x, "B = A X");

  cout << endl;

}


#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 "circle.hpp"

#include <cmath>
#include <iostream>
#include <stdexcept>

#include <boost/format.hpp>

#include "base-types.hpp"

yakovlev::Circle::Circle(double radius, const point_t & center, double) :
  radius_{ radius },
  center_{ center }
{
  if (radius <= 0.0) {
    throw std::invalid_argument{ (boost::format("Invalid circle radius %1% - can not be negative or zero") % radius ).str() };
  }
}

bool yakovlev::Circle::operator==(const Circle & other) const noexcept
{
  return (center_ == other.center_) && (radius_ == other.radius_);
}

bool yakovlev::Circle::operator!=(const Circle & other) const noexcept
{
  return !(*this == other);
}

double yakovlev::Circle::getRadius() const noexcept
{
  return radius_;
}

yakovlev::point_t yakovlev::Circle::getCenter() const noexcept
{
  return center_;
}

double yakovlev::Circle::getArea() const noexcept
{
  return M_PI * radius_ * radius_;
}

yakovlev::rectangle_t yakovlev::Circle::getFrameRect() const noexcept
{
  return { radius_ * 2.0, radius_ * 2.0, center_ };
}

void yakovlev::Circle::move(const point_t & pos) noexcept
{
  center_ = pos;
}

void yakovlev::Circle::move(double x, double y) noexcept
{
  center_.x += x;
  center_.y += y;
}

void yakovlev::Circle::rotate(double) noexcept
{ }

void yakovlev::Circle::scale(double coef)
{
  if (coef <= 0.0) {
    throw std::invalid_argument{ (boost::format("Invalid circle scale coefficient %1% - can not be negative or zero") % coef ).str() };
  }
  radius_ *= coef;
}

void yakovlev::Circle::print(std::ostream & os) const
{
  os << "Circle with a radius of " << radius_ << " placed on " << center_ << '\n';
}


/* test_old_uniform_int.cpp
 *
 * Copyright Steven Watanabe 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)
 *
 * $Id$
 *
 */

#include <boost/random/uniform_int.hpp>
#include <boost/math/distributions/uniform.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::uniform_int<>
#define BOOST_RANDOM_DISTRIBUTION_NAME uniform_int
#define BOOST_MATH_DISTRIBUTION boost::math::uniform
#define BOOST_RANDOM_ARG1_TYPE int
#define BOOST_RANDOM_ARG1_NAME b
#define BOOST_RANDOM_ARG1_DEFAULT 1000
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_int<>(0, n)
#define BOOST_RANDOM_DISTRIBUTION_INIT (0, b)
#define BOOST_MATH_DISTRIBUTION_INIT (0, b+1)
#define BOOST_RANDOM_DISTRIBUTION_MAX b

#include "test_real_distribution.ipp"


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.operator toolbox - dist/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.operator components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <nt2/arithmetic/include/functions/dist.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/constant/constant.hpp>

NT2_TEST_CASE_TPL ( dist_real__2_0,  BOOST_SIMD_REAL_TYPES)
{

  using nt2::dist;
  using nt2::tag::dist_;
  typedef std::complex<T> cT;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<dist_(cT, cT)>::type r_t;
  typedef typename nt2::meta::scalar_of<r_t>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  typedef nt2::imaginary<T> ciT;
  typedef nt2::dry<T> dT;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );

  // specific values tests
  NT2_TEST_EQUAL(dist(cT(nt2::Inf<T>()), cT(nt2::Inf<T>())), nt2::Nan<T>());
  NT2_TEST_EQUAL(dist(cT(nt2::One<T>()), cT(nt2::Zero<T>())), nt2::One<T>());
  NT2_TEST_EQUAL(dist(cT(nt2::Zero<T>()), cT(nt2::Zero<T>())),nt2::Zero<T>());
  NT2_TEST_ULP_EQUAL(dist(cT(0, 1), cT(1, 0)), nt2::Sqrt_2<T>(), 0.5);
  NT2_TEST_ULP_EQUAL(dist(cT(0, nt2::Inf<T>()), cT(nt2::Inf<T>(), 0)), nt2::Inf<T>(), 0.5);
  NT2_TEST_ULP_EQUAL(dist(nt2::Inf<dT>(),nt2::Inf<dT>()), nt2::Nan<T>(), 0.5);
  NT2_TEST_EQUAL(dist(cT(1, 0), cT(1, 0)), nt2::Zero<T>());
  NT2_TEST_EQUAL(dist(cT(2, 1), ciT(1)), nt2::Two<T>());
  NT2_TEST_EQUAL(dist(ciT(1), ciT(0)), nt2::One<T>());
  NT2_TEST_EQUAL(dist(ciT(1), T(0)), nt2::One<T>());
} // end of test for floating_


#include <math_lib/vec_base.h>

#include <math_lib/length.h>
#include <math_lib/line_segment_3d.h>
#include <boost/qvm/vec_operations.hpp>

#include <gtest/gtest.h>

using namespace pagoda;

TEST(LineSegment3D, when_using_default_constructor_should_create_a_unit_line_segment_in_the_x_axis)
{
	LineSegment3D<float> l;
	ASSERT_TRUE(l.GetSourcePoint() == (Vec3F{0, 0, 0}));
	ASSERT_TRUE(l.GetTargetPoint() == (Vec3F{1, 0, 0}));
}

TEST(LineSegment3D, when_using_two_points_constructor_should_create_the_respective_line_segment)
{
	LineSegment3D<float> l(Vec3F{0, 0, 0}, Vec3F{1, 2, 3});
	ASSERT_TRUE(l.GetSourcePoint() == (Vec3F{0, 0, 0}));
	ASSERT_TRUE(l.GetTargetPoint() == (Vec3F{1, 2, 3}));
}

TEST(LineSegment3D, when_comparing_two_equal_line_segments_should_return_equal)
{
	LineSegment3D<float> l1;
	LineSegment3D<float> l2(Vec3F{0, 0, 0}, Vec3F{1, 0, 0});
	ASSERT_TRUE(l1 == l2);
	ASSERT_FALSE(l1 != l2);
}

TEST(LineSegment3D, when_comparing_two_different_line_segments_should_return_not_equal)
{
	LineSegment3D<float> l1(Vec3F{0, 0, 0}, Vec3F{1, 2, 3});
	LineSegment3D<float> l2(Vec3F{0, 0, 0}, Vec3F{1, 2, 4});
	ASSERT_FALSE(l1 == l2);
	ASSERT_TRUE(l1 != l2);
}


//######################################################################
//#   Refine_depth Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                Roman Kaskman under supervision of Slobodan Ilic
//#######################################################################

#ifndef IOUTILS_HPP
#define IOUTILS_HPP

#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "rgbd_types.hpp"
#include <iomanip>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <filesystem>
#include <fstream>

namespace fs = std::filesystem;

template<typename T>
inline std::vector<T> read_matrix_one_line(const std::string &in_file) {
  std::string line;
  std::vector<T> elements;

  std::ifstream f(in_file);

  while (std::getline(f, line)) {
    std::stringstream ss(line);
    T current_value;

    while (ss >> current_value) {
      elements.push_back(current_value);
    }

  }

  return elements;
}

template<typename T, int R, int C>
inline Eigen::Matrix<T, R, C> matrix_from_vector_rowwise(std::vector<T> v) {
  if (v.size() != R * C) {
    std::cerr << "Invalid input vector of size " << v.size() << " for " << R << "x" << C << " matrix" << std::endl;
    throw std::invalid_argument("Invalid vector size");
  }

  return Eigen::Map<Eigen::Matrix<T, R, C, Eigen::RowMajor>>(&v[0]);
}

template<typename T, int R, int C>
inline Eigen::Matrix<T, R, C> read_matrix_from_file(const std::string &in_file) {
  std::vector<T> intrinsics_vector = read_matrix_one_line<T>(in_file);
  return matrix_from_vector_rowwise<T, R, C>(intrinsics_vector);
}

template<typename T>
inline Eigen::Matrix<T, 4, 4> read_pose_from_file(const std::string &in_file) {
  return read_matrix_from_file<T, 4, 4>(in_file);
}

template<typename T>
inline Eigen::Matrix<T, 3, 3> read_intrinsics_from_file(const std::string &in_file) {
  return read_matrix_from_file<T, 3, 3>(in_file);
}

inline RgbdFrame get_rgbd_frame(const RgbdFile &rgbd_file)
{
	cv::Mat rgb = cv::imread(rgbd_file.first);
	cv::Mat depth = cv::imread(rgbd_file.second, -1);

	cv::Mat depth_float;
	depth.convertTo(depth_float, CV_32FC1, 0.001);

	return RgbdFrame(rgb, depth_float);
}

template<typename T>
inline std::vector<Eigen::Matrix<T, 4, 4>> read_scene_poses(const std::string &poses_file)
{
	std::ifstream infile(poses_file);
	std::string line;

	std::vector<Eigen::Matrix<T, 4, 4>> poses;
	int i = 0;

	Eigen::Matrix<T, 4, 4> temp_matrix;

	while (std::getline(infile, line))
	{
		if (i % 5 == 0)
		{
			if (i > 0)
			{
				poses.push_back(temp_matrix);
				temp_matrix = Eigen::Matrix<T, 4, 4>();
			}
		}
		else
		{
			int row = (i % 5) - 1;
			std::stringstream ss(line);

			T current_value;
			int col = 0;

			while (ss >> current_value)
			{
				temp_matrix(row, col) = current_value;
				col++;
			}
		}
		i++;
	}

	poses.push_back(temp_matrix);

	return poses;
}

template <typename T>
inline void store_poses(const std::vector<Eigen::Transform<T, 3, Eigen::Isometry>> &subsampled_valid_poses, const std::string &out_poses_file)
{
	std::ofstream poses_out_stream;

	poses_out_stream.open(out_poses_file);
	for (int i = 0; i < static_cast<int>(subsampled_valid_poses.size()); ++i)
	{
		const Eigen::Matrix<T, 4, 4> &out_pose = subsampled_valid_poses[i].matrix();
		poses_out_stream << i << std::endl << out_pose << std::endl;
	}

	poses_out_stream.close();
}

inline void store_images(const std::vector<RgbdFile> &rgbd_files, const std::string &out_rgb_dir, const std::string &out_depth_dir)
{
	int i = 0;
	for (const auto &rgbd_file : rgbd_files)
	{
		const std::string &in_rgb_file = rgbd_file.first;
		const std::string &in_depth_file = rgbd_file.second;

		std::stringstream filename_ss;
		filename_ss << std::setfill('0') << std::setw(6) << i << ".png";
		const std::string filename = filename_ss.str();

		std::string out_rgb_file = (fs::path(out_rgb_dir) / filename).string();
		std::string out_depth_file = (fs::path(out_depth_dir) / filename).string();

		fs::copy_file(in_rgb_file, out_rgb_file, fs::copy_options::overwrite_existing);
		fs::copy_file(in_depth_file, out_depth_file, fs::copy_options::overwrite_existing);

		i++;
	}
}

inline std::string to_plain_filename(const std::string &f)
{
	return fs::path(f).filename().string();
}

inline std::vector<RgbdFile> get_input_images(const std::string &rgb_images_dir, const std::string &depth_images_dir)
{
	cv::String rgb_file_pattern = (fs::path(rgb_images_dir) / "*.png").string();
	cv::String depth_file_pattern = (fs::path(depth_images_dir) / "*.png").string();

	std::vector<cv::String> rgb_files;
	std::vector<cv::String> depth_files;

	cv::glob(rgb_file_pattern, rgb_files);
	cv::glob(depth_file_pattern, depth_files);

	std::vector<std::string> rgb_filenames(rgb_files.size());
	std::vector<std::string> depth_filenames(depth_files.size());

	std::transform(rgb_files.begin(), rgb_files.end(), rgb_filenames.begin(), to_plain_filename);
	std::transform(depth_files.begin(), depth_files.end(), depth_filenames.begin(), to_plain_filename);

	std::sort(rgb_filenames.begin(), rgb_filenames.end());
	std::sort(depth_filenames.begin(), depth_filenames.end());

	std::vector<std::string> image_filenames;
	std::set_intersection(rgb_filenames.begin(), rgb_filenames.end(),
		depth_filenames.begin(), depth_filenames.end(), back_inserter(image_filenames));

	std::vector<RgbdFile> rgbd_files(image_filenames.size());

	std::transform(image_filenames.begin(), image_filenames.end(), rgbd_files.begin(), [&rgb_images_dir, &depth_images_dir](const std::string &fn)
	{
		const std::string &rgb_file = (fs::path(rgb_images_dir) / fn).string();
		const std::string &depth_file = (fs::path(depth_images_dir) / fn).string();
		return RgbdFile(rgb_file, depth_file);
	});

	return rgbd_files;
}

#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>;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/geometry/geometry_compute_signed_distance_to_triangle.h>
#include <cmath> // needed for std::sqrt

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_geometry_util_compute_signed_distance_to_triangle);

  BOOST_AUTO_TEST_CASE(case_by_case_testing)
  {
    using std::sqrt;

    typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
    typedef math_types::vector3_type                         vector3_type;
    typedef math_types::real_type                            real_type;

    real_type tol = 0.0001;

    vector3_type p;
    vector3_type pi(-5,0,0);
    vector3_type pj(5,0,0);
    vector3_type pk(0,5,0);
    vector3_type nv_i = unit( vector3_type(-1,-1,1) );
    vector3_type nv_j = unit( vector3_type(1,-1,1) );
    vector3_type nv_k = unit( vector3_type(0,1,1) );
    vector3_type ne_i(1,1,1);
    vector3_type ne_j(-1,1,1);
    vector3_type ne_k(0,-1,1);

    p = vector3_type(0,2.5,2);

    real_type d0 = OpenTissue::geometry::compute_signed_distance_to_triangle( p, pi, pj, pk, nv_i, nv_j, nv_k, ne_i, ne_j, ne_k  );
    BOOST_CHECK_CLOSE(d0, 2.0, tol);

    p = vector3_type(0,2.5,-2);
    real_type d1 = OpenTissue::geometry::compute_signed_distance_to_triangle( p, pi, pj, pk, nv_i, nv_j, nv_k, ne_i, ne_j, ne_k  );
    BOOST_CHECK_CLOSE(d1, -2.0, tol);

    nv_i = vector3_type(0,1,0);
    nv_j = vector3_type(0,1,0);
    nv_k = vector3_type(0,-1,0);
    ne_i = vector3_type(0,1,0);
    ne_j = vector3_type(0,1,0);
    ne_k = vector3_type(0,1,0);
    p = vector3_type(0,7,0);
    real_type d2 = OpenTissue::geometry::compute_signed_distance_to_triangle( p, pi, pj, pk, nv_i, nv_j, nv_k, ne_i, ne_j, ne_k  );
    BOOST_CHECK_CLOSE(d2, -2.0, tol);

    nv_i = vector3_type(1,0,0);
    nv_j = vector3_type(-1,0,0);
    nv_k = vector3_type(-1,0,0);
    ne_i = vector3_type(-1,0,0);
    ne_j = vector3_type(-1,0,0);
    ne_k = vector3_type(-1,0,0);
    p = vector3_type(-7,0,0);
    real_type d3 = OpenTissue::geometry::compute_signed_distance_to_triangle( p, pi, pj, pk, nv_i, nv_j, nv_k, ne_i, ne_j, ne_k  );
    BOOST_CHECK_CLOSE(d3, -2.0, tol);

    nv_i = vector3_type(1,0,0);
    nv_j = vector3_type(-1,0,0);
    nv_k = vector3_type(1,0,0);
    ne_i = vector3_type(1,0,0);
    ne_j = vector3_type(1,0,0);
    ne_k = vector3_type(1,0,0);
    p = vector3_type(7,0,0);
    real_type d4 = OpenTissue::geometry::compute_signed_distance_to_triangle( p, pi, pj, pk, nv_i, nv_j, nv_k, ne_i, ne_j, ne_k  );
    BOOST_CHECK_CLOSE(d4, -2.0, tol);

    nv_i = vector3_type(0,-1,0);
    nv_j = vector3_type(0,-1,0);
    nv_k = vector3_type(0,-1,0);
    ne_i = vector3_type(0,-1,0);
    ne_j = vector3_type(0,-1,0);
    ne_k = vector3_type(0,1,0);
    p = vector3_type(0,-2,0);
    real_type d5 = OpenTissue::geometry::compute_signed_distance_to_triangle( p, pi, pj, pk, nv_i, nv_j, nv_k, ne_i, ne_j, ne_k  );
    BOOST_CHECK_CLOSE(d5, -2.0, tol);

    nv_i = vector3_type(1,1,0);
    nv_j = vector3_type(1,1,0);
    nv_k = vector3_type(1,1,0);
    ne_i = vector3_type(-1,-1,0);
    ne_j = vector3_type(1,1,0);
    ne_k = vector3_type(1,1,0);
    p = vector3_type(5,5,0);
    real_type d6 = OpenTissue::geometry::compute_signed_distance_to_triangle( p, pi, pj, pk, nv_i, nv_j, nv_k, ne_i, ne_j, ne_k  );
    BOOST_CHECK_CLOSE(d6, -3.5355339059327376220042218105242, tol);

    nv_i = vector3_type(-1, 1, 0);
    nv_j = vector3_type(-1, 1, 0);
    nv_k = vector3_type(-1, 1, 0);
    ne_i = vector3_type(-1, 1, 0);
    ne_j = vector3_type( 1,-1, 0);
    ne_k = vector3_type(-1, 1, 0);
    p = vector3_type(-5,5,0);
    real_type d7 = OpenTissue::geometry::compute_signed_distance_to_triangle( p, pi, pj, pk, nv_i, nv_j, nv_k, ne_i, ne_j, ne_k  );
    BOOST_CHECK_CLOSE(d7, -3.5355339059327376220042218105242, tol);
  }

BOOST_AUTO_TEST_SUITE_END();


#include "SPX.h"
#include <NTL/lzz_pXFactoring.h>

using namespace std;
using namespace NTL;

#if ((NTL_MAJOR_VERSION<9) || ((NTL_MAJOR_VERSION==9)&&(NTL_MINOR_VERSION<2)))
#warning "NTL < 9.2.0, using SPmodulus wrapper"
#else
#warning "NTL >= 9.2.0, using zz_pContext directly"
#endif

static void zpxFromSPX(zz_pX& zpx, const SPX& spp)
{
  Vec<long> v;
  spp.getCoeffVec(v);  // convert to zz_pX
  conv(zpx.rep, v);
  zpx.normalize();
}

static bool testGCD(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

   SPX b, c, ss, tt;
   zz_pX a_p, b_p, c_p, ss_p, tt_p;

  // test polynomials of various degrees, from 32 to 8192
  for (long n = 32; n <= 8192; n <<= 2) {
    random(a, n, mod);   // get random polynomial
    zpxFromSPX(a_p,a);// convert to zz_pX
    random(b_p, n);      // get random zz_pX
    b = SPX(b_p,mod); // convert to SPX

    GCD(c_p, a_p, b_p);  // work on zz_pX
    GCD(c, a, b);        // work on SPX
    if (c != SPX(c_p,mod)) {
      cerr << "**** SPX Test GCD FAILED!\n";
      return false;
    }

    XGCD(c_p, ss_p, tt_p, a_p, b_p);
    XGCD(c, ss, tt, a, b);
    if (c != SPX(c_p,mod) ||
	ss != SPX(ss_p,mod) || tt != SPX(tt_p,mod)) {
      cerr << "**** SPX Test XGCD FAILED!\n";
      return false;
    }
  }
  return true;
}

static bool testArith(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

   SPX b, c, d;
   zz_pX a_p, b_p, c_p, d_p;

  // test polynomials of various degrees, from 32 to 8192
  for (long n = 32; n <= 8192; n <<= 2) {
    random(a, n, mod);   // get random polynomial
    zpxFromSPX(a_p,a);// convert to zz_pX
    random(b_p, n);      // get random zz_pX
    b = SPX(b_p,mod); // convert to SPX

    c   = (a  +b);
    c_p = (a_p+b_p);
    if (c != SPX(c_p,mod)) {
      cerr << "  ** SPX testArith (1) FAILED!\n";
      return false;
    }
    //    cout << "." << std::flush;

    DivRem(c,  d,  c,  b);  // c = c/b, d=c%b
    DivRem(c_p,d_p,c_p,b_p);
    if (c != SPX(c_p,mod) || d != SPX(d_p,mod)) {
      cerr << "  ** SPX testArith (2) FAILED!\n";
      return false;
    }
    //    cout << "," << std::flush;

    d  = (c << 5) - (b >> 2);
    d_p= (c_p<<5) - (b_p>>2);
    if (d != SPX(d_p,mod)) {
      cerr << "  ** SPX testArith (3) FAILED!\n";
      return false;
    }
    //    cout << ";" << std::flush;

    --d;   ++c;
    --d_p; ++c_p;
    if (c != SPX(c_p,mod) || d != SPX(d_p,mod)) {
      cerr << "  ** SPX testArith (4) FAILED!\n";
      return false;
    }
    //    cout << ";" << std::flush;
  }
  return true;
}

static bool testCoeffs(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

   zz_pX a_p;

  // test polynomials of various degrees, from 32 to 8192
  for (long n = 32; n <= 8192; n <<= 2) {
    random(a, n, mod);   // get random polynomial
    zpxFromSPX(a_p,a);// convert to zz_pX

    if (LeadCoeff(a) != conv<long>(LeadCoeff(a_p))
	|| ConstTerm(a) != conv<long>(ConstTerm(a_p))) {
      cerr << "**** SPX testCoeffs (1) FAILED!\n";
      return false;
    }

    SetCoeff(a, 10, 3);
    SetCoeff(a_p, 10, 3);
    if (a != SPX(a_p,mod)) {
      cerr << "**** SPX testCoeffs (2) FAILED!\n";
      return false;
    }

    SetX(a);
    SetX(a_p);
    if (!IsX(a) || a != SPX(a_p,mod))  {
      cerr << "**** SPX testCoeffs (3) FAILED!\n";
      return false;
    }

    clear(a);
    clear(a_p);
    if (!IsZero(a) || a != SPX(a_p,mod))  {
      cerr << "**** SPX testCoeffs (4) FAILED!\n";
      return false;
    }

    set(a);
    set(a_p);
    if (!IsOne(a) || a != SPX(a_p,mod))  {
      cerr << "**** SPX testCoeffs (5) FAILED!\n";
      return false;
    }
  }
  return true;
}

static bool testMisc(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

  SPX b;
  zz_pX a_p, b_p;

  // test polynomials of various degrees, from 32 to 8192
  for (long n = 32; n <= 8192; n <<= 2) {
    random(a, n, mod);   // get random polynomial
    zpxFromSPX(a_p,a);// convert to zz_pX

    diff(b,  a);
    diff(b_p,a_p);
    if (b != SPX(b_p,mod)) {
      cerr << "**** SPX testMisc (1) FAILED!\n";
      return false;
    }
    reverse(b,  a);
    reverse(b_p,a_p);
    if (b != SPX(b_p,mod)) {
      cerr << "**** SPX testMisc (2) FAILED!\n";
      return false;
    }
    reverse(b,  a,  10);
    reverse(b_p,a_p,10);
    if (b != SPX(b_p,mod)) {
      cerr << "**** SPX testMisc (3) FAILED!\n";
      return false;
    }
  }
  return true;
}

static bool testTrunc(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

  SPX b, c;
  zz_pX a_p, b_p, c_p;

  // test polynomials of various degrees, from 32 to 8192
  for (long n = 32; n <= 8192; n <<= 2) {
    random(a, n, mod);   // get random polynomial
    zpxFromSPX(a_p,a);// convert to zz_pX
    random(b_p, n);      // get random zz_pX
    b = SPX(b_p,mod); // convert to SPX

    MulTrunc(c,  a,  b,  32);    MulTrunc(c_p,a_p,b_p,32);
    SqrTrunc(c,  c,  48);        SqrTrunc(c_p,c_p,48);
    if (c != SPX(c_p,mod)) {
      cerr << "**** SPX testTrunc (1) FAILED!\n";
      return false;
    }

    if (coeff(c,0)==0) {
      SetCoeff(c,0);  SetCoeff(c_p,0);
    }
    InvTrunc(c,  c,  40);        InvTrunc(c_p,c_p,40);
    if (c != SPX(c_p,mod)) {
      cerr << "**** SPX testTrunc (2) FAILED!\n";
      return false;
    }
  }
  return true;
}

static bool testMod(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

  SPX b, c;
  zz_pX a_p, b_p, c_p, F_p;

  // test polynomials of various degrees, from 32 to 512
  for (long n = 32; n <= 512; n <<= 2) {
    BuildIrred(F_p,n+1);  // build an irreducible polynomial
    SPX F(F_p,mod);    // convert to SPX

    random(a, n, mod);   // get random polynomial
    zpxFromSPX(a_p,a);// convert to zz_pX
    random(b_p, n);      // get random zz_pX
    b = SPX(b_p,mod); // convert to SPX

    MulMod(c,  a,  b,  F);    MulMod(c_p,a_p,b_p,F_p);
    SqrMod(c,  c,  F);        SqrMod(c_p,c_p,F_p);
    if (c != SPX(c_p,mod)) {
      cerr << "**** SPX testMod (1) FAILED!\n";
      return false;
    }

    MulByXMod(c, a, F); MulByXMod(c_p,a_p,F_p);
    InvMod(c, c, F);    InvMod(c_p,c_p,F_p);
    if (c != SPX(c_p,mod)) {
      cerr << "**** SPX testMod (2) FAILED!\n";
      return false;
    }
  }
  return true;
}

static bool testModPre(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

  SPX b, c;
  zz_pX a_p, b_p, c_p;

  // test polynomials of various degrees, from 32 to 512
  for (long n = 32; n <= 512; n <<= 2) {
    BuildIrred(c_p,n+1);  // build an irreducible polynomial
    zz_pXModulus F_p=c_p; // set it as modulus
    c = SPX(c_p,mod);  // convert to SPX
    SPXModulus F=c;

    random(a, n, mod);   // get random polynomial
    zpxFromSPX(a_p,a);// convert to zz_pX
    random(b_p, n);      // get random zz_pX
    b = SPX(b_p,mod); // convert to SPX

    MulMod(c,  a,  b,  F);    MulMod(c_p,a_p,b_p,F_p);
    SqrMod(c,  c,  F);        SqrMod(c_p,c_p,F_p);
    if (c != SPX(c_p,mod)) {
      cerr << "**** SPX testModPre (1) FAILED!\n";
      return false;
    }

    PowerXMod(b, n+5, F); PowerXMod(b_p,n+5,F_p);
    MulByXMod(c, b, F);   MulByXMod(c_p,b_p,F_p);
    if (c != SPX(c_p,mod)) {
      cerr << "**** SPX testModPre (2) FAILED!\n";
      return false;
    }

    PowerMod(b, c, n/2, F); PowerMod(b_p, c_p, n/2, F_p);
    if (b != SPX(b_p,mod)) {
      cerr << "**** SPX testModPre (3) FAILED!\n";
      return false;
    }

    random(a_p, 2*n);    // get random zz_pX
    a = SPX(a_p,mod); // convert to SPX
    DivRem(b, c, a, F);  DivRem(b_p,c_p,a_p,F_p);
    if (b != SPX(b_p,mod) || c != SPX(c_p,mod)) {
      cerr << "**** SPX testModPre (4) FAILED!\n";
      return false;
    }

    b = c = a;
    b_p = c_p = a_p;
    b /= F; b_p /= F_p;
    c %= F; c_p %= F_p;
    if (b != SPX(b_p,mod) || c != SPX(c_p,mod)) {
      cerr << "**** SPX testModPre (5) FAILED!\n";
      return false;
    }
  }
  return true;
}

static bool testComp(SPX& a1)
{
  const SPmodulus& mod = a1.getMod();
  getContext(mod).restore();

  SPX a2, a3, b1, b2, b3, c;
  zz_pX a1_p,a2_p,a3_p,b1_p,b2_p,b3_p,c_p;

  // test polynomials of various degrees, from 32 to 512
  for (long n = 32; n <= 512; n <<= 2) {
    BuildIrred(c_p,n+1);  // build an irreducible polynomial
    zz_pXModulus F_p=c_p; // set it as modulus
    c = SPX(c_p,mod);  // convert to SPX
    SPXModulus F=c;

    random(b1_p, n);      // get random zz_pX
    b1 = SPX(b1_p,mod); // convert to SPX

    random(b2_p, n);      // get random zz_pX
    b2 = SPX(b2_p,mod); // convert to SPX

    random(b3_p, n);      // get random zz_pX
    b3 = SPX(b3_p,mod); // convert to SPX

    random(c_p, 3);      // get random zz_pX
    c = SPX(c_p,mod); // convert to SPX

    CompMod(a1, b1, c, F); CompMod(a1_p,b1_p,c_p,F_p);
    if (a1 != SPX(a1_p,mod)) {
      cerr << "**** SPX testComp (1) FAILED!\n";
      return false;
    }

    Comp2Mod(a1, a2, b1, b2, c, F); Comp2Mod(a1_p,a2_p,b1_p,b2_p,c_p,F_p);
    if (a1 != SPX(a1_p,mod) || a2 != SPX(a2_p,mod)) {
      cerr << "**** SPX testComp (2) FAILED!\n";
      return false;
    }

    Comp3Mod(a1,  a2,  a3,  b1,  b2,  b3,  c,  F);
    Comp3Mod(a1_p,a2_p,a3_p,b1_p,b2_p,b3_p,c_p,F_p);
    if (a1 != SPX(a1_p,mod)
	|| a2 != SPX(a2_p,mod) || a3 != SPX(a3_p,mod)) {
      cerr << "**** SPX testComp (3) FAILED!\n";
      return false;
    }

    SPXArgument H;  build(H,  c,  F, 4);
    zz_pXArgument H_p; build(H_p,c_p,F_p,4);

    CompMod(a1, b1, H, F); CompMod(a1_p,b1_p,H_p,F_p);
    if (a1 != SPX(a1_p,mod)) {
      cerr << "**** SPX testComp (4) FAILED!\n";
      return false;
    }
  }
  return true;
}

static bool testProj(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

  Vec<long> v;
  SPX f;

  Vec<zz_p> v_p;
  zz_pX a_p,f_p;

  // test polynomials of various degrees, from 32 to 512
  for (long n = 32; n <= 512; n <<= 2) {
    BuildIrred(f_p,n+1);  // build an irreducible polynomial
    f = SPX(f_p,mod);  // convert to SPX

    random(a_p, n);      // get random zz_pX
    a = SPX(a_p,mod); // convert to SPX

    VectorCopy(v_p,a_p,deg(a_p)+1); // get coefficient vector
    a.getCoeffVec<long>(v);

    random(a_p, n);      // get random zz_pX
    a = SPX(a_p,mod); // convert to SPX

    {long x = project(v, a);
    zz_p x_p = project(v_p, a_p);
    if (x != conv<long>(x_p)) {
      cerr << "**** SPX testProj (1) FAILED! ("
	   << x << "!=" << conv<long>(x_p) << ")\n";
      return false;
    }}
    Vec<long> x;
    Vec<zz_p> x_p;
    ProjectPowers(x,  v,  5, a, f);
    ProjectPowers(x_p,v_p,5,a_p,f_p);
    if (x != conv< Vec<long> >(x_p)) {
      cerr << "**** SPX testProj (2) FAILED!\n";
      return false;
    }

    SPXModulus F=f;
    ProjectPowers(x,  v,  5, a, F);
    if (x != conv< Vec<long> >(x_p)) {
      cerr << "**** SPX testProj (3) FAILED!\n";
      return false;
    }
  }
  return true;
}

/*
static bool testMinp(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();
  // ...
}
*/

static bool testTrace(SPX& a)
{
  const SPmodulus& mod = a.getMod();
  getContext(mod).restore();

  SPX f;
  zz_pX ap, fp;

  // test polynomials of various degrees, from 32 to 512
  for (long n = 32; n <= 512; n <<= 2) {
    BuildIrred(fp,n+1);  // build an irreducible polynomial
    f = SPX(fp,mod);  // convert to SPX
    SPXModulus F=f;

    random(ap, n);      // get random zz_pX
    a = SPX(ap,mod); // convert to SPX

    long t = TraceMod(a, f);
    zz_p tp= TraceMod(ap,fp);
    if (t != conv<long>(tp)) {
      cerr << "**** SPX testTrace (1) FAILED!\n";
      return false;
    }

    t = TraceMod(a, F);
    if (t != conv<long>(tp)) {
      cerr << "**** SPX testTrace (2) FAILED!\n";
      return false;
    }

    Vec<long> tv = TraceVec(f);
    Vec<zz_p> tvp= TraceVec(fp);
    if (tv != conv<vec_long>(tvp)) {
      cerr << "**** SPX testTrace (3) FAILED!\n";
      return false;
    }
  }
  return true;
}

int main()
{
  for (long p=11; p>1; p =(p-1)/2) { // test p=11, p=5, and p=2
    SPmodulus mod(p);
    SPX a(mod);
    getContext(mod).restore();

    cout << "\np="<<p<<endl;
    if (testGCD(a))   cout << "  testGCD\tPASS\n" << std::flush;
    if (testArith(a)) cout << "  testArith\tPASS\n" << std::flush;
    if (testCoeffs(a)) cout<< "  testCoeffs\tPASS\n" << std::flush;
    if (testMisc(a))  cout << "  testMisc\tPASS\n" << std::flush;
    if (testTrunc(a)) cout << "  testTrunc\tPASS\n" << std::flush;
    if (testMod(a))   cout << "  testMod\tPASS\n" << std::flush;
    if (testModPre(a)) cout<< "  testModPre\tPASS\n" << std::flush;
    if (testComp(a))  cout << "  testComp\tPASS\n" << std::flush;
    if (testProj(a))  cout << "  testProj\tPASS\n" << std::flush;
    //    if (testMinp(a))  cout << "  testMinp\tPASS\n" << std::flush;
    if (testTrace(a)) cout << "  testTrace\tPASS\n" << std::flush;
  }
}