//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_INF_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_INF_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Inf Inf (function template)

  Generates a constant greater than all other numeric values of a given type.

  @headerref{<boost/simd/constant/inf.hpp>}

  @par Description

  1.  @code
      template<typename T> T Inf();
      @endcode

  2.  @code
      template<typename T> T Inf( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T which value is greater than all other numeric values of type @c T.
  Compared to the [C++ standard infinity](http://en.cppreference.com/w/cpp/types/numeric_limits/infinity),
  `Inf<T>()` is defined for both IEEEValue and IntegerValue.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to:
  - `T(std::numeric_limits<scalar_of_t<T>>::inf()` if @c T models IEEEValue.
  - [the maximum representable value](@ref constant-Valmax) if @c T models IntegerValue.

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/inf.hpp>
#include <boost/simd/constant/simd/inf.hpp>

#endif


/**
 * @file TriangulationOperator.hpp
 * @author bwu
 * @brief Triangulation operator that manipulating the triangulation data
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_TRI_TRIANGULATIONOPERATOR_HPP
#define GENERIC_GEOMETRY_TRI_TRIANGULATIONOPERATOR_HPP
#include <boost/geometry/index/rtree.hpp>
#include <boost/container/flat_set.hpp>
#include "Triangulation.hpp"
#include <unordered_map>
#include <unordered_set>
namespace generic  {
namespace geometry {
namespace tri {

///@base template of vertex index tree
namespace bgi = boost::geometry::index;
template <typename point_t>
class VertexIndexTree
{
};

///@brief represents a class that manage/search Point2D
template <typename num_type>
class VertexIndexTree<Point2D<num_type> >
{
    using Point = Point2D<num_type>;
    using IndexPoint = std::pair<Point, VerIdx>;
    bgi::rtree<IndexPoint, bgi::linear<16, 4> > m_rtree;
public:
    void Clear() { m_rtree.clear(); }
    void AddOneVertex(const Point & pos, const VerIdx index)
    {
        m_rtree.insert(std::make_pair(pos, index));
    }

    void RemoveVertex(const Point & pos, const VerIdx index)
    {
        m_rtree.remove(std::make_pair(pos, index));
    }

    VerIdx NearestVertex(const Point & pos) const
    {
        std::vector<IndexPoint> query;
        m_rtree.query(bgi::nearest(pos, 1), std::back_inserter(query));
        return query.front().second;
    }
};

///@brief represents a class that operate triangulation data
template <typename point_t>
class TriangulationOperator
{
    using Point = point_t;
    using Triangle = triangle_type<point_t>;
    using VerIdxSet = std::unordered_set<VerIdx>;
    using TriIdxSet = std::unordered_set<TriIdx>;
    using VerIdxMap = std::unordered_map<VerIdx, VerIdx>;
    using TriIdxMap = std::unordered_map<TriIdx, TriIdx>;
    using TriIdxFlatSet = boost::container::flat_set<TriIdx>;
    using EdgeSet = typename Triangulation<Point>::EdgeSet;
    using Utility = TriangulationUtility<Point>;

    VerIdxSet m_removedVertices;
    TriIdxSet m_removedTriangles;
    Triangulation<Point> & m_triangulation;
    VertexIndexTree<Point> m_vertexIndexTree;
public:
    ///@brief constructs a TriangulationOperator that manipulating the triangulation data
    TriangulationOperator(Triangulation<Point> & triangulation)
     : m_triangulation(triangulation)
    {
        RebuildVertexIndexTree();
    }

    bool isVertexRemoved(VerIdx iv) const { return m_removedVertices.count(iv); }
    bool isTriangleRemoved(TriIdx it) const { return m_removedTriangles.count(it); }
    size_t CurrentTriangleSize() const { return m_triangulation.triangles.size() - m_removedTriangles.size(); }
    size_t CurrentVertexSize() const { return m_triangulation.vertices.size() - m_removedVertices.size(); }
    const TriIdxSet & GetRemovedTriangles() const { return m_removedTriangles; }
    const VerIdxSet & GetRemovedVertices() const { return m_removedVertices; }
    VerIdx AddOneVertex(const Point & p, TriIdxSet triangles = TriIdxSet{})
    {
        if(m_removedVertices.size()){
            VerIdx iv = *m_removedVertices.begin();
            m_removedVertices.erase(iv);
            auto & vertex = m_triangulation.vertices[iv];
            vertex.triangles = triangles;
            m_triangulation.points[vertex.index] = p;
            m_vertexIndexTree.AddOneVertex(p, iv);
            return iv;
        }

        IndexVertex vertex;
        vertex.index = m_triangulation.points.size();
        m_triangulation.points.push_back(p);
        vertex.triangles = triangles;
        VerIdx iv = m_triangulation.vertices.size();
        m_triangulation.vertices.push_back(vertex);
        m_vertexIndexTree.AddOneVertex(p, iv);
        return iv;
    }

    void RemoveOneVertex(VerIdx iv)
    {
        m_removedVertices.insert(iv);
        m_triangulation.vertices[iv].Clear();
        m_vertexIndexTree.RemoveVertex(m_triangulation.VerIdxPoint(iv), iv);
    }

    TriIdx AddOneTriangle()
    {
        TriIdx it;
        if(m_removedTriangles.size()){
            it = *m_removedTriangles.begin();
            m_removedTriangles.erase(it);
        }
        else {
            it = m_triangulation.triangles.size();
            m_triangulation.triangles.push_back(IndexTriangle());
        }
        return it;
    }

    TriIdx AddOneTriangle(IndexTriangle t)
    {
        TriIdx it = AddOneTriangle();
        m_triangulation.triangles[it] = t;
        return it;
    }

    template <typename std::enable_if<traits::is_2d_point_t<Point>::value, bool>::type = true>
    TriIdx AddOneTriangle(VerIdx iv1, VerIdx iv2, VerIdx iv3)
    {
        if(!Utility::isCCW(m_triangulation, iv1, iv2, iv3))
            std::swap(iv2, iv3);

        TriIdx it = AddOneTriangle();

        m_triangulation.triangles[it].vertices = std::array<VerIdx, 3>{iv1, iv2, iv3};
        m_triangulation.AddAdjacentTriangle(iv1, it);
        m_triangulation.AddAdjacentTriangle(iv2, it);
        m_triangulation.AddAdjacentTriangle(iv3, it);

        std::pair<TriIdx, TriIdx> ns;
        ns = m_triangulation.GetTriangles(IndexEdge(iv1, iv2));
        m_triangulation.ChangeNeighbor(ns.first, iv1, iv2, ns.second);
        m_triangulation.ChangeNeighbor(ns.second, iv1, iv2, ns.first);

        ns = m_triangulation.GetTriangles(IndexEdge(iv2, iv3));
        m_triangulation.ChangeNeighbor(ns.first, iv2, iv3, ns.second);
        m_triangulation.ChangeNeighbor(ns.second, iv2, iv3, ns.first);

        ns = m_triangulation.GetTriangles(IndexEdge(iv3, iv1));
        m_triangulation.ChangeNeighbor(ns.first, iv3, iv1, ns.second);
        m_triangulation.ChangeNeighbor(ns.second, iv3, iv1, ns.first);

        return it;
    }

    void RemoveOneTriangle(TriIdx it)
    {
        for(VerIdx iv : m_triangulation.triangles[it].vertices)
            m_triangulation.RemoveAdjacentTriangle(iv, it);

        for(TriIdx neighbor : m_triangulation.triangles[it].neighbors){
            if(noNeighbor == neighbor) continue;
            m_triangulation.ChangeNeighbor(neighbor, it, noNeighbor);
        }
        m_triangulation.triangles[it].Reset();
        m_removedTriangles.insert(it);
    }

    template <typename std::enable_if<Point::dim == 2, bool>::type = true>
    std::pair<TriIdx, TriIdx> TraversalTriangleAt(const Point & pos) const
    {
        auto out = std::make_pair(noNeighbor, noNeighbor);
        auto start = m_vertexIndexTree.NearestVertex(pos);
        auto triIdx = TraversalTriangles(start, pos);
        const auto & t  = m_triangulation.triangles[triIdx];
        const auto & p1 = m_triangulation.TrianglePoint(t, 0);
        const auto & p2 = m_triangulation.TrianglePoint(t, 1);
        const auto & p3 = m_triangulation.TrianglePoint(t, 2);
        auto loc = GetPointTriangleLocation(pos, p1, p2, p3);
        if(loc == PointTriangleLocation::Outside)
            throw std::runtime_error("No triangle was found at position");

        out.first = triIdx;
        if(loc < PointTriangleLocation::Inside && loc > PointTriangleLocation::Outside)
            out.second = m_triangulation.triangles[triIdx].neighbors[int(loc) - int(PointTriangleLocation::OnEdge1)];
        return out;
    }

    template <typename std::enable_if<Point::dim == 2, bool>::type = true>
    TriIdx TraversalTriangles(const VerIdx & start, const Point & pos) const
    {
        TriIdx curr = *(m_triangulation.vertices[start].triangles.begin());
        #ifdef GENERIC_UNIT_TEST
        TriIdxFlatSet visited;
        #else//which is faster?
        TriIdxSet visited;
        #endif
        bool found = false;
        while(!found){
            const auto & t = m_triangulation.triangles[curr];
            found = true;

            #ifdef GENERIC_UNIT_TEST
            auto offset = 0;
            #else
            auto offset = math::Random<index_t>(0, 2);
            #endif
            for(auto i = 0; i < 3; ++i){
                auto n = (i + offset) % 3;
                auto sVerPos = m_triangulation.TrianglePoint(t, n);
                auto eVerPos = m_triangulation.TrianglePoint(t, IndexTriangle::ccw(n));
                PointLineLocation edgeCheck = GetPointLineLocation(pos, sVerPos, eVerPos);
                if(edgeCheck == PointLineLocation::Right &&
                    t.neighbors[n] != noNeighbor &&
                    visited.insert(t.neighbors[n]).second){
                    found = false;
                    curr = t.neighbors[n];
                    break;
                }
            }
        }
        return curr;
    }

    void RebuildVertexIndexTree()
    {
        m_vertexIndexTree.Clear();
        for(VerIdx iv = 0; iv < m_triangulation.vertices.size(); ++iv)
            m_vertexIndexTree.AddOneVertex(m_triangulation.VerIdxPoint(iv), iv);
    }

    void ReallocateTriangulation()
    {
        VerIdxMap verIdxMap;
        TriIdxMap triIdxMap;
        ReallocateTriangulation(verIdxMap, triIdxMap);
    }

    bool ReallocateTriangulation(VerIdxMap & verIdxMap, TriIdxMap & triIdxMap)
    {
        if(!m_removedVertices.size() && !m_removedTriangles.size()) return false;

        verIdxMap.clear();
        triIdxMap.clear();
        verIdxMap.insert(std::make_pair(noVertex, noVertex));
        triIdxMap.insert(std::make_pair(noNeighbor, noNeighbor));

        //points
        std::unordered_set<index_t> removedPoints;
        std::unordered_map<index_t, index_t> ptIdxMap;
        for(VerIdx iv : m_removedVertices)
            removedPoints.insert(m_triangulation.vertices[iv].index);

        for(index_t ip = 0, ipNew = 0; ip < m_triangulation.points.size(); ++ip){
            if(removedPoints.count(ip)) continue;
            ptIdxMap.insert(std::make_pair(ip, ipNew));
            m_triangulation.points[ipNew] = m_triangulation.points[ip];
            ipNew++;
        }
        m_triangulation.points.erase(m_triangulation.points.end() - removedPoints.size(), m_triangulation.points.end());

        //vertices
        for(VerIdx iv = 0, ivNew = 0; iv < m_triangulation.vertices.size(); ++iv){
            if(m_removedVertices.count(iv)) continue;
            verIdxMap.insert(std::make_pair(iv, ivNew));
            m_triangulation.vertices[ivNew] = m_triangulation.vertices[iv];
            ivNew++;
        }
        m_triangulation.vertices.erase(m_triangulation.vertices.end() - m_removedVertices.size(), m_triangulation.vertices.end());
        for(auto & vertex : m_triangulation.vertices){
            vertex.index = ptIdxMap.at(vertex.index);
        }

        //triangles
        for(TriIdx it = 0, itNew = 0; it < m_triangulation.triangles.size(); ++it){
            if(m_removedTriangles.count(it)) continue;
            triIdxMap.insert(std::make_pair(it, itNew));
            m_triangulation.triangles[itNew] = m_triangulation.triangles[it];
            itNew++;
        }
        m_triangulation.triangles.erase(m_triangulation.triangles.end() - m_removedTriangles.size(), m_triangulation.triangles.end());

        for(auto & vertex : m_triangulation.vertices){
            TriIdxSet triangles;
            for(auto it : vertex.triangles)
                triangles.insert(triIdxMap.at(it));
            std::swap(vertex.triangles, triangles);
        }

        for(auto & triangle : m_triangulation.triangles){
            std::array<VerIdx, 3> vertices;
            for(auto i = 0; i < 3; ++i)
                vertices[i] = verIdxMap.at(triangle.vertices[i]);
            std::swap(triangle.vertices, vertices);

            std::array<TriIdx, 3> neighbors;
            for(auto i = 0; i < 3; ++i)
                neighbors[i] = triIdxMap.at(triangle.neighbors[i]);
            std::swap(triangle.neighbors, neighbors);
        }

        //fixed edges
        EdgeSet fixedEdges;
        for(auto & e : m_triangulation.fixedEdges){
            fixedEdges.insert(IndexEdge(verIdxMap.at(e.v1()), verIdxMap.at(e.v2())));
        }
        std::swap(m_triangulation.fixedEdges, fixedEdges);

        m_removedTriangles.clear();
        m_removedVertices.clear();

        RebuildVertexIndexTree();
        return true;
    }

    /* Insert point into triangle: split into 3 triangles:
    *  - create 2 new triangles
    *  - re-use old triangle for the 3rd
    *                      v3
    *                    / | \
    *                   /  |  \ <-- original triangle (t)
    *                  /   |   \
    *          e3, n3 /    |    \ e2, n2
    *                /newT2|newT1\
    *               /      v      \
    *              /    __/ \__    \
    *             /  __/       \__  \
    *            / _/      t'     \_ \
    *          v1 ___________________ v2
    *                     e1, n1
    */
    VerIdx InsertPointInTriangle(const Point & pos, TriIdx it0, std::stack<TriIdx> & out)
    {
        std::array<VerIdx, 3> vv = m_triangulation.triangles[it0].vertices;
        std::array<TriIdx, 3> nn = m_triangulation.triangles[it0].neighbors;

        VerIdx iv1 = vv[0], iv2 = vv[1], iv3 = vv[2];
        TriIdx in1 = nn[0], in2 = nn[1], in3 = nn[2];

        TriIdx it1 = AddOneTriangle();
        TriIdx it2 = AddOneTriangle();
        VerIdx iv  = AddOneVertex(pos, {it0, it1, it2});

        m_triangulation.triangles[it1].vertices = std::array<VerIdx, 3>{iv2, iv3, iv};
        m_triangulation.triangles[it1].neighbors = std::array<TriIdx, 3>{in2, it2, it0};

        m_triangulation.triangles[it2].vertices = std::array<VerIdx, 3>{iv3, iv1, iv};
        m_triangulation.triangles[it2].neighbors = std::array<TriIdx, 3>{in3, it0, it1};

        m_triangulation.triangles[it0].vertices = std::array<VerIdx, 3>{iv1, iv2, iv};
        m_triangulation.triangles[it0].neighbors = std::array<TriIdx, 3>{in1, it1, it2};

        m_triangulation.AddAdjacentTriangle(iv1, it2);
        m_triangulation.AddAdjacentTriangle(iv2, it1);
        m_triangulation.RemoveAdjacentTriangle(iv3, it0);
        m_triangulation.AddAdjacentTriangle(iv3, it1);
        m_triangulation.AddAdjacentTriangle(iv3, it2);

        m_triangulation.ChangeNeighbor(in2, it0, it1);
        m_triangulation.ChangeNeighbor(in3, it0, it2);

        out.push(it0);
        out.push(it1);
        out.push(it2);
        return iv;
    }

    /* Inserting a point on the edge between two triangles
    *    T1 (top)        v1
    *                   /|\
    *              n1 /  |  \ n4
    *               /    |    \
    *             /  T1' | Tnew1\
    *           v2-------v-------v4
    *             \ Tnew2| T2'  /
    *               \    |    /
    *              n2 \  |  / n3
    *                   \|/
    *   T2 (bottom)      v3
    */
    VerIdx InsertPointOnSharedEdge(const Point & pos, TriIdx it1, TriIdx it2, std::stack<TriIdx> & out)
    {
        auto & t1 = m_triangulation.triangles[it1];
        auto & t2 = m_triangulation.triangles[it2];
        index_t i  = t1.TnOpiVe(it2);
        VerIdx iv1 = t1.vertices[i];
        VerIdx iv2 = t1.vertices[IndexTriangle::ccw(i)];
        TriIdx in1 = t1.neighbors[i];
        TriIdx in4 = t1.neighbors[IndexTriangle::cw(i)];
        index_t j  = t2.TnOpiVe(it1);
        VerIdx iv3 = t2.vertices[j];
        VerIdx iv4 = t2.vertices[IndexTriangle::ccw(j)];
        TriIdx in3 = t2.neighbors[j];
        TriIdx in2 = t2.neighbors[IndexTriangle::cw(j)];

        TriIdx itNew1 = AddOneTriangle();
        TriIdx itNew2 = AddOneTriangle();
        VerIdx iv = AddOneVertex(pos, {it1, itNew2, it2, itNew1});

        m_triangulation.triangles[it1].vertices = std::array<VerIdx, 3>{iv1, iv2, iv};
        m_triangulation.triangles[it1].neighbors = std::array<TriIdx, 3>{in1, itNew2, itNew1};

        m_triangulation.triangles[it2].vertices = std::array<VerIdx, 3>{iv3, iv4, iv};
        m_triangulation.triangles[it2].neighbors = std::array<TriIdx, 3>{in3, itNew1, itNew2};

        m_triangulation.triangles[itNew1].vertices = std::array<VerIdx, 3>{iv1, iv, iv4};
        m_triangulation.triangles[itNew1].neighbors = std::array<TriIdx, 3>{it1, it2, in4};

        m_triangulation.triangles[itNew2].vertices = std::array<VerIdx, 3>{iv3, iv, iv2};
        m_triangulation.triangles[itNew2].neighbors = std::array<TriIdx, 3>{it2, it1, in2};

        m_triangulation.ChangeNeighbor(in4, it1, itNew1);
        m_triangulation.ChangeNeighbor(in2, it2, itNew2);
        m_triangulation.AddAdjacentTriangle(iv1, itNew1);
        m_triangulation.AddAdjacentTriangle(iv3, itNew2);
        m_triangulation.RemoveAdjacentTriangle(iv2, it2);
        m_triangulation.AddAdjacentTriangle(iv2, itNew2);
        m_triangulation.RemoveAdjacentTriangle(iv4, it1);
        m_triangulation.AddAdjacentTriangle(iv4, itNew1);

        out.push(it1);
        out.push(itNew2);
        out.push(it2);
        out.push(itNew1);
        return iv;
    }

    /* Inserting a point on the edge on boundary triangle
    *    T1 (top)        v1
    *                   /|\
    *              n1 /  |  \ n3
    *               /    |    \
    *             /  T'  | Tnew \
    *           v2-------v-------v3
    *                    n2
    */
    VerIdx InsertPointOnBoundaryEdge(const Point & pos, TriIdx it, const IndexEdge & e, std::stack<TriIdx> & out)
    {
        TriIdx itNew = AddOneTriangle();
        VerIdx iv = AddOneVertex(pos, {it, itNew});

        auto & t = m_triangulation.triangles[it];
        index_t ie = t.iEg(e);
        index_t i  = IndexTriangle::iEgOpiVe(ie);
        VerIdx iv1 = t.vertices[i];
        VerIdx iv2 = t.vertices[IndexTriangle::ccw(i)];
        VerIdx iv3 = t.vertices[IndexTriangle::cw(i)];
        TriIdx in1 = t.neighbors[i];
        TriIdx in2 = t.neighbors[IndexTriangle::ccw(i)];
        TriIdx in3 = t.neighbors[IndexTriangle::cw(i)];
        assert(in2 == noNeighbor);

        m_triangulation.triangles[it].vertices = std::array<VerIdx, 3>{iv1, iv2, iv};
        m_triangulation.triangles[it].neighbors = std::array<TriIdx, 3>{in1, in2, itNew};
        m_triangulation.triangles[itNew].vertices = std::array<VerIdx, 3>{iv1, iv, iv3};
        m_triangulation.triangles[itNew].neighbors = std::array<TriIdx, 3>{it, in2, in3};

        m_triangulation.ChangeNeighbor(in3, it, itNew);
        m_triangulation.AddAdjacentTriangle(iv1, itNew);
        m_triangulation.RemoveAdjacentTriangle(iv3, it);
        m_triangulation.AddAdjacentTriangle(iv3, itNew);

        out.push(it);
        out.push(itNew);
        return iv;
    }

    void Delaunay(std::stack<TriIdx> & triangles, VerIdx iv)
    {
        auto fixed = [this](TriIdx it, VerIdx iv)
        {
            auto e = m_triangulation.triangles[it].VeOpEg(iv);
            return m_triangulation.fixedEdges.count(e);
        };

        while(!triangles.empty()){
            TriIdx it = triangles.top();
            triangles.pop();

            const auto & t = m_triangulation.triangles[it];
            TriIdx itOp = t.VeOpTn(iv);
            if(itOp == noNeighbor) continue;
            if(fixed(it, iv)) continue;
            if(!Utility::isLocallyDelaunay(m_triangulation, it, itOp)){
                m_triangulation.FlipEdge(it, itOp);
                triangles.push(it);
                triangles.push(itOp);
            }
        }
    }

    bool TryDecayOneEdge(const IndexEdge & e)
    {
        VerIdx iv1  = e.v1();
        VerIdx iv2  = e.v2();
        const auto & p1 = m_triangulation.VerIdxPoint(iv1);
        const auto & p2 = m_triangulation.VerIdxPoint(iv2);
        Point p = (p1 + p2) * 0.5;
        auto modifiable = [&, this](const TriIdx it, const Point & p)
        {
            if(noNeighbor == it) return true;
            const auto & t = m_triangulation.triangles[it];

            VerIdx ivOp = t.EgOpVe(e);
            TriIdx itn1 = t.Tn(ivOp, iv1);
            TriIdx itn2 = t.Tn(ivOp, iv2);
            if(noNeighbor != itn1){
                const auto & tn1 = m_triangulation.triangles[itn1];
                auto [iu, iw] = tn1.VeOpVes(iv1);
                const auto & u = m_triangulation.VerIdxPoint(iu);
                const auto & w = m_triangulation.VerIdxPoint(iw);
                if(!Triangle::isCCW(p, w, u)) return false;
            }

            if(noNeighbor != itn2){
                const auto & tn2 = m_triangulation.triangles[itn2];
                auto [iu, iw] = tn2.VeOpVes(iv2);
                const auto & u = m_triangulation.VerIdxPoint(iu);
                const auto & w = m_triangulation.VerIdxPoint(iw);
                if(!Triangle::isCCW(p, w, u)) return false;
            }
            return true;
        };
        auto [it1, it2] = m_triangulation.GetTriangles(e);
        if(!modifiable(it1, p)) return false;
        if(!modifiable(it2, p)) return false;

        DecayToVertex(e);
        return true;
    }

    void DecayToVertex(const IndexEdge & e)
    {
        VerIdx iv1  = e.v1();
        VerIdx iv2  = e.v2();
        const auto & p1 = m_triangulation.VerIdxPoint(iv1);
        const auto & p2 = m_triangulation.VerIdxPoint(iv2);
        Point p = (p1 + p2) * 0.5;
        m_triangulation.points[m_triangulation.vertices[iv1].index] = p;
        auto removeOneTriangle = [&, this](TriIdx it) mutable
        {
            if(noNeighbor == it) return;
            const auto & t = m_triangulation.triangles[it];

            VerIdx ivOp = t.EgOpVe(e);
            TriIdx itn1 = t.Tn(ivOp, iv1);
            TriIdx itn2 = t.Tn(ivOp, iv2);

            RemoveOneTriangle(it);
            m_triangulation.ChangeNeighbor(itn1, iv1, ivOp, itn2);
            m_triangulation.ChangeNeighbor(itn2, iv2, ivOp, itn1);
            if(noNeighbor != itn1 && !Utility::isCCW(m_triangulation, itn1))
                m_triangulation.triangles[itn1].Reverse();
            if(noNeighbor != itn2 && !Utility::isCCW(m_triangulation, itn2))
                m_triangulation.triangles[itn2].Reverse();
        };
        auto [it1, it2] = m_triangulation.GetTriangles(e);
        removeOneTriangle(it1);
        removeOneTriangle(it2);
        for(auto it : m_triangulation.vertices[iv2].triangles){
            auto & t = m_triangulation.triangles[it];
            t.vertices[t.iVe(iv2)] = iv1;
            auto & v = m_triangulation.vertices[iv1];
            v.triangles.insert(it);
        }

        RemoveOneVertex(iv2);

        std::list<IndexEdge> toRemove, toInsert;
        for(const auto & edge : m_triangulation.fixedEdges){
            if(edge.hasVertex(iv2)){
                toRemove.push_back(edge);
                if(edge != e){
                    if(edge.v1() == iv2) toInsert.emplace_back(IndexEdge{iv1, edge.v2()});
                    else toInsert.emplace_back(IndexEdge{edge.v1(), iv1});
                }
            }
        }

        for(const auto & edge : toRemove)
            m_triangulation.fixedEdges.erase(edge);
        for(const auto & edge : toInsert)
            m_triangulation.fixedEdges.insert(edge);
    }

    void Clear()
    {
        m_triangulation.Clear();
        m_vertexIndexTree.Clear();
        m_removedTriangles.clear();
        m_removedVertices.clear();
    }
};

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


//==============================================================================
//         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_BENCH_MODULE "nt2 trigonometric toolbox - cosine/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// timing Test behavior of trigonometric components in simd mode
//////////////////////////////////////////////////////////////////////////////
#include <nt2/trigonometric/include/functions/cosine.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/trigonometric/include/functions/fast_cos.hpp>
#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/timing.hpp>
#include <nt2/include/constants/pi.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <cmath>
typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;

//////////////////////////////////////////////////////////////////////////////
// simd runtime benchmark for functor<cosine_> from trigonometric
//////////////////////////////////////////////////////////////////////////////
using nt2::tag::cosine_;
using nt2::tag::fast_cos_;

//////////////////////////////////////////////////////////////////////////////
// range macro
//////////////////////////////////////////////////////////////////////////////
#define RS(T,V1,V2) (T, (V1) ,(V2))
namespace nnnn01 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(fast_cos_,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nnnn1 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(fast_cos_,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace nnn01 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::clipped_pio4_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nnn1 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::clipped_pio4_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace nn01 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::clipped_small_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nn1 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::clipped_small_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace n01 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace n1 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace n011 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::direct_small_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace n11 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::direct_small_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace nn011 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nn11 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace nn0 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::direct_medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nn1 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::direct_medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace nn0111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::clipped_medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nn111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::clipped_medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}

namespace n11 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace n111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace n111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,T(-1.0e38),T(1.0e38))))
}
namespace n111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,nt2::Nan < T>(),nt2::Nan < T>())))
}

namespace n02 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace n2 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}

namespace n21 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}

namespace n211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}

namespace n211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,T(-1.0e15),T(1.0e15))))
}

namespace n211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,nt2::Nan < T>(),nt2::Nan < T>())))
}

namespace m01 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace m02 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}namespace m1 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace m2 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace m11 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace m21 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace m111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace m211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace m111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,T(-1.0e38),T(1.0e38))))
}
namespace m211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,T(-1.0e38),T(1.0e38))))
}
namespace m111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,nt2::Nan < T>(),nt2::Nan < T>())))
}
namespace m211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,nt2::Nan < T>(),nt2::Nan < T>())))
}

namespace n01 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace n02 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}namespace n1 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace n2 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace n11 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace n21 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace n111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace n211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace n111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,T(-1.0e38),T(1.0e38))))
}
namespace n211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,T(-1.0e38),T(1.0e38))))
}
namespace n111 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,nt2::Nan < T>(),nt2::Nan < T>())))
}
namespace n211 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  typedef boost::simd::native<T,ext_t> vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,nt2::Nan < T>(),nt2::Nan < T>())))
}

#undef RS


#include <iostream>
#include <boost/graph/undirected_graph.hpp>

using namespace std;
using namespace boost;

int main( ) {
  undirected_graph<> g;
  undirected_graph<>::vertex_descriptor u = g.add_vertex();
  undirected_graph<>::vertex_descriptor v = g.add_vertex();
  undirected_graph<>::vertex_descriptor w = g.add_vertex();
  undirected_graph<>::vertex_descriptor x = g.add_vertex();
  add_edge(u, v, g); add_edge(u, w, g); add_edge(u, x, g);

  cout << "Degree of u: " << degree(u, g);
  return 0;
}


#pragma once
#include <iostream>
#include <string>

#include "ros/ros.h"

#include <image_geometry/pinhole_camera_model.h>
#include <image_transport/image_transport.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/circular_buffer.hpp>

#include <std_srvs/SetBool.h>

#include "sub8_msgs/BMatrix.h"
#include "sub8_msgs/VisionRequest2D.h"

using namespace boost::accumulators;

class Sub8StartGateDetector
{
public:
  Sub8StartGateDetector();
  ~Sub8StartGateDetector();

  // 76 inches x 39.5 inches
  // 1.9304 x 1.0033
  const double START_GATE_WIDTH = 1.9304;  // Meters
  const double START_GATE_HEIGHT = 1.0033;
  const double START_GATE_SIZE_TOLERANCE = 10000;

  void imageCallback(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &info_msg);
  bool requestStartGatePosition2d(sub8_msgs::VisionRequest2D::Request &req, sub8_msgs::VisionRequest2D::Response &resp);
  bool requestStartGateEnable(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp);
  bool requestStartGateDistance(sub8_msgs::BMatrix::Request &req, sub8_msgs::BMatrix::Response &resp);
  void findGate(const sensor_msgs::ImageConstPtr &image_msg);

  double getGateDistance();

  ros::NodeHandle nh_;
  bool running_;

  image_transport::ImageTransport image_transport_;
  image_transport::CameraSubscriber image_sub_;

  image_geometry::PinholeCameraModel cam_model_;
  ros::Time image_time_;

  int rows_;
  int cols_;

  boost::circular_buffer<std::vector<std::vector<cv::Point2f>>> gate_line_buffer_;
  accumulator_set<int, features<tag::mean, tag::variance>> accX_;
  accumulator_set<int, features<tag::mean, tag::variance>> accY_;
  accumulator_set<int, features<tag::mean, tag::variance>> accSizeX_;
  accumulator_set<int, features<tag::mean, tag::variance>> accSizeY_;

  ros::ServiceServer service_enable_;
  ros::ServiceServer service_2d_;
  ros::ServiceServer service_distance_;

  image_transport::Publisher pubImage_;
};


/*=============================================================================

  LeastSquaresPackage: A software package for estimating the rotation and translation of rigid bodies.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "catch.hpp"
#include "lsqCatchMain.h"
#include "lsqLeastSquare3D.h"
#include "lsqComputeRotation.h"
#include <Eigen/Dense>
#include <iostream>

TEST_CASE( "Throw exception when we pop an element from empty array", "[add_remove_points]" ) {

  lsq::LeastSquare3D add_example;
  Eigen::Vector3d point_3D;

  SECTION( "Pop an element from the first vector" ) {
    REQUIRE_THROWS( point_3D = add_example.pop_point_first_vector() );
  }
  SECTION( "Pop an element from the second vector" ) {
    REQUIRE_THROWS( point_3D = add_example.pop_point_second_vector() );
  }

}

TEST_CASE( "Add 3D point to first and second set", "[add_remove_points]" ) {

  lsq::LeastSquare3D add_example;
  Eigen::Vector3d point_3D_in = {1,2,3};
  Eigen::Vector3d point_3D_out;

  SECTION( "Add point to first vector and pop it out" ) {
    add_example.add_point_first_vector(point_3D_in);
    point_3D_out = add_example.pop_point_first_vector();
    REQUIRE( point_3D_out == point_3D_in );
  }

  SECTION( "Add point to second vector and pop it out" ) {
    add_example.add_point_second_vector(point_3D_in);
    point_3D_out = add_example.pop_point_second_vector();
    REQUIRE( point_3D_out == point_3D_in );
  }

  SECTION( "Check if the two vectors have same size when they are empty" ) {
    REQUIRE( add_example.same_number_of_points() == true );
  }

  SECTION( "Check if the two vectors have same size when it is true" ) {
    add_example.add_point_first_vector(point_3D_in);
    add_example.add_point_second_vector(point_3D_in);
    REQUIRE( add_example.same_number_of_points() == true );
  }

  SECTION( "Check if the two vectors have same size when it is false" ) {
    add_example.add_point_first_vector(point_3D_in);
    add_example.add_point_second_vector(point_3D_in);
    add_example.add_point_second_vector(point_3D_in);
    REQUIRE( add_example.same_number_of_points() == false );
  }

}

TEST_CASE( "Compute centroid for the sets of points and update the set", "[centroids]" ) {

  lsq::LeastSquare3D centroid_example;
  Eigen::Vector3d point_3D_in;

  point_3D_in = {1.,0.,0.};
  centroid_example.add_point_first_vector(point_3D_in);
  point_3D_in = {0.,1.,0.};
  centroid_example.add_point_first_vector(point_3D_in);
  point_3D_in = {0.,0.,1.};
  centroid_example.add_point_first_vector(point_3D_in);

  Eigen::Vector3d expected_centroid = {0.,0.,0.};
  Eigen::Vector3d obtained_centroid;

  SECTION( "Get first centroid without having computed it" ) {
    obtained_centroid = centroid_example.get_centroid_first_vector();
    REQUIRE( expected_centroid == obtained_centroid );
  }

  SECTION( "Get second centroid without having computed it" ) {
    obtained_centroid = centroid_example.get_centroid_second_vector();
    REQUIRE( expected_centroid == obtained_centroid );
  }

  expected_centroid = {1./3., 1./3., 1./3.};

  SECTION( "Compute first centroid and check it" ) {
    centroid_example.centroid_first_vector();
    obtained_centroid = centroid_example.get_centroid_first_vector();
    REQUIRE( expected_centroid == obtained_centroid );
  }

  SECTION( "Compute again the first centroid and check that it does not change" ) {
    centroid_example.centroid_first_vector();
    obtained_centroid = centroid_example.get_centroid_first_vector();
    REQUIRE( expected_centroid == obtained_centroid );
  }

  SECTION( "Compute second centroid and get an error (the second vector is empty)" ) {
    REQUIRE_THROWS( centroid_example.centroid_second_vector() );
  }

  Eigen::Vector3d expected_point;
  Eigen::Vector3d obtained_point;
  std::vector<Eigen::Vector3d> expected_updated_first_vector;

  expected_updated_first_vector.push_back( Eigen::Vector3d( {0.-1./3., 0.-1./3., 1.-1./3.} ) );
  expected_updated_first_vector.push_back( Eigen::Vector3d( {0.-1./3., 1.-1./3., 0.-1./3.} ) );
  expected_updated_first_vector.push_back( Eigen::Vector3d( {1.-1./3., 0.-1./3., 0.-1./3.} ) );

  SECTION( "Update the first set of points around the centroid" ) {
    centroid_example.centroid_first_vector();
    centroid_example.update_first_points_around_centroid();
    for(int i = 0 ; i < 3 ; ++i) {
      obtained_point = centroid_example.pop_point_first_vector();
      expected_point = expected_updated_first_vector[i];
      REQUIRE( expected_point == obtained_point );
    }
  }

  SECTION( "Updating the second set of points around the centroid gives an error since it is empty" ) {
    REQUIRE_THROWS( centroid_example.update_second_points_around_centroid() );
  }

}

TEST_CASE( "Compute the H matrix for the problem", "[H_matrix]" ) {

  lsq::LeastSquare3D H_matrix_example;

  Eigen::Vector3d point_one;
  Eigen::Vector3d point_two;
  Eigen::Matrix3d obtained_matrix;
  Eigen::Matrix3d expected_matrix = Eigen::Matrix3d::Zero(3, 3);

  SECTION( "Return the H matrix without compute it." ) {
    obtained_matrix = H_matrix_example.get_H_matrix();
    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "No element in both sets of points." ) {
    REQUIRE_THROWS( H_matrix_example.compute_H_matrix() );
  }

  SECTION( "No element in first sets of points." ) {
    point_two = {1.,0.,0.};
    H_matrix_example.add_point_second_vector(point_two);
    REQUIRE_THROWS( H_matrix_example.compute_H_matrix() );
  }

  SECTION( "No element in second sets of points." ) {
    point_one = {1.,0.,0.};
    H_matrix_example.add_point_first_vector(point_one);
    REQUIRE_THROWS( H_matrix_example.compute_H_matrix() );
  }

  SECTION( "One element in each set of points: xx case" ) {
    point_one = {1.,0.,0.};
    point_two = {1.,0.,0.};
    expected_matrix(0,0) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "One element in each set of points: xy case" ) {
    point_one = {1.,0.,0.};
    point_two = {0.,1.,0.};
    expected_matrix(0,1) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "One element in each set of points: xz case" ) {
    point_one = {1.,0.,0.};
    point_two = {0.,0.,1.};
    expected_matrix(0,2) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "One element in each set of points: yx case" ) {
    point_one = {0.,1.,0.};
    point_two = {1.,0.,0.};
    expected_matrix(1,0) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "One element in each set of points: yy case" ) {
    point_one = {0.,1.,0.};
    point_two = {0.,1.,0.};
    expected_matrix(1,1) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "One element in each set of points: yz case" ) {
    point_one = {0.,1.,0.};
    point_two = {0.,0.,1.};
    expected_matrix(1,2) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "One element in each set of points: zx case" ) {
    point_one = {0.,0.,1.};
    point_two = {1.,0.,0.};
    expected_matrix(2,0) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "One element in each set of points: zy case" ) {
    point_one = {0.,0.,1.};
    point_two = {0.,1.,0.};
    expected_matrix(2,1) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "One element in each set of points: zz case" ) {
    point_one = {0.,0.,1.};
    point_two = {0.,0.,1.};
    expected_matrix(2,2) = 1;

    H_matrix_example.add_point_first_vector(point_one);
    H_matrix_example.add_point_second_vector(point_two);
    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "Two element in first set, one in the second." ) {
    point_one = {1.,0.,0.};
    H_matrix_example.add_point_first_vector(point_one);
    point_two = {1.,0.,0.};
    H_matrix_example.add_point_second_vector(point_two);
    point_one = {0.,1.,0.};
    H_matrix_example.add_point_first_vector(point_one);

    expected_matrix(0,0) = 1;

    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "Two element in first set, two in the second." ) {
    point_one = {1.,0.,0.};
    H_matrix_example.add_point_first_vector(point_one);
    point_two = {1.,0.,0.};
    H_matrix_example.add_point_second_vector(point_two);

    expected_matrix(0,0) = 1;

    point_one = {0.,1.,0.};
    H_matrix_example.add_point_first_vector(point_one);
    point_two = {0.,0.,1.};
    H_matrix_example.add_point_second_vector(point_two);

    expected_matrix(1,2) = 1;

    H_matrix_example.compute_H_matrix();
    obtained_matrix = H_matrix_example.get_H_matrix();

    REQUIRE( expected_matrix == obtained_matrix );
  }

}

TEST_CASE( "Set the algorithm we use for computing the rotation", "[set_strategy]" ) {

  lsq::LeastSquare3D strategy_example;

  std::unique_ptr<lsq::ComputeRotation> algorithm( new lsq::SVDMethod() );

  SECTION( "Check that the pointer algorithm is not empty." ) {
    if( !algorithm ) {
      REQUIRE(false);
    }
    REQUIRE(true);
  }

  SECTION( "Check that the pointer to the selected algorithm in the class is empty." ) {
    if( strategy_example.is_rotation_strategy() ) {
      REQUIRE(false);
    }
    REQUIRE(true);
  }

  strategy_example.set_rotation_strategy( std::move(algorithm) );

  SECTION( "Check that the pointer algorithm is empty." ) {
    if( algorithm ) {
      REQUIRE(false);
    }
    REQUIRE(true);
  }

  SECTION( "Check that the pointer to the selected algorithm in the class is not empty." ) {
    if( !strategy_example.is_rotation_strategy() ) {
      REQUIRE(false);
    }
    REQUIRE(true);
  }

}

TEST_CASE( "Compute the rotation matrix for the problem", "[R_matrix]" ) {

  lsq::LeastSquare3D rotation_example;

  Eigen::Matrix3d obtained_matrix;
  Eigen::Matrix3d expected_matrix = Eigen::Matrix3d::Zero(3, 3);

  SECTION( "Return the rotation without computing it." ) {
    obtained_matrix = rotation_example.get_rotation_matrix();
    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "Compute the rotation with the SVDMethod, without first computing H." ) {
    std::unique_ptr<lsq::ComputeRotation> algorithm( new lsq::SVDMethod() );
    rotation_example.set_rotation_strategy( std::move(algorithm) );

    rotation_example.compute_rotation_matrix();
    obtained_matrix = rotation_example.get_rotation_matrix();

    expected_matrix = Eigen::Matrix3d::Identity(3, 3);

    REQUIRE( expected_matrix == obtained_matrix );
  }

  SECTION( "Compute the rotation with the QuaternionMethod, without first computing H." ) {
    std::unique_ptr<lsq::ComputeRotation> newalgorithm( new lsq::QuaternionMethod() );
    rotation_example.set_rotation_strategy( std::move(newalgorithm) );

    rotation_example.compute_rotation_matrix();
    obtained_matrix = rotation_example.get_rotation_matrix();

    expected_matrix = Eigen::Matrix3d::Identity(3, 3);

    REQUIRE( expected_matrix == obtained_matrix );
  }

  Eigen::Vector3d point_3D;

  point_3D = {1.,0.,0.};
  rotation_example.add_point_first_vector(point_3D);
  point_3D = {0.,1.,0.};
  rotation_example.add_point_first_vector(point_3D);
  point_3D = {0.,0.,1.};
  rotation_example.add_point_first_vector(point_3D);

  point_3D = {1.,0.,0.};
  rotation_example.add_point_second_vector(point_3D);
  point_3D = {0.,0.,-1.};
  rotation_example.add_point_second_vector(point_3D);
  point_3D = {0.,1.,0.};
  rotation_example.add_point_second_vector(point_3D);

  SECTION( "Properly compute the rotation with the SVDMethod." ) {
    std::unique_ptr<lsq::ComputeRotation> algorithm( new lsq::SVDMethod() );
    rotation_example.set_rotation_strategy( std::move(algorithm) );

    rotation_example.centroid_first_vector();
    rotation_example.update_first_points_around_centroid();
    rotation_example.centroid_second_vector();
    rotation_example.update_second_points_around_centroid();
    rotation_example.compute_H_matrix();
    rotation_example.compute_rotation_matrix();

    obtained_matrix = rotation_example.get_rotation_matrix();

    expected_matrix(0,0) = 1.;
    expected_matrix(2,1) = -1.;
    expected_matrix(1,2) = 1.;

    REQUIRE( obtained_matrix.isApprox(expected_matrix,1.e-10) );
  }

  SECTION( "Properly compute the rotation with the QuaternionMethod." ) {
    std::unique_ptr<lsq::ComputeRotation> newalgorithm( new lsq::QuaternionMethod() );
    rotation_example.set_rotation_strategy( std::move(newalgorithm) );

    rotation_example.add_point_second_vector(point_3D);
    rotation_example.centroid_first_vector();
    rotation_example.update_first_points_around_centroid();
    rotation_example.centroid_second_vector();
    rotation_example.update_second_points_around_centroid();
    rotation_example.compute_H_matrix();
    rotation_example.compute_rotation_matrix();

    obtained_matrix = rotation_example.get_rotation_matrix();

    expected_matrix(0,0) = 1.;
    expected_matrix(2,1) = -1.;
    expected_matrix(1,2) = 1.;

    REQUIRE( obtained_matrix.isApprox(expected_matrix,1.e-10) );
  }

}

TEST_CASE( "Compute the translation vector for the problem", "[translation_vector]" ) {

  lsq::LeastSquare3D translation_example;

  Eigen::Vector3d point_one;
  Eigen::Vector3d point_two;

  Eigen::Vector3d obtained_vector;
  Eigen::Vector3d expected_vector = Eigen::Vector3d::Zero(3);

  SECTION( "Return the translation without computing it." ) {
    obtained_vector = translation_example.get_translation_vector();
    REQUIRE( expected_vector == obtained_vector );
  }

  SECTION( "Compute the translation vector without computing centroids and rotation." ) {
    translation_example.compute_translation_vector();
    obtained_vector = translation_example.get_translation_vector();
    REQUIRE( expected_vector == obtained_vector );
  }

  SECTION( "Compute the translation vector while computing the centroids (no rotation though)." ) {
    point_one = {1.,0.,0.};
    translation_example.add_point_first_vector(point_one);
    translation_example.centroid_first_vector();  /* This first centroid should be useless since rotation is zero matrix. */

    point_two = {0.,1.,0.};
    translation_example.add_point_second_vector(point_two);
    translation_example.centroid_second_vector();

    translation_example.compute_translation_vector();
    obtained_vector = translation_example.get_translation_vector();
    expected_vector = {0.,1.,0.};
    REQUIRE( expected_vector == obtained_vector );
  }

}


//***********************************************************************
// program to analyze bands and report some information on changing
//
//***********************************************************************
// version
// 2.0.0	29/09/2018	Rémi Saint-Amant	Creation from DisturbanceAnalyser

#define NOMINMAX
#include <SDKDDKVer.h>
#include <stdio.h>
#include <tchar.h>

//#include "stdafx.h"
#include <float.h>
#include <math.h>
#include <array>
#include <utility>
#include <iostream>
#include <bitset>
#include <boost/dynamic_bitset.hpp>

#include "Basic/UtilTime.h"
#include "Basic/UtilMath.h"
#include "Basic/OpenMP.h"
#include "Geomatic/GDALBasic.h"
#include "Cubistdefns.h"
#include "Geomatic/LandsatDataset.h"

#pragma warning(disable: 4275 4251)
#include "gdal_priv.h"

using namespace std;
using namespace WBSF;
using namespace WBSF::Landsat;

extern CaseNo		MaxCase;
extern String		FileStem;
extern RRuleSet	*	CubistModel;
extern int			MaxAtt;
extern DataRec		*Case;
extern Definition	*AttDef;
extern char		*SpecialStatus;
extern void Error(int ErrNo, String S1, String S2);
extern String		*AttName;
extern String		**AttValName;
extern DiscrValue	*MaxAttVal;

static const char* version = "2.0.0";

enum TFilePath { CubistImage_FILE_PATH, INPUT_FILE_PATH, OUTPUT_FILE_PATH, NB_FILE_PATH };
typedef vector<float> DTCodeVector;
typedef std::vector<float>  OutputData;

class CCubistImageOption : public CBaseOptions
{
public:
	CCubistImageOption()
	{
		m_nbPixel = 0;
		m_nbPixelDT = 0;
		m_scenesSize = 1;

		m_appDescription = "This software execute Cubist over an image";

		static const COptionDef OPTIONS[] =
		{
			//{ "-Debug",0,"",false,"Output debug information."},
			{ "CubistImageModel",0,"",false,"Cubist rules file path."},
			{ "srcfile",0,"",false, "Input image file path."},
			{ "dstfile",0,"",false, "Output image file path."}
		};

		for (int i = 0; i < sizeof(OPTIONS) / sizeof(COptionDef); i++)
			AddOption(OPTIONS[i]);

		static const CIOFileInfoDef IO_FILE_INFO[] =
		{
			{ "Input Model", "DTModel","","","","Decision tree model file generate by CubistImage."},
			{ "Input Image", "srcfile","","nbBands","same number iof band as the input CubistImage model"},
			{ "Output Image", "dstfile","1","","CubistImage model result"},
		};

		for (int i = 0; i < sizeof(IO_FILE_INFO) / sizeof(CIOFileInfoDef); i++)
			AddIOFileInfo(IO_FILE_INFO[i]);

	}

	virtual ERMsg ParseOption(int argc, char* argv[])
	{
		ERMsg msg = CBaseOptions::ParseOption(argc, argv);

		ASSERT(NB_FILE_PATH == 3);
		if (msg && m_filesPath.size() != NB_FILE_PATH)
		{
			msg.ajoute("ERROR: Invalid argument line. 3 files are needed: Cubist model, the intput image and the destination image.");
			msg.ajoute("Argument found: ");
			for (size_t i = 0; i < m_filesPath.size(); i++)
				msg.ajoute("   " + to_string(i + 1) + "- " + m_filesPath[i]);
		}

		return msg;
	}

	virtual ERMsg ProcessOption(int& i, int argc, char* argv[])
	{
		ERMsg msg;

		/*	if (IsEqual(argv[i], "-Trigger"))
			{
				string str = argv[++i];
				ReplaceString(str, "<", " < ");
				ReplaceString(str, ">", " > ");
				StringVector p(str, " ");
				if (p.size() == 3)
				{
					TIndices type = GetIndiceType(p[0]);
					string op = p[1];
					double threshold = atof(p[2].c_str());

					if (type != I_INVALID)
					{
						if (CIndices::IsValidOp(op))
							m_trigger.push_back(CIndices(type, op, threshold, 0));
						else
							msg.ajoute(op + " is an invalid operator for -Trigger option");
					}
					else
					{
						msg.ajoute(str + " is an invalid type for -Trigger option");
					}
				}
				else
				{
					msg.ajoute(str + " is an invalid type for -Trigger option");
				}

			}
			else if (IsEqual(argv[i], "-Despike"))
			{
				string str = argv[++i];
				TIndices type = GetIndiceType(str);
				double threshold = atof(argv[++i]);
				double min_trigger = atof(argv[++i]);

				if (type != I_INVALID)
				{
					m_despike.push_back(CIndices(type, "<", threshold, min_trigger));
				}
				else
				{
					msg.ajoute(str + " is an invalid type for -Despike option");
				}
			}
			else if (IsEqual(argv[i], "-nbDisturbances"))
			{
				m_nbDisturbances = atoi(argv[++i]);
			}
			else if (IsEqual(argv[i], "-FireSeverity"))
			{
				m_bFireSeverity = true;
			}
			else if (IsEqual(argv[i], "-ExportBands"))
			{
				m_bExportBands = true;
			}
			else if (IsEqual(argv[i], "-ExportTimeSeries"))
			{
				m_bExportTimeSeries = true;
			}
			else if (IsEqual(argv[i], "-Debug"))
			{
				m_bDebug = true;
			}
			else
			{*/
			//Look to see if it's a know base option
		msg = CBaseOptions::ProcessOption(i, argc, argv);
		//}

		return msg;
	}

	__int64 m_nbPixelDT;
	__int64 m_nbPixel;
};

//***********************************************************************
//
//	Main
//
//***********************************************************************

class CCubistImage
{
public:

	string GetDescription() { return  string("CubistImageImages version ") + version + " (" + _T(__DATE__) + ")\n"; }
	ERMsg Execute();

	ERMsg OpenAll(CGDALDatasetEx& inputDS, CGDALDatasetEx& maskDS, CGDALDatasetEx& outputDS);
	void ReadBlock(int xBlock, int yBlock, CBandsHolder& bandHolder);
	void ProcessBlock(int xBlock, int yBlock, const CBandsHolder& bandHolder, RRuleSet * model, OutputData& output);
	void WriteBlock(int xBlock, int yBlock, CGDALDatasetEx& outputDS, OutputData& output);
	void CloseAll(CGDALDatasetEx& inputDS, CGDALDatasetEx& maskDS, CGDALDatasetEx& outputDS);

	//void Evaluate(int x, int y, const vector<array<short, 3>>& DTCode, vector<vector<vector<short>>>& output);
	//void LoadData(const CBandsHolder& bandHolder1, const CBandsHolder& bandHolder2, vector< vector< CDADVector >>& data);
	//void LoadData(int x, int y, const CBandsHolder& bandHolder, CCubistImageTreeBlock& block);
	ERMsg LoadModel(string filePath, RRuleSet ** model);
	void GetDataRecGDAL(int xBlock, int yBlock, const CBandsHolder &bandHolder, Boolean Train, DataRec* pBl);

	CCubistImageOption m_options;

	//static int FindIndex(int start, const vector<short>& bandsvalue, int dir);
	//static void LoadModel(CCubistImageTree& DT, string filePath);
	//static short DTCode2ChangeCodeIndex(short DTCode);
	//static short DTCode2ChangeCode(short DTCode);

};

ERMsg CCubistImage::LoadModel(string filePath, RRuleSet ** model)
{
	ERMsg msg;

	CTimer timer(true);

	if (!m_options.m_bQuiet)
		_tprintf("Read model...\n");

	//  Read information on attribute names and values
	string fileName = m_options.m_filesPath[0] + ".names";
	FILE* F = fopen(fileName.c_str(), "r");

	if (!F)
		Error(0, (String)fileName.c_str(), "");

	GetNames(F);

	fclose(F);

	// Read the model file that defines the ruleset and sets values
	// for various global variables such as USEINSTANCES
	FileStem = (String)m_options.m_filesPath[0].c_str();
	*model = CubistModel = GetCommittee(".model");

	fileName = m_options.m_filesPath[0] + ".data";
	F = fopen(fileName.c_str(), "r");

	if (!F)
		Error(0, (String)fileName.c_str(), "");

	GetData(F, true, false);

	fclose(F);

	//  Prepare the file of instances and the kd-tree index
	if (!m_options.m_bQuiet)
		_tprintf("Init model...\n");
	InitialiseInstances(CubistModel);

	//  Reorder instances to improve caching
	try
	{
		if (!m_options.m_bQuiet)
			_tprintf("Copy instance...\n");

		CopyInstances();
		for (int i = 0; i <= MaxCase; i++)
		{
			Free(Case[i]);
		}
		Free(Case);
	}
	catch (...)
	{
		msg.ajoute("Exception throw when compiling model");
	}

	timer.Stop();

	if (!m_options.m_bQuiet)
		cout << "Read rules time = " << SecondToDHMS(timer.Elapsed()).c_str() << endl << endl;

	return msg;
}

ERMsg CCubistImage::OpenAll(CGDALDatasetEx& inputDS, CGDALDatasetEx& maskDS, CGDALDatasetEx& outputDS)
{
	ERMsg msg;

	if (!m_options.m_bQuiet)
		cout << endl << "Open input image..." << endl;

	msg = inputDS.OpenInputImage(m_options.m_filesPath[INPUT_FILE_PATH], m_options);

	if (msg)
	{

		inputDS.UpdateOption(m_options);

		if (!m_options.m_bQuiet)
		{
			CGeoExtents extents = inputDS.GetExtents();
			CProjectionPtr pPrj = inputDS.GetPrj();
			string prjName = pPrj ? pPrj->GetName() : "Unknown";

			cout << "    Size           = " << inputDS->GetRasterXSize() << " cols x " << inputDS->GetRasterYSize() << " rows x " << inputDS.GetRasterCount() << " bands" << endl;
			cout << "    Extents        = X:{" << ToString(extents.m_xMin) << ", " << ToString(extents.m_xMax) << "}  Y:{" << ToString(extents.m_yMin) << ", " << ToString(extents.m_yMax) << "}" << endl;
			cout << "    Projection     = " << prjName << endl;
			cout << "    NbBands        = " << inputDS.GetRasterCount() << endl;
		}
	}

	if (msg && !m_options.m_maskName.empty())
	{
		if (!m_options.m_bQuiet)
			cout << "Open mask image..." << endl;
		msg += maskDS.OpenInputImage(m_options.m_maskName);
	}

	if (msg && m_options.m_bCreateImage)
	{

		if (!m_options.m_bQuiet)
			cout << "Create output image..." << endl;

		CCubistImageOption option = m_options;
		option.m_nbBands = 1;

		string filePath = option.m_filesPath[OUTPUT_FILE_PATH];
		msg += outputDS.CreateImage(filePath, option);
	}

	return msg;
}

ERMsg CCubistImage::Execute()
{
	ERMsg msg;

	if (!m_options.m_bQuiet)
	{
		cout << "Output: " << m_options.m_filesPath[OUTPUT_FILE_PATH] << endl;
		cout << "From:   " << m_options.m_filesPath[INPUT_FILE_PATH] << endl;
		cout << "Using:  " << m_options.m_filesPath[CubistImage_FILE_PATH] << endl;

		if (!m_options.m_maskName.empty())
			cout << "Mask:   " << m_options.m_maskName << endl;
	}

	GDALAllRegister();

	//if (m_options.m_outputType != GDT_Unknown && m_options.m_outputType != GDT_Int16 && m_options.m_outputType != GDT_Int32)
		//msg.ajoute("Invalid -ot option. Only GDT_Int16 or GDT_Int32 are supported");

	if (!msg)
		return msg;

	RRuleSet	* model = NULL;
	msg += LoadModel(m_options.m_filesPath[CubistImage_FILE_PATH], &model);
	if (!msg)
		return msg;

	CGDALDatasetEx inputDS;
	CGDALDatasetEx maskDS;
	CGDALDatasetEx outputDS;

	msg = OpenAll(inputDS, maskDS, outputDS);

	if (msg && inputDS.GetRasterCount() != (MaxAtt - 1))
		msg.ajoute("The number of bands in the input image (" + to_string(inputDS.GetRasterCount()) + ") is not equal as the number of independants variables (" + to_string(MaxAtt - 1) + ") in the model.");

	if (msg)
	{
		CBandsHolderMT bandHolder(1, m_options.m_memoryLimit, m_options.m_IOCPU, m_options.m_BLOCK_THREADS);

		if (maskDS.IsOpen())
			bandHolder.SetMask(maskDS.GetSingleBandHolder(), m_options.m_maskDataUsed);

		msg += bandHolder.Load(inputDS, m_options.m_bQuiet, m_options.m_extents, m_options.m_period);
		if (!msg)
			return msg;

		if (!m_options.m_bQuiet && m_options.m_bCreateImage)
			cout << "Create output images x(" << m_options.m_extents.m_xSize << " C x " << m_options.m_extents.m_ySize << " R ) with " << m_options.m_CPU << " threads..." << endl;

		CGeoExtents extents = bandHolder.GetExtents();
		m_options.ResetBar((size_t)extents.m_xSize*extents.m_ySize);

		vector<pair<int, int>> XYindex = extents.GetBlockList();

		omp_set_nested(1);
#pragma omp parallel for schedule(static, 1) num_threads(m_options.m_BLOCK_THREADS) if (m_options.m_bMulti)
		for (int b = 0; b < (int)XYindex.size(); b++)
		{
			int thread = omp_get_thread_num();
			int xBlock = XYindex[b].first;
			int yBlock = XYindex[b].second;

			//data
			OutputData data;
			ReadBlock(xBlock, yBlock, bandHolder[thread]);
			ProcessBlock(xBlock, yBlock, bandHolder[thread], model, data);
			WriteBlock(xBlock, yBlock, outputDS, data);
		}//for all blocks

		//  Close decision tree and free allocated memory
		FreeCttee(model);
		FreeNamesData();

		//close inputs and outputs
		CloseAll(inputDS, maskDS, outputDS);

	}

	return msg;
}

void CCubistImage::ReadBlock(int xBlock, int yBlock, CBandsHolder& bandHolder)
{
#pragma omp critical(BlockIO)
	{

		m_options.m_timerRead.Start();
		bandHolder.LoadBlock(xBlock, yBlock);
		m_options.m_timerRead.Stop();
	}
}

//Get input image reference
void CCubistImage::ProcessBlock(int xBlock, int yBlock, const CBandsHolder& bandHolder, RRuleSet* model, OutputData& data)
{
	CGeoExtents extents = bandHolder.GetExtents();
	CGeoSize blockSize = extents.GetBlockSize(xBlock, yBlock);

	if (bandHolder.IsEmpty())
	{
#pragma omp atomic
		m_options.m_xx += (size_t)blockSize.m_x*blockSize.m_y;
		m_options.UpdateBar();

		return;
	}

	data.resize(blockSize.m_x*blockSize.m_y);

	//#pragma omp critical(ProcessBlock)
		//{
	m_options.m_timerProcess.Start();

	//allocate process memory

	DataRec* Block = NULL;
	Block = AllocZero(blockSize.m_x*blockSize.m_y, DataRec);
	float* Err = AllocZero(blockSize.m_x*blockSize.m_y, float);

	for (size_t xy = 0; xy < blockSize.m_x*blockSize.m_y; xy++)
		Block[xy] = AllocZero(MaxAtt + 3, AttValue);

	GetDataRecGDAL(xBlock, yBlock, bandHolder, false, Block);

	//process all x and y
#pragma omp parallel for num_threads( m_options.BLOCK_CPU()) if (m_options.m_bMulti)
	for (int xy = 0; xy < blockSize.m_y; xy++)
	{
		data[xy] = PredictValue(CubistModel, Block[xy], &Err[xy]);
	}//for x

#pragma omp atomic
	m_options.m_xx += (size_t)blockSize.m_x*blockSize.m_y;

	m_options.UpdateBar();

	Free(Block);					Block = Nil;
	Free(Err);						Err = Nil;

	m_options.m_timerProcess.Stop();
}

void CCubistImage::WriteBlock(int xBlock, int yBlock, CGDALDatasetEx& outputDS, OutputData& data)
{
	if (m_options.m_bCreateImage)
	{
#pragma omp critical(BlockIO)
		{

			m_options.m_timerWrite.Start();

			CGeoExtents extents = outputDS.GetExtents();
			CGeoSize blockSize = extents.GetBlockSize(xBlock, yBlock);
			CGeoRectIndex outputRect = extents.GetBlockRect(xBlock, yBlock);

			ASSERT(outputRect.Width() == blockSize.m_x);
			ASSERT(outputRect.Height() == blockSize.m_y);

			float noDataOut = (float)outputDS.GetNoData(0);
			ASSERT(data.size() == blockSize.m_x*blockSize.m_y);
			GDALRasterBand *pBand = outputDS.GetRasterBand(0);

			//for (size_t b = 0; b < outputDS.GetRasterCount(); b++)
			if (!data.empty())
			{
				pBand->RasterIO(GF_Write, outputRect.m_x, outputRect.m_y, outputRect.Width(), outputRect.Height(), &(data[0]), outputRect.Width(), outputRect.Height(), GDT_Float32, 0, 0);
			}//for all output bands
			else
			{
				pBand->RasterIO(GF_Write, outputRect.m_x, outputRect.m_y, outputRect.Width(), outputRect.Height(), &noDataOut, 1, 1, GDT_Float32, 0, 0);
			}

			m_options.m_timerWrite.Stop();
		}
	}//if create image

}

void CCubistImage::CloseAll(CGDALDatasetEx& inputDS, CGDALDatasetEx& maskDS, CGDALDatasetEx& outputDS)
{
	if (!m_options.m_bQuiet)
		_tprintf("\nClose all files...\n");

	inputDS.Close();
	maskDS.Close();

	//close output
	m_options.m_timerWrite.Start();
	outputDS.Close(m_options);
	m_options.m_timerWrite.Stop();

	/*if (!m_options.m_bQuiet)
	{
		double percent = m_options.m_nbPixel > 0 ? (double)m_options.m_nbPixelDT / m_options.m_nbPixel * 100 : 0;

		_tprintf("\n");
		_tprintf("Percentage of pixel treated by DecisionTree: %0.3lf %%\n\n", percent);
	}*/

	m_options.PrintTime();
}

void CCubistImage::GetDataRecGDAL(int xBlock, int yBlock, const CBandsHolder &bandHolder, Boolean Train, DataRec* pBl)
{
	ASSERT(bandHolder.GetRasterCount() == MaxAtt - 1);

	CGeoExtents extents = bandHolder.GetExtents();
	CGeoSize blockSize = extents.GetBlockSize(xBlock, yBlock);

	for (Attribute Att = 1; Att <= MaxAtt; Att++)
	{
		if (AttDef[Att])
		{
			for (int xy = 0; xy < blockSize.m_x*blockSize.m_y; xy++)
			{
				pBl[xy][Att] = EvaluateDef(AttDef[Att], pBl[xy]);
				//if (Continuous(Att))
				//{
				CheckValue(pBl[xy], Att);
				//}
			}

			continue;
		}

		if (Att == 1)
		{
			for (int xy = 0; xy < blockSize.m_x*blockSize.m_y; xy++)
			{
				//if (Continuous(Att))
				CVal(pBl[xy], Att) = UNKNOWN;
				//else
					//DVal(pBl[x], Att) = 0;
			}
			continue;
		}

		//  Get the attribute values
		CDataWindowPtr pWindow = bandHolder.GetWindow(Att - 1 - 1);
		for (size_t y = 0; y < blockSize.m_y; y++)
		{
			for (size_t x = 0; x < blockSize.m_x; x++)
			{
				double test = pWindow->at((int)x, (int)y);
				CVal(pBl[y*blockSize.m_x + x], Att) = pWindow->at( (int)x, (int)y);
				CheckValue(pBl[y*blockSize.m_x + x], Att);
			}
		}

		//for (size_t y = 0; y < blockSize.m_y; y++)
		//{
		//	for (size_t x = 0; x < blockSize.m_x; x++)
		//	{
		//		int xy = y * blockSize.m_x + x;
		//		if (Continuous(Att))
		//		{
		//			CVal(pBl[xy], Att) = pWindow->at((int)x, (int)y);
		//			CheckValue(pBl[xy], Att);
		//		}
		//		else
		//		{
		//			char	Name[1000] = { 0 };
		//			_gcvt_s(Name, 1000, pWindow->at((int)x, (int)y), 5);

		//			int Dv = Which(Name, AttValName[Att], 1, MaxAttVal[Att]);
		//			if (!Dv)
		//			{
		//				if (StatBit(Att, DISCRETE))
		//				{
		//					if (Train)
		//					{
		//						//  Add value to list
		//						if (MaxAttVal[Att] >= (long)AttValName[Att][0])
		//						{
		//							Error(TOOMANYVALS, AttName[Att], (char *)AttValName[Att][0] - 1);
		//							Dv = MaxAttVal[Att];
		//						}
		//						else
		//						{
		//							Dv = ++MaxAttVal[Att];
		//							AttValName[Att][Dv] = _strdup(Name);
		//							AttValName[Att][Dv + 1] = "<other>"; // no free
		//						}
		//					}
		//					else
		//					{
		//						//  Set value to "<other>"
		//						Dv = MaxAttVal[Att] + 1;
		//					}
		//				}
		//				else
		//				{
		//					Error(BADATTVAL, AttName[Att], Name);
		//				}
		//			}
		//			DVal(pBl[x], Att) = Dv;
		//		}
		//	}
		//}
	}

	//  Preserve original case number
	for (int xy = 0; xy < blockSize.m_x*blockSize.m_y; xy++)
		DVal(pBl[xy], 0) = MaxCase + 1;
}

int _tmain(int argc, _TCHAR* argv[])
{
	CTimer timer(true);

	CCubistImage regressionTree;
	ERMsg msg = regressionTree.m_options.ParseOption(argc, argv);

	if (!msg || !regressionTree.m_options.m_bQuiet)
		cout << regressionTree.GetDescription() << endl;

	if (msg)
		msg = regressionTree.Execute();

	if (!msg)
	{
		PrintMessage(msg);
		return -1;
	}

	timer.Stop();

	if (!regressionTree.m_options.m_bQuiet)
		cout << endl << "Total time = " << SecondToDHMS(timer.Elapsed()) << endl;

	int nRetCode = 0;
}


/**
 * @file Covariances.cpp
 * @brief Providing access to entries of the covariance.
 * @author Michael Kaess
 * @version $Id: Covariances.cpp 7858 2013-01-14 03:50:24Z kaess $
 *
 * Copyright (C) 2009-2013 Massachusetts Institute of Technology.
 * Michael Kaess, Hordur Johannsson, David Rosen,
 * Nicholas Carlevaris-Bianco and John. J. Leonard
 *
 * This file is part of iSAM.
 *
 * iSAM is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * iSAM is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with iSAM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <vector>
#include <list>
#include <Eigen/Dense>

#include "isam/covariance.h"
#include "isam/Slam.h"

#include "isam/Covariances.h"

using namespace std;
using namespace Eigen;

namespace isam {

Covariances::Covariances(Slam& slam) : _slam(NULL), _R(slam._R) {
  // update pointers into matrix before making a copy
  slam.update_starts();
  const list<Node*>& nodes = slam.get_nodes();
  for (list<Node*>::const_iterator it = nodes.begin(); it!=nodes.end(); it++) {
    Node* node = *it;
    int start = node->_start;
    int dim = node->dim();
    _index[node] = make_pair(start, dim);
  }
}

int Covariances::get_start(Node* node) const {
  if (_slam) {
    return node->_start;
  } else {
    return _index.find(node)->second.first;
  }
}

int Covariances::get_dim(Node* node) const {
  if (_slam) {
    return node->_dim;
  } else {
    return _index.find(node)->second.second;
  }
}

list<MatrixXd> Covariances::marginal(const node_lists_t& node_lists) const {
  const SparseSystem& R = (_slam==NULL) ? _R : _slam->_R;
  if (_slam) {
    _slam->update_starts();
  }

  vector < vector<int> > index_lists(node_lists.size());

  if (R.num_rows()>1) { // skip if _R not calculated yet (eg. before batch step)
    int n=0;
    const int* trans = R.a_to_r();
    for (node_lists_t::const_iterator it_list = node_lists.begin();
        it_list != node_lists.end();
        it_list++, n++) {
      // assemble list of indices
      vector<int> indices;
      for (list<Node*>::const_iterator it = it_list->begin(); it!=it_list->end(); it++) {
        Node* node = *it;
        int start = get_start(node);
        int dim = get_dim(node);
        for (int i=0; i<dim; i++) {
          index_lists[n].push_back(trans[start+i]);
        }
      }
    }
    return cov_marginal(R, _cache, index_lists);
  }
  list<MatrixXd> empty_list;
  return empty_list;
}

MatrixXd Covariances::marginal(const list<Node*>& nodes) const {
  node_lists_t node_lists;
  node_lists.push_back(nodes);
  return marginal(node_lists).front();
}

list<MatrixXd> Covariances::access(const node_pair_list_t& node_pair_list) const {
  const SparseSystem& R = (_slam==NULL) ? _R : _slam->_R;
  if (_slam) {
    _slam->update_starts();
  }

  if (R.num_rows()>1) { // skip if _R not calculated yet (eg. before batch step)

    // count how many entries in requested blocks
    int num = 0;
    for (node_pair_list_t::const_iterator it = node_pair_list.begin();
         it != node_pair_list.end(); it++) {
      num += get_dim(it->first) * get_dim(it->second);
    }

    // request individual covariance entries
    vector < pair<int, int> > index_list(num);
    const int* trans = R.a_to_r();
    int n = 0;
    for (node_pair_list_t::const_iterator it = node_pair_list.begin();
         it != node_pair_list.end(); it++) {
      int start_r = get_start(it->first);
      int start_c = get_start(it->second);
      int dim_r = get_dim(it->first);
      int dim_c = get_dim(it->second);
      for (int r=0; r<dim_r; r++) {
        for (int c=0; c<dim_c; c++) {
          index_list[n] = make_pair(trans[start_r + r], trans[start_c + c]);
          n++;
        }
      }
    }
    list<double> covs = cov_marginal(R, _cache, index_list);

    // assemble into block matrices
    list<MatrixXd> result;
    list<double>::iterator it_cov = covs.begin();
    for (node_pair_list_t::const_iterator it = node_pair_list.begin();
         it != node_pair_list.end(); it++) {
      int dim_r = get_dim(it->first);
      int dim_c = get_dim(it->second);
      MatrixXd matrix(dim_r, dim_c);
      for (int r=0; r<dim_r; r++) {
        for (int c=0; c<dim_c; c++) {
          matrix(r,c) = *it_cov;
          it_cov++;
        }
      }
      result.push_back(matrix);
    }
    return result;
  }
  list<MatrixXd> empty_list;
  return empty_list;
}

}