/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * DM20-0442
 */

/**
 * Implementations of all GraphBLAS functions optimized for the sequential
 * (CPU) backend.
 */

#pragma once

#include <functional>
#include <utility>
#include <vector>
#include <iterator>
#include <iostream>
#include <string>
#include <graphblas/algebra.hpp>
#include <graphblas/indices.hpp>
#include <boost/container/vector.hpp>

//****************************************************************************

namespace grb
{
    namespace backend
    {
        template <typename ScalarT>
        void print_vec(std::ostream &os, std::string label,
                       std::vector<std::tuple<IndexType, ScalarT> > vec)
        {
            os << label << " ";
            bool first = true;

            for (auto&& [idx, val] : vec)
            {
                os << (!first ? "," : " ") << idx << ":" << val;
                first = false;
            }
            os << std::endl;
        }

        //**********************************************************************

        template <typename DstMatrixT,
                  typename SrcMatrixT>
        void sparse_copy(DstMatrixT &dstMatrix,
                         SrcMatrixT const &srcMatrix)
        {
            using DstScalarType = typename DstMatrixT::ScalarType;
            std::vector<std::tuple<IndexType, DstScalarType> > dstRow;

            // Copying removes the contents of the other matrix so clear it first.
            dstMatrix.clear();

            IndexType nrows(dstMatrix.nrows());
            for (IndexType row_idx = 0; row_idx < nrows; ++row_idx)
            {
                auto&& srcRow = srcMatrix[row_idx];
                dstRow.clear();

                // We need to construct a new row with the appropriate cast!
                for (auto&& [idx, srcVal] : srcRow)
                {
                    dstRow.emplace_back(idx, static_cast<DstScalarType>(srcVal));
                }

                if (!dstRow.empty())
                    dstMatrix.setRow(row_idx, dstRow);
            }
        }

        // @todo: Make a sparse copy where they are the same type for efficiency

        //**********************************************************************
        /// Advance the provided iterator until the value evaluates to true or
        /// the end is reached.
        ///
        /// Iter is iterator to std::vector<std::tuple<grb::IndexType,T>>
        template <typename Iter>
        void increment_until_true(Iter &iter, Iter const &iter_end)
        {
            while ((iter != iter_end) && !(std::get<1>(*iter))) {
                ++iter;
            }
        }

        //**********************************************************************
        /// Increment the provided iterator while the index is less than the
        /// provided index
        ///
        /// @retval true   If targeted index is found (false could mean end is
        ///                reached first or targeted index was not present
        template <typename Iter>
        bool increment_while_below(Iter                 &iter,
                                   Iter           const &iter_end,
                                   grb::IndexType        target_idx)
        {
            while ((iter != iter_end) && (std::get<0>(*iter) < target_idx))
            {
                ++iter;
            }
            return ((iter != iter_end) && (std::get<0>(*iter) == target_idx));
        }

        //**********************************************************************
        /// Increment the provided iterator while the index is less than the
        /// provided index and append any elements to vec along the way
        template <typename Iter, typename V>
        void increment_and_add_while_below(Iter                 &iter,
                                           Iter          const  &iter_end,
                                           grb::IndexType        idx,
                                           V                    &vec)
        {
            while ((iter != iter_end) && (std::get<0>(*iter) < idx)) {
                vec.push_back(*iter);  // anything more efficient?
                ++iter;
            }
        }

        //**********************************************************************
        /// Perform the dot product of a row of a matrix with a sparse vector
        /// without pulling the indices out of the vector first.
        template <typename D1, typename D2, typename D3, typename SemiringT>
        bool dot2(D3                                                &ans,
                  std::vector<std::tuple<grb::IndexType,D1> > const &A_row,
                  std::vector<bool>                           const &u_bitmap,
                  std::vector<D2>                             const &u_vals,
                  grb::IndexType                                     u_nvals,
                  SemiringT                                          op)
        {
            bool value_set(false);
            ans = op.zero();

            if ((u_nvals == 0) || A_row.empty())
            {
                return value_set;
            }

            // find first stored value in u
            grb::IndexType u_idx(0);
            while (!u_bitmap[u_idx]) ++u_idx; // skip unstored elements

            // pull first value out of the row
            auto A_iter = A_row.begin();
            D1 a_val;
            grb::IndexType a_idx;

            // loop through both ordered sets to compute sparse dot prod
            while ((A_iter != A_row.end()) && (u_idx < u_vals.size()))
            {
                std::tie(a_idx, a_val) = *A_iter;
                if (u_idx == a_idx)
                {
                    if (value_set)
                    {
                        ans = op.add(ans, op.mult(a_val, u_vals[u_idx]));
                    }
                    else
                    {
                        ans = op.mult(a_val, u_vals[u_idx]);
                        value_set = true;
                    }

                    do { ++u_idx; } while ((u_idx < u_vals.size()) && !u_bitmap[u_idx]);
                    ++A_iter;
                }
                else if (u_idx > a_idx)
                {
                    ++A_iter;
                }
                else
                {
                    do { ++u_idx; } while ((u_idx < u_vals.size()) && !u_bitmap[u_idx]);
                }
            }

            return value_set;
        }

        //************************************************************************
        /// A dot product of two sparse vectors (vectors<tuple(index,value)>)
        template <typename D1, typename D2, typename D3, typename SemiringT, typename allocator_t>
        bool dot(D3                                                &ans,
                 boost::container::vector<std::tuple<grb::IndexType,D1>, allocator_t > const &vec1,
                 boost::container::vector<std::tuple<grb::IndexType,D2>, allocator_t > const &vec2,
                 SemiringT                                          op)
        {
            bool value_set(false);

            if (vec2.empty() || vec1.empty())
            {
                return value_set;
            }

            // point to first entries of the vectors
            auto v1_it = vec1.begin();
            auto v2_it = vec2.begin();

            // loop through both ordered sets to compute sparse dot prod
            while ((v1_it != vec1.end()) && (v2_it != vec2.end()))
            {
                if (std::get<0>(*v2_it) == std::get<0>(*v1_it))
                {
                    if (value_set)
                    {
                        ans = op.add(ans, op.mult(std::get<1>(*v1_it),
                                                  std::get<1>(*v2_it)));
                    }
                    else
                    {
                        ans = op.mult(std::get<1>(*v1_it),
                                      std::get<1>(*v2_it));
                        value_set = true;
                    }

                    ++v2_it;
                    ++v1_it;
                }
                else if (std::get<0>(*v2_it) > std::get<0>(*v1_it))
                {
                    ++v1_it;
                }
                else
                {
                    ++v2_it;
                }
            }

            return value_set;
        }

        //************************************************************************
        /// A dot product of two sparse vectors (vectors<tuple(index,value)>)
        template <typename D1, typename D2, typename D3, typename SemiringT>
        bool dot_rev(D3                                                &ans,
                     std::vector<std::tuple<grb::IndexType,D1> > const &vec2,
                     std::vector<std::tuple<grb::IndexType,D2> > const &vec1,
                     SemiringT                                          op)
        {
            bool value_set(false);

            if (vec2.empty() || vec1.empty())
            {
                return value_set;
            }

            // point to first entries of the vectors
            auto v1_it = vec1.begin();
            auto v2_it = vec2.begin();

            // loop through both ordered sets to compute sparse dot prod
            while ((v1_it != vec1.end()) && (v2_it != vec2.end()))
            {
                if (std::get<0>(*v2_it) == std::get<0>(*v1_it))
                {
                    if (value_set)
                    {
                        ans = op.add(ans, op.mult(std::get<1>(*v2_it),
                                                  std::get<1>(*v1_it)));
                    }
                    else
                    {
                        ans = op.mult(std::get<1>(*v2_it),
                                      std::get<1>(*v1_it));
                        value_set = true;
                    }

                    ++v2_it;
                    ++v1_it;
                }
                else if (std::get<0>(*v2_it) > std::get<0>(*v1_it))
                {
                    ++v1_it;
                }
                else
                {
                    ++v2_it;
                }
            }

            return value_set;
        }

        //************************************************************************
        /// A reduction of a sparse vector (vector<tuple(index,value)>) using a
        /// binary op or a monoid.
        template <typename D1, typename D3, typename BinaryOpT, typename allocator_t>
        bool reduction(
            D3                                                &ans,
            boost::container::vector<std::tuple<grb::IndexType,D1>, allocator_t > const &vec,
            BinaryOpT                                          op)
        {
            if (vec.empty())
            {
                return false;
            }

            using D3ScalarType =
                decltype(op(std::declval<D1>(), std::declval<D1>()));
            D3ScalarType tmp;

            if (vec.size() == 1)
            {
                tmp = static_cast<D3ScalarType>(std::get<1>(vec[0]));
            }
            else
            {
                /// @note Since op is associative and commutative left to right
                /// ordering is not strictly required.
                tmp = op(std::get<1>(vec[0]), std::get<1>(vec[1]));

                /// @todo replace with call to std::reduce?
                for (size_t idx = 2; idx < vec.size(); ++idx)
                {
                    tmp = op(tmp, std::get<1>(vec[idx]));
                }
            }

            ans = static_cast<D3>(tmp);
            return true;
        }

        //**********************************************************************
        /// Apply element-wise operation to union of 2 sparse vectors, store in 3rd.
        /// ans = op(vec1, vec2)
        ///
        /// @note ans must be a unique vector from either vec1 or vec2
        template <typename D1, typename D2, typename D3, typename BinaryOpT>
        void ewise_or(D1       &ans,
                      D2 const &vec1,
                      D3 const &vec2,
                      BinaryOpT op)
        {
            if (((void*)&ans == (void*)&vec1) || ((void*)&ans == (void*)&vec2))
            {
                throw PanicException(
                    "backend::ewise_or called with same vector for input and output.");
            }

            ans.clear();

            // point to first entries of the vectors
            auto v1_it = vec1.begin();
            auto v2_it = vec2.begin();
            using scType = std::tuple_element_t<1, typename D3::value_type>;

            // loop through both ordered sets to compute ewise_or
            while ((v1_it != vec1.end()) || (v2_it != vec2.end()))
            {
                if ((v1_it != vec1.end()) && (v2_it != vec2.end()))
                {
                    auto&& [v1_idx, v1_val] = *v1_it;
                    auto&& [v2_idx, v2_val] = *v2_it;
                    //std::tie(v1_idx, v1_val) = *v1_it;
                    //std::tie(v2_idx, v2_val) = *v2_it;

                    if (v2_idx == v1_idx)
                    {
                        ans.emplace_back(v1_idx,
                                         static_cast<scType>(op(v1_val, v2_val)));

                        ++v2_it;
                        ++v1_it;
                    }
                    else if (v2_idx > v1_idx)
                    {
                        ans.emplace_back(v1_idx, static_cast<scType>(v1_val));
                        ++v1_it;
                    }
                    else
                    {
                        ans.emplace_back(v2_idx, static_cast<scType>(v2_val));
                        ++v2_it;
                    }
                }
                else if (v1_it != vec1.end())
                {
                    ans.emplace_back(std::get<0>(*v1_it),
                                     static_cast<scType>(std::get<1>(*v1_it)));
                    ++v1_it;
                }
                else // v2_it != vec2.end())
                {
                    ans.emplace_back(std::get<0>(*v2_it),
                                     static_cast<scType>(std::get<1>(*v2_it)));
                    ++v2_it;
                }
            }
        }

        //********************************************************************
        // ALL SUPPORT
        // This is where we turn alls into the correct range

        template <typename SequenceT>
        bool searchIndices(SequenceT seq, IndexType n)
        {
            for (auto it : seq)
            {
                if (it == n) return true;
            }
            return false;
        }

        bool searchIndices(AllIndices seq, IndexType n)
        {
            return true;
        }

        //**********************************************************************
        /// Apply element-wise operation to union on sparse vectors.
        /// Indices in the stencil indicate where elements of vec2 should be
        /// used (whether there is a stored value or not); otherwise the value
        /// in vec1 should be taken.  Note: that it is assumed that if a value
        /// is stored in vec2 then the corresponding location is contained in
        /// stencil indices.
        ///
        /// Truth table (for each element, i, of the answer, where '-' means
        /// no stored value):
        ///
        ///  vec1_i   vec2   s_i   ans_i
        ///    -        -     -      -
        ///    -        -     x      -
        ///    -        x --> x    vec2_i
        ///    x        -     -    vec1_i
        ///    x        -     x      -    (take vec1_i which is no stored value)
        ///    x        x --> x    vec1_i
        ///
        /// \tparam D1
        /// \tparam D2
        /// \tparam D3
        /// \tparam SequenceT  Could be a out of order subset of indices
        ///
        /// \param ans   A row of the answer (Z or z), starts empty
        /// \param vec1  A row of the output container (C or w), indices increasing order
        /// \param vec2  A row of the T (or t) container, indices in increasing order
        /// \param stencil_indices  Assumed to not be in order
        ///
        template <typename D1, typename D2, typename D3, typename SequenceT>
        void ewise_or_stencil(
            std::vector<std::tuple<grb::IndexType,D3> >       &ans,
            std::vector<std::tuple<grb::IndexType,D1> > const &vec1,
            std::vector<std::tuple<grb::IndexType,D2> > const &vec2,
            SequenceT                                          stencil_indices)
        {
            ans.clear();

            //auto stencil_it = stencil_indices.begin();
            //if (v1_it)
            //while ((stencil_it != stencil_indices.end()) &&
            //       (*stencil_it < std::get<0>(*v1_it)))
            //{
            //    ++stencil_it;
            //}

            D1 v1_val;
            D2 v2_val;
            grb::IndexType v1_idx, v2_idx;

            // loop through both ordered sets to compute ewise_or
            auto v1_it = vec1.begin();
            auto v2_it = vec2.begin();
            while ((v1_it != vec1.end()) || (v2_it != vec2.end()))
            {
                if ((v1_it != vec1.end()) && (v2_it != vec2.end()))
                {
                    std::tie(v1_idx, v1_val) = *v1_it;
                    std::tie(v2_idx, v2_val) = *v2_it;

                    // If v1 and v2 both have stored values, it is assumed index
                    // is in stencil_indices so v2 should be stored
                    if (v2_idx == v1_idx)
                    {
                        ans.emplace_back(v2_idx, static_cast<D3>(v2_val));

                        ++v2_it;
                        ++v1_it;
                    }
                    // In this case v1 has a value and not v2.  We need to search
                    // stencil indices to see if index is present
                    else if (v1_idx < v2_idx) // advance v1 and annihilate
                    {
                        if (!searchIndices(stencil_indices, v1_idx))
                        {
                            ans.emplace_back(v1_idx, static_cast<D3>(v1_val));
                        }
                        ++v1_it;
                    }
                    else
                    {
                        //std::cerr << "Copying v2, Advancing v2_it" << std::endl;
                        ans.emplace_back(v2_idx, static_cast<D3>(v2_val));
                        ++v2_it;
                    }
                }
                else if (v1_it != vec1.end())  // vec2 exhausted
                {
                    std::tie(v1_idx, v1_val) = *v1_it;

                    if (!searchIndices(stencil_indices, v1_idx))
                    {
                        ans.emplace_back(v1_idx, static_cast<D3>(v1_val));
                    }
                    ++v1_it;
                }
                else // v2_it != vec2.end()) and vec1 exhausted
                {
                    std::tie(v2_idx, v2_val) = *v2_it;
                    ans.emplace_back(v2_idx, static_cast<D3>(v2_val));
                    ++v2_it;
                }
            }
        }

        //**********************************************************************
        template <typename ZScalarT,
                  typename WVectorT,
                  typename TScalarT,
                  typename SequenceT,
                  typename BinaryOpT >
        void ewise_or_stencil_opt_accum_1D(
            std::vector<std::tuple<grb::IndexType,ZScalarT>>       &z,
            WVectorT const                                         &w,
            std::vector<std::tuple<grb::IndexType,TScalarT>> const &t,
            SequenceT const                                        &indices,
            BinaryOpT                                               accum)
        {
            // If there is an accumulate operations, do nothing with the stencil
            ewise_or(z, w.getContents(), t, accum);
        }

        //**********************************************************************
        template <typename ZScalarT,
                  typename WVectorT,
                  typename TScalarT,
                  typename SequenceT>
        void ewise_or_stencil_opt_accum_1D(
            std::vector<std::tuple<grb::IndexType,ZScalarT>>       &z,
            WVectorT const                                         &w,
            std::vector<std::tuple<grb::IndexType,TScalarT>> const &t,
            SequenceT const                                        &indices,
            grb::NoAccumulate)
        {
            // If there is no accumulate we need to annihilate stored values
            // in w that fall in the stencil
            ewise_or_stencil(z, w.getContents(), t, indices);
        }

        //**********************************************************************
        template < typename ZMatrixT,
                   typename CMatrixT,
                   typename TMatrixT,
                   typename RowSequenceT,
                   typename ColSequenceT,
                   typename BinaryOpT >
        void ewise_or_stencil_opt_accum(ZMatrixT           &Z,
                                        CMatrixT     const &C,
                                        TMatrixT     const &T,
                                        RowSequenceT const &row_indices,
                                        ColSequenceT const &col_indices,
                                        BinaryOpT           accum)
        {
            // If there is an accumulate operation, do nothing with the stencil
            using ZScalarType = typename ZMatrixT::ScalarType;
            using ZRowType = std::vector<std::tuple<IndexType,ZScalarType> >;

            ZRowType tmp_row;
            IndexType nRows(Z.nrows());

            for (IndexType row_idx = 0; row_idx < nRows; ++row_idx)
            {
                ewise_or(tmp_row, C[row_idx], T[row_idx], accum);
                Z.setRow(row_idx, tmp_row);
            }
        }

        //**********************************************************************
        template < typename ZMatrixT,
                   typename CMatrixT,
                   typename TMatrixT,
                   typename RowSequenceT,
                   typename ColSequenceT>
        void ewise_or_stencil_opt_accum(ZMatrixT           &Z,
                                        CMatrixT     const &C,
                                        TMatrixT     const &T,
                                        RowSequenceT const &row_indices,
                                        ColSequenceT const &col_indices,
                                        grb::NoAccumulate)
        {
            // If there is no accumulate, we need to annihilate stored values
            // in C that fall in the stencil
            using ZScalarType = typename ZMatrixT::ScalarType;
            using ZRowType = std::vector<std::tuple<IndexType,ZScalarType> >;

            ZRowType tmp_row;
            IndexType nRows(Z.nrows());

            for (IndexType row_idx = 0; row_idx < nRows; ++row_idx)
            {
                if (searchIndices(row_indices, row_idx))
                {
                    // Row Stenciled. merge C, T, using col stencil\n";
                    ewise_or_stencil(tmp_row, C[row_idx], T[row_idx],
                                     col_indices);
                    Z.setRow(row_idx, tmp_row);
                }
                else
                {
                    // Row not stenciled.  Take row from C only
                    // There should be nothing in T for this row
                    Z.setRow(row_idx, C[row_idx]);
                }
            }
        }

        //**********************************************************************
        template < typename ZMatrixT,
                   typename CMatrixT,
                   typename TMatrixT,
                   typename BinaryOpT >
        void ewise_or_opt_accum(ZMatrixT         &Z,
                                CMatrixT const   &C,
                                TMatrixT const   &T,
                                BinaryOpT         accum)
        {
            using ZScalarType = typename ZMatrixT::ScalarType;
            using ZRowType = std::vector<std::tuple<IndexType,ZScalarType> >;

            ZRowType tmp_row;
            IndexType nRows(Z.nrows());

            for (IndexType row_idx = 0; row_idx < nRows; ++row_idx)
            {
                ewise_or(tmp_row, C[row_idx], T[row_idx], accum);
                Z.setRow(row_idx, tmp_row);
            }
        }

        //**********************************************************************

        // Specialized version that gets used when we don't have an accumulator
        template < typename ZMatrixT,
                   typename CMatrixT,
                   typename TMatrixT>
        void ewise_or_opt_accum(ZMatrixT               &Z,
                                CMatrixT          const &C,
                                TMatrixT          const &T,
                                grb::NoAccumulate )
        {
            sparse_copy(Z, T);
        }

        //**********************************************************************
        template <typename R1,
                  typename R2,
                  typename R3,
                  typename R4>
        void ewise_or_opt_accum_1D(
            R1       &z,
            R2 const &w,
            R3 const &t,
            R4 accum)
        {
            //z.clear();
            ewise_or(z, w.getContents(), t, accum);
        }

        //**********************************************************************
        // Specialized version that gets used when we don't have an accumulator
        template <typename ZScalarT,
                  typename WVectorT,
                  typename TScalarT>
        void ewise_or_opt_accum_1D(
            std::vector<std::tuple<grb::IndexType,ZScalarT>>       &z,
            WVectorT const                                         &w,
            std::vector<std::tuple<grb::IndexType,TScalarT>> const &t,
            grb::NoAccumulate )
        {
            //sparse_copy(z, t);
            for (auto tupl: t)
            {
                z.emplace_back(std::get<0>(tupl),
                               static_cast<ZScalarT>(std::get<1>(tupl)));
            }
        }

        //**********************************************************************
        template <typename ZScalarT,
                  typename WScalarT,
                  typename TScalarT,
                  typename BinaryOpT>
        void opt_accum_scalar(ZScalarT       &z,
                              WScalarT const &w,
                              TScalarT const &t,
                              BinaryOpT       accum)
        {
            z = static_cast<ZScalarT>(accum(w, t));
        }

        //**********************************************************************
        // Specialized version that gets used when we don't have an accumulator
        template <typename ZScalarT,
                  typename WScalarT,
                  typename TScalarT>
        void opt_accum_scalar(ZScalarT                &z,
                              WScalarT          const &w,
                              TScalarT          const &t,
                              grb::NoAccumulate        accum)
        {
            z = static_cast<ZScalarT>(t);
        }

        //************************************************************************
        /// Apply element-wise operation to intersection of sparse vectors.
        template <typename D1, typename D2, typename D3, typename BinaryOpT>
        void ewise_and(std::vector<std::tuple<grb::IndexType,D3> >       &ans,
                       std::vector<std::tuple<grb::IndexType,D1> > const &vec1,
                       std::vector<std::tuple<grb::IndexType,D2> > const &vec2,
                       BinaryOpT                                          op)
        {
            ans.clear();

            // todo: early exit if either input vector is empty?

            // point to first entries of the vectors
            auto v1_it = vec1.begin();
            auto v2_it = vec2.begin();

            // loop through both ordered sets to compute ewise_or
            while ((v1_it != vec1.end()) && (v2_it != vec2.end()))
            {
                if (std::get<0>(*v2_it) == std::get<0>(*v1_it))
                {
                    ans.emplace_back(std::get<0>(*v1_it),
                                     static_cast<D3>(op(std::get<1>(*v1_it),
                                                        std::get<1>(*v2_it))));

                    ++v2_it;
                    ++v1_it;
                }
                else if (std::get<0>(*v2_it) > std::get<0>(*v1_it))
                {
                    ++v1_it;
                }
                else
                {
                    ++v2_it;
                }
            }
        }

        //**********************************************************************
        //**********************************************************************
        /**
         * This merges the values of C and Z into the result vector based on the
         * values of the mask.
         *
         * If outp == REPLACE:
         *
         * \f[ L(C) = {(i,j,Zij):(i,j) \in (ind(Z) \cap ind(M))} \f]
         *
         * If outp == MERGE:
         *
         * \f[ L(C) = {(i,j,Zij):(i,j) \in (ind(C) \cap ind(\neg M))} \cup
         *            {(i,j,Zij):(i,j) \in (ind(Z) \cap ind(\neg M))} \f]
         *
         * @tparam CScalarT The scalar type of the C vector input AND result.
         * @tparam ZScalarT The scalar type of the Z vector input.
         * @tparam MScalarT The scalar type of the mask vector.
         *
         * @param result   Result vector.  We clear this first.
         * @param c_vec    The original c values that may be carried through.
         * @param z_vec    The new values to insert/overlay.
         * @param mask_vec The mask which specifies which values to use.
         * @param outp     If REPLACE, we should always clear the values specified
         *                 by the mask regardless if they are overlayed.
         */
        template < typename R1,
                   typename R2,
                   typename R3,
                   typename R4>
        void apply_with_mask(
            R1          &result,
            R2 const    &c_vec,
            R3 const    &z_vec,
            R4 const    &mask_vec,
            OutputControlEnum                                       outp)
        {
            using scType = std::tuple_element_t<1, typename R3::value_type>;
            auto c_it = c_vec.begin();
            auto z_it = z_vec.begin();
            auto mask_it = mask_vec.begin();

            result.clear();

            // Design: This approach is driven by the mask.
            while (mask_it != mask_vec.end())
            {
                // Make sure the mask is on a valid value
                increment_until_true(mask_it, mask_vec.end());

                // If we run out of mask, we are done!
                if (mask_it == mask_vec.end())
                {
                    break;
                }

                // Get the mask location
                auto mask_idx(std::get<0>(*mask_it));

                // If outp==REPLACE, then we don't consider original C values.
                // If outp==MERGE, then we want to keep C values outside the mask
                // Any values in C "outside" the mask should now be applied
                // So, we catch "c" up to the mask.  This is the intersection
                // of C and !M.

                if (outp == MERGE)
                {
                    // This is the part of (ind(C) \cap int(\not M)
                    increment_and_add_while_below(c_it, c_vec.end(), mask_idx,
                                                  result);
                }

                // "Catch up" the input to the mask.
                if (increment_while_below(z_it, z_vec.end(), mask_idx))
                {
                    // Now, at the mask point add the value from Z if we have one.
                    result.emplace_back(
                        mask_idx, static_cast<scType>(std::get<1>(*z_it)));
                }

                // If there is a C here, skip it
                if (c_it != c_vec.end())
                {
                    if (std::get<0>(*c_it) == mask_idx)
                        ++c_it;
                }

                // Now move to the next mask entry.
                ++mask_it;
            } // while mask_it != end

            // Now, we need to add the remaining C values beyond the mask
            if (outp == MERGE)
            {
                // This is the part of (ind(C) \cap int(\not M)
                while (c_it != c_vec.end())
                {
                    result.emplace_back(*c_it);
                    ++c_it;
                }
            }
        } // apply_with_mask

        //**********************************************************************
        // Matrix Mask Churn
        //**********************************************************************

        //**********************************************************************
        // WARNING: costly
        template <typename MatrixT>
        decltype(auto)
        get_structure_row(MatrixT const &mat, IndexType row_idx)
        {
            std::vector<std::tuple<IndexType, bool> > mask_tuples;
            mask_tuples.reserve(mat.ncols() - mat[row_idx].size());

            for (auto&& [ix, val] : mat[row_idx])
            {
                mask_tuples.emplace_back(ix, true);
            }

            return mask_tuples;
        }

        //**********************************************************************
        // WARNING: costly
        template <typename MatrixT>
        decltype(auto)
        get_complement_row(MatrixT const &mat, IndexType row_idx)
        {
            std::vector<std::tuple<IndexType, bool> > mask_tuples;
            auto &row_tuples = mat[row_idx];
            mask_tuples.reserve(mat.ncols() - row_tuples.size());
            auto it = row_tuples.begin();

            for (IndexType ix = 0; ix < mat.ncols(); ++ix)
            {
                if ((it == row_tuples.end()) || (ix < std::get<0>(*it)))
                {
                    mask_tuples.emplace_back(ix, true);
                }
                else
                {
                    if (static_cast<bool>(std::get<1>(*it)) == false)
                    {
                        mask_tuples.emplace_back(ix, true);
                    }
                    ++it;
                }
            }

            return mask_tuples;
        }

        //**********************************************************************
        // WARNING: costly
        template <typename MatrixT>
        decltype(auto)
        get_structural_complement_row(MatrixT const &mat, IndexType row_idx)
        {
            std::vector<std::tuple<IndexType, bool> > mask_tuples;
            auto &row_tuples = mat[row_idx];
            mask_tuples.reserve(mat.ncols() - row_tuples.size());
            auto it = row_tuples.begin();

            for (IndexType ix = 0; ix < mat.ncols(); ++ix)
            {
                if ((it == row_tuples.end()) || (ix < std::get<0>(*it)))
                {
                    mask_tuples.emplace_back(ix, true);
                }
                else
                {
                    ++it;
                }
            }

            return mask_tuples;
        }

        //**********************************************************************
        // Matrix version
        template < typename CMatrixT,
                   typename ZMatrixT,
                   typename MMatrixT>
        void write_with_opt_mask(CMatrixT           &C,
                                 ZMatrixT   const   &Z,
                                 MMatrixT   const   &Mask,
                                 OutputControlEnum   outp)
        {
            using CScalarType = typename CMatrixT::ScalarType;
            using CRowType = std::vector<std::tuple<IndexType, CScalarType> >;

            CRowType tmp_row;
            IndexType nRows(C.nrows());
            for (IndexType row_idx = 0; row_idx < nRows; ++row_idx)
            {
                apply_with_mask(tmp_row, C[row_idx], Z[row_idx],
                                Mask[row_idx], outp);

                // Now, set the new one.  Yes, we can optimize this later
                C.setRow(row_idx, tmp_row);
            }
        }

        //**********************************************************************
        // Matrix version specialized for complement mask
        template < typename CMatrixT,
                   typename ZMatrixT,
                   typename MMatrixT>
        void write_with_opt_mask(
            CMatrixT                                  &C,
            ZMatrixT                            const &Z,
            grb::MatrixComplementView<MMatrixT> const &Mask,
            OutputControlEnum                          outp)
        {
            using CScalarType = typename CMatrixT::ScalarType;
            using CRowType = std::vector<std::tuple<IndexType, CScalarType> >;

            CRowType tmp_row;
            IndexType nRows(C.nrows());
            for (IndexType row_idx = 0; row_idx < nRows; ++row_idx)
            {
                apply_with_mask(tmp_row, C[row_idx], Z[row_idx],
                                get_complement_row(Mask.m_mat, row_idx),
                                outp);

                // Now, set the new one.  Yes, we can optimize this later
                C.setRow(row_idx, tmp_row);
            }
        }

        //**********************************************************************
        // Matrix version specialized for structure mask
        template < typename CMatrixT,
                   typename ZMatrixT,
                   typename MMatrixT>
        void write_with_opt_mask(
            CMatrixT                                 &C,
            ZMatrixT                           const &Z,
            grb::MatrixStructureView<MMatrixT> const &Mask,
            OutputControlEnum                         outp)
        {
            using CScalarType = typename CMatrixT::ScalarType;
            using CRowType = std::vector<std::tuple<IndexType, CScalarType> >;

            CRowType tmp_row;
            IndexType nRows(C.nrows());
            for (IndexType row_idx = 0; row_idx < nRows; ++row_idx)
            {
                apply_with_mask(tmp_row, C[row_idx], Z[row_idx],
                                get_structure_row(Mask.m_mat, row_idx),
                                outp);

                // Now, set the new one.  Yes, we can optimize this later
                C.setRow(row_idx, tmp_row);
            }
        }

        //**********************************************************************
        // Matrix version specialized for structural complement mask
        template < typename CMatrixT,
                   typename ZMatrixT,
                   typename MMatrixT>
        void write_with_opt_mask(
            CMatrixT                                            &C,
            ZMatrixT                                      const &Z,
            grb::MatrixStructuralComplementView<MMatrixT> const &Mask,
            OutputControlEnum                                    outp)
        {
            using CScalarType = typename CMatrixT::ScalarType;
            using CRowType = std::vector<std::tuple<IndexType, CScalarType> >;

            CRowType tmp_row;
            IndexType nRows(C.nrows());
            for (IndexType row_idx = 0; row_idx < nRows; ++row_idx)
            {
                apply_with_mask(tmp_row, C[row_idx], Z[row_idx],
                                get_structural_complement_row(Mask.m_mat, row_idx),
                                outp);

                // Now, set the new one.  Yes, we can optimize this later
                C.setRow(row_idx, tmp_row);
            }
        }

        //**********************************************************************
        // Matrix version specialized for no mask
        template < typename CMatrixT,
                   typename ZMatrixT >
        void write_with_opt_mask(CMatrixT                   &C,
                                 ZMatrixT           const   &Z,
                                 grb::NoMask        const   &foo,
                                 OutputControlEnum           outp)
        {
            sparse_copy(C, Z);
        }

        //**********************************************************************
        // Vector Mask Churn
        //**********************************************************************

        //**********************************************************************
        // WARNING: costly
        template <typename VectorT>
        decltype(auto)
        get_structure_contents(VectorT const &vec)
        {
            std::vector<std::tuple<IndexType, bool> > mask_tuples;

            for (auto [ix, val] : vec.getContents())
            {
                mask_tuples.emplace_back(ix, true);
            }

            return mask_tuples;
        }

        //**********************************************************************
        // WARNING: costly
        template <typename VectorT>
        decltype(auto)
        get_complement_contents(VectorT const &vec)
        {
            std::vector<std::tuple<IndexType, bool> > mask_tuples;
            auto row_tuples(vec.getContents());
            auto it = row_tuples.begin();

            for (IndexType ix = 0; ix < vec.size(); ++ix)
            {
                if ((it == row_tuples.end()) || (ix < std::get<0>(*it)))
                {
                    mask_tuples.emplace_back(ix, true);
                }
                else
                {
                    if (static_cast<bool>(std::get<1>(*it)) == false)
                    {
                        mask_tuples.emplace_back(ix, true);
                    }
                    ++it;
                }
            }

            return mask_tuples;
        }

        //**********************************************************************
        // WARNING: costly
        template <typename VectorT>
        decltype(auto)
        get_structural_complement_contents(VectorT const &vec)
        {
            std::vector<std::tuple<IndexType, bool> > mask_tuples;
            auto row_tuples(vec.getContents());
            auto it = row_tuples.begin();

            for (IndexType ix = 0; ix < vec.size(); ++ix)
            {
                if ((it == row_tuples.end()) || (ix < std::get<0>(*it)))
                {
                    mask_tuples.emplace_back(ix, true);
                }
                else
                {
                    ++it;
                }
            }

            return mask_tuples;
        }

        //**********************************************************************
        // Vector version
        template <typename WVectorT,
                  typename ZScalarT,
                  typename MaskT>
        void write_with_opt_mask_1D(
            WVectorT                                           &w,
            std::vector<std::tuple<IndexType, ZScalarT>> const &z,
            MaskT const                                        &mask,
            OutputControlEnum                                   outp)
        {
            using WScalarType = typename WVectorT::ScalarType;
            std::vector<std::tuple<IndexType, WScalarType> > tmp_row;

            apply_with_mask(tmp_row, w.getContents(), z,
                            mask.getContents(),
                            outp);

            // Now, set the new one.  Yes, we can optimize this later
            w.setContents(tmp_row);
        }

        //**********************************************************************
        // Vector version specialized for complement mask
        template <typename WVectorT,
                  typename ZScalarT,
                  typename MaskT>
        void write_with_opt_mask_1D(
            WVectorT                                           &w,
            std::vector<std::tuple<IndexType, ZScalarT>> const &z,
            grb::VectorComplementView<MaskT>             const &mask,
            OutputControlEnum                                   outp)
        {
            using WScalarType = typename WVectorT::ScalarType;
            std::vector<std::tuple<IndexType, WScalarType> > tmp_row;

            apply_with_mask(tmp_row, w.getContents(), z,
                            get_complement_contents(mask.m_vec),
                            outp);

            // Now, set the new one.  Yes, we can optimize this later
            w.setContents(tmp_row);
        }

        //**********************************************************************
        // Vector version specialized for structure mask
        template <typename WVectorT,
                  typename ZScalarT,
                  typename MaskT>
        void write_with_opt_mask_1D(
            WVectorT                                           &w,
            std::vector<std::tuple<IndexType, ZScalarT>> const &z,
            grb::VectorStructureView<MaskT>              const &mask,
            OutputControlEnum                                   outp)
        {
            using WScalarType = typename WVectorT::ScalarType;
            std::vector<std::tuple<IndexType, WScalarType> > tmp_row;

            apply_with_mask(tmp_row, w.getContents(), z,
                            get_structure_contents(mask.m_vec),
                            outp);

            // Now, set the new one.  Yes, we can optimize this later
            w.setContents(tmp_row);
        }

        //**********************************************************************
        // Vector version specialized for structural complement mask
        template <typename WVectorT,
                  typename ZScalarT,
                  typename MaskT>
        void write_with_opt_mask_1D(
            WVectorT                                               &w,
            std::vector<std::tuple<IndexType, ZScalarT>>     const &z,
            grb::VectorStructuralComplementView<MaskT>       const &mask,
            OutputControlEnum                                       outp)
        {
            using WScalarType = typename WVectorT::ScalarType;
            std::vector<std::tuple<IndexType, WScalarType> > tmp_row;

            apply_with_mask(tmp_row, w.getContents(), z,
                            get_structural_complement_contents(mask.m_vec),
                            outp);

            // Now, set the new one.  Yes, we can optimize this later
            w.setContents(tmp_row);
        }

        //**********************************************************************
        // Vector version specialized for no mask
        template <typename WVectorT,
                  typename ZScalarT>
        void write_with_opt_mask_1D(
            WVectorT                                           &w,
            std::vector<std::tuple<IndexType, ZScalarT>> const &z,
            grb::NoMask                                  const &foo,
            OutputControlEnum                                   outp)
        {
            //sparse_copy(w, z);
            w.setContents(z);
        }

        //********************************************************************
        // Index-out-of-bounds is an execution error and a responsibility of
        // the backend.
        template <typename SequenceT>
        void check_index_array_content(SequenceT   const &array,
                                       IndexType          dim,
                                       std::string const &msg)
        {
            if (!IsAllSequence(array))
            {
                for (auto ix : array)
                {
                    if (ix >= dim)
                    {
                        throw IndexOutOfBoundsException(msg);
                    }
                }
            }
        }

        //********************************************************************
        // ALL SUPPORT
        // This is where we turns alls into the correct range

        template <typename SequenceT>
        SequenceT setupIndices(SequenceT seq, IndexType n)
        {
            return seq;
        }

        IndexSequenceRange setupIndices(AllIndices seq, IndexType n)
        {
            return IndexSequenceRange(0, n);
        }

        //********************************************************************
        // mxm helpers (may be of more general use).
        //********************************************************************

        // *******************************************************************
        // Return true if iterator points to location with target_index;
        // otherwise returns the at the insertion point for target_index
        // which could be it_end.
        template <typename TupleIteratorT>
        bool advance_and_check_tuple_iterator(
            TupleIteratorT       &it,
            TupleIteratorT const &it_end,
            IndexType             target_index)
        {
            GRB_LOG_FN_BEGIN("advance_and_check_tuple_iterator: tgt = "
                             << target_index);

            while ((it != it_end) && (std::get<0>(*it) < target_index))
            {
                ++it;
            }

            bool tmp = ((it != it_end) && (std::get<0>(*it) == target_index));
            GRB_LOG_FN_END("advance_and_check_tuple_iterator target_found = "
                           << tmp);
            return tmp;
        }

        // *******************************************************************
        // Only returns true if target index is found AND it evaluates to true
        /// @todo Need to add support for STRUCTURE_ONLY
        template <typename TupleIteratorT>
        bool advance_and_check_mask_iterator(
            TupleIteratorT       &it,
            TupleIteratorT const &it_end,
            bool                  structure_flag,
            IndexType             target_index)
        {
            GRB_LOG_FN_BEGIN("advance_and_check_mask_iterator: sflag = "
                             << structure_flag << ", tgt = " << target_index);

            bool tmp =
                (advance_and_check_tuple_iterator(it, it_end, target_index) &&
                 (structure_flag || static_cast<bool>(std::get<1>(*it))));

            GRB_LOG_FN_END("advance_and_check_mask_iterator: result = " << tmp);
            return tmp;
        }

        //**********************************************************************
        /// accumulate one sparse vector with another (applying op in intersection).
        ///  "xpey = x plus equals y"
        /// vec1 += vec2
        ///
        /// @note similarities with axpy()
        template <typename D1, typename D2, typename BinaryOpT>
        void xpey(
            std::vector<std::tuple<grb::IndexType,D1> >       &vec1,
            std::vector<std::tuple<grb::IndexType,D2> > const &vec2,
            BinaryOpT                                          op)
        {
            // point to first entries of the destination vector
            auto v1_it = vec1.begin();

            for (auto&& [v2_idx, v2_val] : vec2)
            {
                // scan forward through vec1 to find insert or merge point
                if (advance_and_check_tuple_iterator(v1_it, vec1.end(), v2_idx))
                {
                    // merge
                    std::get<1>(*v1_it) = op(std::get<1>(*v1_it), v2_val);
                    ++v1_it;
                }
                else
                {
                    // insert
                    v1_it = vec1.insert(
                        v1_it, std::make_tuple(v2_idx, static_cast<D1>(v2_val)));
                    ++v1_it;
                }
            }
        }

        // *******************************************************************
        /// perform the following operation on sparse vectors implemented as
        /// vector<tuple<Index, value>>
        ///
        /// c += a_ik*b[:]
        template<typename R1,
                 typename R2,
                 typename R3,
                 typename R4>
        void axpy(
            R1 &c,
            R2 semiring,
            R3 a,
            R4 const &b)
        {
            GRB_LOG_FN_BEGIN("axpy");
            auto c_it = c.begin();
            using scType = std::tuple_element_t<1, typename R1::value_type>;
            for (auto&& [j, b_j] : b)
            {
                GRB_LOG_VERBOSE("j = " << j);

                auto t_j(semiring.mult(a, b_j));
                GRB_LOG_VERBOSE("temp = " << t_j);

                // scan through C_row to find insert/merge point
                if (advance_and_check_tuple_iterator(c_it, c.end(), j))
                {
                    GRB_LOG_VERBOSE("Accumulating");
                    std::get<1>(*c_it) = semiring.add(std::get<1>(*c_it), t_j);
                    ++c_it;
                }
                else
                {
                    GRB_LOG_VERBOSE("Inserting");
                    c_it = c.insert(c_it,
                                    std::make_tuple(j, static_cast<scType>(t_j)));
                    ++c_it;
                }
            }
            GRB_LOG_FN_END("axpy");
        }

        // *******************************************************************
        /// perform the following operation on sparse vectors implemented as
        /// vector<tuple<Index, value>>
        ///
        /// c<[m[:]]> += a_ik*b[:]
        template<typename CScalarT,
                 typename MScalarT,
                 typename SemiringT,
                 typename AScalarT,
                 typename BScalarT>
        void masked_axpy(
            std::vector<std::tuple<IndexType, CScalarT>>       &c,
            std::vector<std::tuple<IndexType, MScalarT>> const &m,
            bool                                                structure_flag,
            bool                                                complement_flag,
            SemiringT                                           semiring,
            AScalarT                                            a,
            std::vector<std::tuple<IndexType, BScalarT>> const &b)
        {
            GRB_LOG_FN_BEGIN("masked_axpy");

            if (m.empty() && complement_flag)
            {
                axpy(c, semiring, a, b);
                return;
            }

            auto c_it = c.begin();
            auto m_it = m.begin();

            for (auto const &b_elt : b)
            {
                IndexType    j(std::get<0>(b_elt));
                GRB_LOG_VERBOSE("j = " << j);

                // scan through M[i] to see if mask allows write.
                if (advance_and_check_mask_iterator(
                        m_it, m.end(), structure_flag, j) == complement_flag)
                {
                    GRB_LOG_VERBOSE("Skipping j = " << j);
                    continue;
                }

                BScalarT  b_j(std::get<1>(b_elt));

                auto t_j(semiring.mult(a, b_j));
                GRB_LOG_VERBOSE("temp = " << t_j);

                // scan through C_row to find insert/merge point
                if (advance_and_check_tuple_iterator(c_it, c.end(), j))
                {
                    GRB_LOG_VERBOSE("Accumulating");
                    std::get<1>(*c_it) = semiring.add(std::get<1>(*c_it), t_j);
                    ++c_it;
                }
                else
                {
                    GRB_LOG_VERBOSE("Inserting");
                    c_it = c.insert(c_it,
                                    std::make_tuple(j, static_cast<CScalarT>(t_j)));
                    ++c_it;
                }
            }
            GRB_LOG_FN_END("masked_axpy");
        }

        // *******************************************************************
        /// Perform the following operation on sparse vectors implemented as
        /// vector<tuple<Index, value>> (t assumed to be masked already)
        ///
        /// z = m ^ c (accum) t
        template<typename CScalarT,
                 typename AccumT,
                 typename MScalarT,
                 typename AScalarT,
                 typename BScalarT>
        void masked_accum(
            std::vector<std::tuple<IndexType, CScalarT>>       &z,
            std::vector<std::tuple<IndexType, MScalarT>> const &m,
            bool                                                structure_flag,
            bool                                                complement_flag,
            AccumT                                       const &accum,
            std::vector<std::tuple<IndexType, AScalarT>> const &c,
            std::vector<std::tuple<IndexType, BScalarT>> const &t)
        {
            GRB_LOG_FN_BEGIN("masked_accum.v2");
            auto t_it = t.begin();
            auto m_it = m.begin();
            auto c_it = c.begin();

            // for each element of c find out if it is not in mask
            while ((t_it != t.end()) && (c_it != c.end()))
            {
                IndexType t_idx(std::get<0>(*t_it));
                IndexType c_idx(std::get<0>(*c_it));
                if (t_idx < c_idx)
                {
                    // t already masked
                    z.emplace_back(t_idx,
                                   static_cast<CScalarT>(std::get<1>(*t_it)));
                    ++t_it;
                }
                else if (c_idx < t_idx)
                {
                    if (advance_and_check_mask_iterator(
                            m_it, m.end(), structure_flag, c_idx) != complement_flag)
                    {
                        z.emplace_back(c_idx,
                                       static_cast<CScalarT>(std::get<1>(*c_it)));
                    }
                    ++c_it;
                }
                else
                {
                    z.emplace_back(
                        t_idx,
                        static_cast<CScalarT>(accum(std::get<1>(*c_it),
                                                    std::get<1>(*t_it))));
                    ++t_it;
                    ++c_it;
                }
            }

            while (t_it != t.end())
            {
                z.emplace_back(std::get<0>(*t_it),
                               static_cast<CScalarT>(std::get<1>(*t_it)));
                ++t_it;
            }

            while (c_it != c.end())
            {
                IndexType c_idx(std::get<0>(*c_it));
                if (advance_and_check_mask_iterator(
                        m_it, m.end(), structure_flag, c_idx) != complement_flag)
                {
                    z.emplace_back(c_idx, std::get<1>(*c_it));
                }
                ++c_it;
            }
            GRB_LOG_FN_END("masked_accum.v2");
        }

        // *******************************************************************
        /// Perform the following operation on sparse vectors implemented as
        /// vector<tuple<Index, value>>
        ///
        /// if complement_flag == false:
        ///    c = (!m ^ ci) U z, where z = (m ^ t);  i.e., union assumes disjoint sets
        /// else:
        ///   c =   (m ^ ci) U z, where z = (!m ^ t)
        template<typename CScalarT,
                 typename MScalarT,
                 typename ZScalarT>
        void masked_merge(
            std::vector<std::tuple<IndexType, CScalarT>>       &c,
            std::vector<std::tuple<IndexType, MScalarT>> const &m,
            bool                                                structure_flag,
            bool                                                complement_flag,
            std::vector<std::tuple<IndexType, CScalarT>> const &ci,
            std::vector<std::tuple<IndexType, ZScalarT>> const &z)
        {
            GRB_LOG_FN_BEGIN("masked_merge.v2");
            auto m_it = m.begin();
            auto c_it = ci.begin();
            auto z_it = z.begin();

            c.clear();

            IndexType next_z;
            for (auto const &elt : z)
            {
                next_z = std::get<0>(elt);
                while (c_it != ci.end() && (std::get<0>(*c_it) < next_z))
                {
                    IndexType next_c(std::get<0>(*c_it));
                    if (advance_and_check_mask_iterator(
                            m_it, m.end(), structure_flag, next_c) == complement_flag)
                    {
                        c.emplace_back(next_c, std::get<1>(*c_it));
                    }
                    ++c_it;
                }
                c.emplace_back(next_z, static_cast<CScalarT>(std::get<1>(elt)));
            }

            while (c_it != ci.end() && (!z.empty() && (std::get<0>(*c_it) <= next_z)))
            {
                ++c_it;
            }

            while (c_it != ci.end())
            {
                IndexType next_c(std::get<0>(*c_it));
                if (advance_and_check_mask_iterator(
                        m_it, m.end(), structure_flag, next_c) == complement_flag)
                {
                    c.emplace_back(next_c, std::get<1>(*c_it));
                }
                ++c_it;
            }

            GRB_LOG_FN_END("masked_merge.v2");
        }

    } // backend
} // grb


/**
 * @Author: Maxime Agor (4rzael)
 * @Date:   Sat Jun 23 2018
 * @Email:  maxime.agor23@gmail.com
 * @Project: CUDA-Based Simulator of Quantum Systems
 * @Filename: Circuit.hpp
 * @Last modified by:   vial-dj
 * @Last modified time: Wed Nov 14 2018, 11:27:21
 * @License: MIT License
 */

#pragma once
#include <limits>
#include <string>
#include <vector>
#include <boost/variant/variant.hpp>

#include "Parser/AST.hpp"

struct Circuit {
    // methods
    Circuit() {}
    Circuit(Circuit const &other, uint beginStep=0, uint endStep=std::numeric_limits<uint>::max());

    void removeMeasurements();

    struct Register {
        Register(const std::string &n, const uint &s) : name(n), size(s) {}
        std::string name;
        uint        size;
    };

    struct Qubit {
        Qubit(const std::string &n, const uint &e) : registerName(n), element(e) {}
        explicit Qubit(const Parser::AST::t_bit &b) : registerName(b.name), element(b.value) {}
        bool operator==(const Qubit &other) const;
        std::string registerName;
        uint        element;
    };

    struct GateInterface {
        virtual std::vector<Qubit> getTargets() const = 0;
        virtual ~GateInterface() {}
    };

    struct CXGate : public GateInterface {
        CXGate(const Qubit &ctrl, const Qubit &trgt)
        : control(ctrl), target(trgt) {}
        std::vector<Qubit> getTargets() const;

        Qubit control;
        Qubit target;
    };

    struct UGate : public GateInterface {
        UGate(double t, double p, double l, const Qubit &trgt)
        : theta(t), phi(p), lambda(l), target(trgt) {}
        std::vector<Qubit> getTargets() const;

        double theta;
        double phi;
        double lambda;
        Qubit  target;
    };

    struct Measurement : public GateInterface {
        Measurement(Measurement const &other): source(other.source), dest(other.dest) {}
        Measurement(const Qubit &src, const Qubit &dst)
        : source(src), dest(dst) {}
        std::vector<Qubit> getTargets() const;

        Qubit source;
        Qubit dest;
    };

    struct Reset : public GateInterface {
        explicit Reset(const Qubit &trgt)
        : target(trgt) {}
        std::vector<Qubit> getTargets() const;

        Qubit target;
    };

    struct Barrier : public GateInterface {
        explicit Barrier(const Qubit &trgt)
        : target(trgt) {}
        std::vector<Qubit> getTargets() const;

        Qubit target;
    };

    typedef boost::variant<CXGate, UGate, Measurement, Reset> ConditionalCompatibleGate;
    struct ConditionalGate : public GateInterface {
        ConditionalGate(const Circuit::Register &tested, uint value, ConditionalCompatibleGate const &_gate)
        : testedRegister(tested.name), expectedValue(value), gate(_gate), m_maxTestedRegisterSize(tested.size) {}
        std::vector<Qubit> getTargets() const;

        std::string testedRegister;
        uint expectedValue;
        ConditionalCompatibleGate gate;
    private:
        uint m_maxTestedRegisterSize; // only used for the getTargets() method
    };

    typedef boost::variant<CXGate, UGate, Measurement, Reset, Barrier, ConditionalGate> Gate;

    struct Step : public std::vector<Gate> {
        /**
         * @brief Returns whether a qubit is used in this step
         *
         * @param qubit The qubit to check
         * @return true if the qubit is used
         * @return false otherwise
         */
        bool isQubitUsed(Qubit const &qubit) const;

        /**
         * @brief Returns whether or not the step contains a measurement gate
         *
         * @return true if a measurement is present
         * @return false otherwise
         */
        bool containsMeasurement() const;
    };

    std::vector<Register> creg;
    std::vector<Register> qreg;
    std::vector<Step> steps;

    friend std::ostream& operator<< (std::ostream& stream, const Circuit & c);
};


#include <algorithm>
#include <iostream>
#include <numeric>
#include <string>

#define CERES_FOUND true

#include <opencv2/core/utils/filesystem.hpp>
#include <opencv2/core/utils/logger.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/sfm.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/xfeatures2d.hpp>

#include <boost/filesystem.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graphviz.hpp>

#define _USE_OPENCV true
#include <libs/MVS/Interface.h>

using namespace cv;
using namespace std;
namespace fs = boost::filesystem;

class StructureFromMotion {

public:
    StructureFromMotion(const string& dir, const float matchSurvivalRate = 0.5f,
        const bool viz = false, const string mvs = "", const string cloud = "",
        const bool saveDebug = false)
        : PAIR_MATCH_SURVIVAL_RATE(matchSurvivalRate)
        , visualize(viz)
        , saveMVS(mvs)
        , saveCloud(cloud)
        , saveDebugVisualizations(saveDebug)

    {
        findImagesInDiretcory(dir);
    }

    void runSfM()
    {
        extractFeatures();
        matchFeatures();
        buildTracks();
        reconstructFromTracks();
        if (visualize) {
            visualize3D();
        }
        if (saveMVS != "") {
            saveToMVSFile();
        }
        if (saveCloud != "") {
            CV_LOG_INFO(TAG, "Save point cloud to: " + saveCloud);
            viz::writeCloud(saveCloud, pointCloud, pointCloudColor);
        }
    }

private:
    void findImagesInDiretcory(const string& dir)
    {
        CV_LOG_INFO(TAG, "Finding images in " + dir);

        utils::fs::glob(dir, "*.jpg", imagesFilenames);
        utils::fs::glob(dir, "*.JPG", imagesFilenames);
        utils::fs::glob(dir, "*.png", imagesFilenames);
        utils::fs::glob(dir, "*.PNG", imagesFilenames);

        std::sort(imagesFilenames.begin(), imagesFilenames.end());

        CV_LOG_INFO(TAG, "Found " + std::to_string(imagesFilenames.size()) + " images");

        CV_LOG_INFO(TAG, "Reading images...");
        for (const auto& i : imagesFilenames) {
            CV_LOG_INFO(TAG, i);
            images[i] = imread(i);
            imageIDs[i] = images.size() - 1;
        }
    }

    void extractFeatures()
    {
        CV_LOG_INFO(TAG, "Extract Features");

        auto detector = AKAZE::create();
        auto extractor = AKAZE::create();

        for (const auto& i : imagesFilenames) {
            Mat grayscale;
            cvtColor(images[i], grayscale, COLOR_BGR2GRAY);
            detector->detect(grayscale, keypoints[i]);
            extractor->compute(grayscale, keypoints[i], descriptors[i]);

            CV_LOG_INFO(TAG, "Found " + to_string(keypoints[i].size()) + " keypoints in " + i);

            if (saveDebugVisualizations) {
                Mat out;
                drawKeypoints(images[i], keypoints[i], out, Scalar(0, 0, 255));
                imwrite(fs::basename(fs::path(i)) + "_features.jpg", out);
            }
        }
    }

    vector<DMatch> matchWithRatioTest(
        const DescriptorMatcher& matcher, const Mat& desc1, const Mat& desc2)
    {
        // Raw match
        vector<vector<DMatch>> nnMatch;
        matcher.knnMatch(desc1, desc2, nnMatch, 2);

        // Ratio test filter
        vector<DMatch> ratioMatched;
        for (size_t i = 0; i < nnMatch.size(); i++) {
            DMatch first = nnMatch[i][0];
            float dist1 = nnMatch[i][0].distance;
            float dist2 = nnMatch[i][1].distance;

            if (dist1 < MATCH_RATIO_THRESHOLD * dist2) {
                ratioMatched.push_back(first);
            }
        }

        return ratioMatched;
    }

    void matchFeatures()
    {
        CV_LOG_INFO(TAG, "Match Features");

        BFMatcher matcher(NORM_HAMMING);

        for (size_t i = 0; i < imagesFilenames.size() - 1; ++i) {
            for (size_t j = i + 1; j < imagesFilenames.size(); ++j) {
                const string imgi = imagesFilenames[i];
                const string imgj = imagesFilenames[j];

                // Match with ratio test filter
                vector<DMatch> match
                    = matchWithRatioTest(matcher, descriptors[imgi], descriptors[imgj]);

                // Reciprocity test filter
                vector<DMatch> matchRcp
                    = matchWithRatioTest(matcher, descriptors[imgj], descriptors[imgi]);
                vector<DMatch> merged;
                for (const DMatch& dmr : matchRcp) {
                    bool found = false;
                    for (const DMatch& dm : match) {
                        // Only accept match if 1 matches 2 AND 2 matches 1.
                        if (dmr.queryIdx == dm.trainIdx and dmr.trainIdx == dm.queryIdx) {
                            merged.push_back(dm);
                            found = true;
                            break;
                        }
                    }
                    if (found) {
                        continue;
                    }
                }

                // Fundamental matrix filter
                vector<uint8_t> inliersMask(merged.size());
                vector<Point2f> imgiPoints, imgjPoints;
                for (const auto& m : merged) {
                    imgiPoints.push_back(keypoints[imgi][m.queryIdx].pt);
                    imgjPoints.push_back(keypoints[imgj][m.trainIdx].pt);
                }
                findFundamentalMat(imgiPoints, imgjPoints, inliersMask);

                vector<DMatch> final;
                for (size_t m = 0; m < merged.size(); m++) {
                    if (inliersMask[m]) {
                        final.push_back(merged[m]);
                    }
                }

                if ((float) final.size() / (float)match.size() < PAIR_MATCH_SURVIVAL_RATE) {
                    CV_LOG_INFO(TAG,
                        "Final match '" + imgi + "'->'" + imgj + "' has less than "
                            + to_string(PAIR_MATCH_SURVIVAL_RATE) + " inliers from orignal. Skip");
                    continue;
                }

                matches[make_pair(imgi, imgj)] = final;

                CV_LOG_INFO(TAG,
                    "Matching " + imgi + " and " + imgj + ": " + to_string(final.size()) + " / "
                        + to_string(match.size()));

                if (saveDebugVisualizations) {
                    Mat out;
                    vector<DMatch> rawmatch;
                    matcher.match(descriptors[imgi], descriptors[imgj], rawmatch);
                    vector<pair<string, vector<DMatch>&>> showList { { "Raw Match", rawmatch },
                        { "Ratio Test Filter", match }, { "Reciprocal Filter", merged },
                        { "Epipolar Filter", final } };
                    for (size_t i = 0; i < showList.size(); i++) {
                        drawMatches(images[imgi], keypoints[imgi], images[imgj], keypoints[imgj],
                            showList[i].second, out, CV_RGB(255, 0, 0));
                        putText(out, showList[i].first, Point(10, 50), FONT_HERSHEY_COMPLEX, 2.0,
                            CV_RGB(255, 255, 255), 2);
                        putText(out, "# Matches: " + to_string(showList[i].second.size()),
                            Point(10, 100), FONT_HERSHEY_COMPLEX, 1.0, CV_RGB(255, 255, 255));
                        imwrite(fs::basename(fs::path(imgi)) + "_" + fs::basename(fs::path(imgj))
                                + "_" + to_string(i) + ".jpg",
                            out);
                    }
                }
            }
        }
    }

    void buildTracks()
    {
        CV_LOG_INFO(TAG, "Build tracks");

        using namespace boost;

        struct ImageFeature {
            string image;
            size_t featureID;
        };
        typedef adjacency_list<listS, vecS, undirectedS, ImageFeature> Graph;
        typedef graph_traits<Graph>::vertex_descriptor Vertex;

        map<pair<string, int>, Vertex> vertexByImageFeature;

        Graph g;

        // Add vertices - image features
        for (const auto& imgi : keypoints) {
            for (size_t i = 0; i < imgi.second.size(); i++) {
                Vertex v = add_vertex(g);
                g[v].image = imgi.first;
                g[v].featureID = i;
                vertexByImageFeature[make_pair(imgi.first, i)] = v;
            }
        }

        // Add edges - feature matches
        for (const auto& match : matches) {
            for (const DMatch& dm : match.second) {
                Vertex& vI = vertexByImageFeature[make_pair(match.first.first, dm.queryIdx)];
                Vertex& vJ = vertexByImageFeature[make_pair(match.first.second, dm.trainIdx)];
                add_edge(vI, vJ, g);
            }
        }

        using Filtered = filtered_graph<Graph, keep_all, std::function<bool(Vertex)>>;
        Filtered gFiltered(g, keep_all {}, [&g](Vertex vd) { return degree(vd, g) > 0; });

        // Get connected components
        std::vector<int> component(num_vertices(gFiltered), -1);
        int num = connected_components(gFiltered, &component[0]);
        map<int, vector<Vertex>> components;
        for (size_t i = 0; i != component.size(); ++i) {
            if (component[i] >= 0) {
                components[component[i]].push_back(i);
            }
        }
        // Filter bad components (with more than 1 feature from a single image)
        std::vector<int> vertexInGoodComponent(num_vertices(gFiltered), -1);
        map<int, vector<Vertex>> goodComponents;
        for (const auto& c : components) {
            set<string> imagesInComponent;
            bool isComponentGood = true;
            for (int j = 0; j < c.second.size(); ++j) {
                const string imgId = g[c.second[j]].image;
                if (imagesInComponent.count(imgId) > 0) {
                    // Image already represented in this component
                    isComponentGood = false;
                    break;
                } else {
                    imagesInComponent.insert(imgId);
                }
            }
            if (isComponentGood) {
                for (int j = 0; j < c.second.size(); ++j) {
                    vertexInGoodComponent[c.second[j]] = 1;
                }
                goodComponents[c.first] = c.second;
            }
        }

        Filtered gGoodComponents(g, keep_all {},
            [&vertexInGoodComponent](Vertex vd) { return vertexInGoodComponent[vd] > 0; });

        CV_LOG_INFO(TAG, "Total number of components found: " + to_string(components.size()));
        CV_LOG_INFO(TAG, "Number of good components: " + to_string(goodComponents.size()));
        const int accum = std::accumulate(goodComponents.begin(), goodComponents.end(), 0,
            [](int a, pair<const int, vector<Vertex>>& v) { return a + v.second.size(); });
        CV_LOG_INFO(TAG,
            "Average component size: " + to_string((float)accum / (float)(goodComponents.size())));

        if (saveDebugVisualizations) {
            struct my_node_writer {
                my_node_writer(Graph& g_, const map<string, int>& iid_)
                    : g(g_)
                    , iid(iid_) {};
                void operator()(std::ostream& out, Vertex v)
                {
                    const int imgId = iid[g[v].image];
                    out << " [label=\"" << imgId
                        << "\" colorscheme=\"accent8\" fillcolor=" << (imgId + 1)
                        << " style=filled]";
                };
                Graph g;
                map<string, int> iid;
            };
            std::ofstream ofs("match_graph_good_components.dot");
            write_graphviz(ofs, gGoodComponents, my_node_writer(g, imageIDs));
            std::ofstream ofsf("match_graph_filtered.dot");
            write_graphviz(ofsf, gFiltered, my_node_writer(g, imageIDs));
        }

        // Each component is a track
        const size_t nViews = imagesFilenames.size();
        tracks.resize(nViews);
        for (int i = 0; i < nViews; i++) {
            tracks[i].create(2, goodComponents.size(), CV_64FC1);
            tracks[i].setTo(-1.0);
        }
        int i = 0;
        for (auto c = goodComponents.begin(); c != goodComponents.end(); ++c, ++i) {
            for (const int v : c->second) {
                const int imageID = imageIDs[g[v].image];
                const size_t featureID = g[v].featureID;
                const Point2f p = keypoints[g[v].image][featureID].pt;
                tracks[imageID].at<double>(0, i) = p.x;
                tracks[imageID].at<double>(1, i) = p.y;
            }
        }

        if (saveDebugVisualizations) {
            vector<Scalar> colors
                = { CV_RGB(240, 248, 255), CV_RGB(250, 235, 215), CV_RGB(0, 255, 255),
                      CV_RGB(127, 255, 212), CV_RGB(240, 255, 255), CV_RGB(245, 245, 220),
                      CV_RGB(255, 228, 196), CV_RGB(255, 235, 205), CV_RGB(0, 0, 255),
                      CV_RGB(138, 43, 226), CV_RGB(165, 42, 42), CV_RGB(222, 184, 135) };

            vector<Mat> imagesM;
            for (const auto m : images)
                imagesM.push_back(m.second);
            Mat out;
            hconcat(vector<Mat>(imagesM.begin(), imagesM.begin() + 4), out);
            RNG& rng = cv::theRNG();
            const Size imgS = imagesM[0].size();
            for (int tId = 0; tId < 20; tId++) {
                const int trackId = rng(tracks[0].cols); // Randomize a track ID

                // Show track over images
                for (int i = 0; i < 3; i++) {
                    Point2f a = Point2f(tracks[i].col(trackId));
                    Point2f b = Point2f(tracks[i + 1].col(trackId));

                    if (a.x < 0 or a.y < 0 or b.x < 0 or b.y < 0) {
                        continue;
                    }

                    const Scalar c = colors[tId % colors.size()];
                    a.x += imgS.width * i;
                    b.x += imgS.width * (i + 1);
                    circle(out, a, 7, c, FILLED);
                    circle(out, b, 7, c, FILLED);
                    line(out, a, b, c, 3);
                }
                imwrite("tracks.jpg", out);

                // Show track patches
                const int patchSize = 20;
                const Point2f patch(patchSize, patchSize);
                for (int i = 0; i < tracks.size(); i++) {
                    Point2f a = Point2f(tracks[i].col(trackId));
                    if (a.x < patchSize or a.y < patchSize or a.x > imgS.width - patchSize
                        or a.y > imgS.height - patchSize) {
                        continue;
                    }

                    imwrite("track_" + to_string(trackId) + "_" + to_string(i) + ".png",
                        imagesM[i](Rect(a - patch, a + patch)));
                }
            }
        }
    }

    bool reconstructFromTracks()
    {
        CV_LOG_INFO(TAG, "Reconstruct from " + to_string(tracks[0].cols) + " tracks");
        const Size imgS = images.begin()->second.size();
        const float f = std::max(imgS.width, imgS.height);
        Mat K
            = Mat(Matx33f { f, 0.0, imgS.width / 2.0f, 0.0, f, imgS.height / 2.0f, 0.0, 0.0, 1.0 });
        cv::sfm::reconstruct(tracks, Rs, Ts, K, points3d, true);

        K.copyTo(K_);

        CV_LOG_INFO(TAG, "Reconstruction: ");
        CV_LOG_INFO(TAG, "Estimated 3D points: " + to_string(points3d.size()));
        CV_LOG_INFO(TAG, "Estimated cameras: " + to_string(Rs.size()));
        CV_LOG_INFO(TAG, "Refined intrinsics: ");
        CV_LOG_INFO(TAG, K_);

        if (Rs.size() != imagesFilenames.size()) {
            CV_LOG_ERROR(TAG,
                "Unable to reconstruct all camera views (" + to_string(imagesFilenames.size())
                    + ")");
            return false;
        }

        if (tracks[0].cols != points3d.size()) {
            CV_LOG_WARNING(
                TAG, "Unable to reconstruct all tracks (" + to_string(tracks[0].cols) + ")");
        }

        // Create the point cloud
        pointCloud.clear();
        for (const auto& p : points3d)
            pointCloud.emplace_back(Vec3f(p));

        // Get the point colors
        pointCloudColor.resize(pointCloud.size(), Vec3b(0, 255, 0));
        vector<Point2f> point2d(1);
        for (int i = 0; i < (int)pointCloud.size(); i++) {
            for (int j = 0; j < imagesFilenames.size(); ++j) {
                Mat point3d = Mat(pointCloud[i]).reshape(1, 1);
                cv::projectPoints(point3d, Rs[j], Ts[j], K_, Mat(), point2d);
                if (point2d[0].x < 0 or point2d[0].x >= imgS.width or point2d[0].y < 0
                    or point2d[0].y >= imgS.height) {
                    continue;
                }
                pointCloudColor[i] = images[imagesFilenames[j]].at<Vec3b>(point2d[0]);
                break;
            }
        }

        return true;
    }

    void visualize3D()
    {
        CV_LOG_INFO(TAG, "Visualize reconstruction");

        if (saveDebugVisualizations) {
            // 3d point reprojections
            Mat points2d;
            Mat points3dM(points3d.size(), 1, CV_32FC3);
            for (int i = 0; i < points3d.size(); i++) {
                points3dM.at<Vec3f>(i) = Vec3f(points3d[i]);
            }
            for (int j = 0; j < imagesFilenames.size(); j++) {
                cv::projectPoints(points3dM, Rs[j], Ts[j], K_, noArray(), points2d);

                Mat out;
                images[imagesFilenames[j]].copyTo(out);
                for (int i = 0; i < points2d.rows; i++) {
                    circle(out, points2d.at<Point2f>(i), 3, CV_RGB(255, 0, 0), FILLED);
                }
                imwrite("reprojection_" + to_string(j) + ".jpg", out);
            }
        }

        // Create 3D windows
        viz::Viz3d window("Coordinate Frame");
        window.setWindowSize(Size(500, 500));
        window.setWindowPosition(Point(150, 150));
        window.setBackgroundColor(viz::Color::white());

        // Recovering cameras
        vector<Affine3d> path;
        for (size_t i = 0; i < Rs.size(); ++i)
            path.push_back(Affine3d(Rs[i], Ts[i]));

        // Add the pointcloud
        viz::WCloud cloud_widget(pointCloud, pointCloudColor);
        window.showWidget("point_cloud", cloud_widget);
        // Add cameras
        window.showWidget("cameras_frames_and_lines",
            viz::WTrajectory(path, viz::WTrajectory::BOTH, 0.1, viz::Color::black()));
        window.showWidget(
            "cameras_frustums", viz::WTrajectoryFrustums(path, K_, 0.1, viz::Color::navy()));
        window.setViewerPose(path[0]);

        /// Wait for key 'q' to close the window
        CV_LOG_INFO(TAG, "Press 'q' to close ... ")

        window.spin();
    }

    void saveToMVSFile()
    {
        CV_LOG_INFO(TAG, "Save reconstruction to MVS file: " + saveMVS)

        MVS::Interface interface;
        MVS::Interface::Platform p;

        // Add camera
        MVS::Interface::Platform::Camera c;
        const Size imgS = images[imagesFilenames[0]].size();
        c.K = Matx33d(K_);
        c.R = Matx33d::eye();
        c.C = Point3d(0, 0, 0);
        c.name = "Camera1";
        c.width = imgS.width;
        c.height = imgS.height;
        p.cameras.push_back(c);

        // Add views
        p.poses.resize(Rs.size());
        for (size_t i = 0; i < Rs.size(); ++i) {
            Mat t = -Rs[i].t() * Ts[i];
            p.poses[i].C.x = t.at<double>(0);
            p.poses[i].C.y = t.at<double>(1);
            p.poses[i].C.z = t.at<double>(2);
            Mat r;
            Rs[i].copyTo(r);
            Mat(r).convertTo(p.poses[i].R, CV_64FC1);

            // Add corresponding image
            MVS::Interface::Image image;
            image.cameraID = 0;
            image.poseID = i;
            image.name = imagesFilenames[i];
            image.platformID = 0;
            interface.images.push_back(image);
        }
        p.name = "Platform1";
        interface.platforms.push_back(p);

        // Add point cloud
        for (size_t k = 0; k < points3d.size(); ++k) {
            MVS::Interface::Color c;
            MVS::Interface::Vertex v;
            v.X = Vec3f(points3d[k]);

            // Reproject to see if in image bounds and get the RGB color
            Mat point3d;
            Mat(points3d[k].t()).convertTo(point3d, CV_32FC1);
            for (uint32_t j = 0; j < tracks.size(); ++j) {
                vector<Point2f> points2d(1);
                cv::projectPoints(point3d, Rs[j], Ts[j], K_, Mat(), points2d);
                if (points2d[0].x < 0 or points2d[0].x > imgS.width or points2d[0].y < 0
                    or points2d[0].y > imgS.height) {
                    continue;
                } else {
                    c.c = images[imagesFilenames[j]].at<Vec3b>(points2d[0]);
                    v.views.push_back({ j, 1.0 });
                }
            }

            interface.verticesColor.push_back(c);
            interface.vertices.push_back(v);
        }

        MVS::ARCHIVE::SerializeSave(interface, saveMVS);
    }

    vector<String> imagesFilenames;
    map<string, int> imageIDs;
    map<string, Mat> images;
    map<string, vector<KeyPoint>> keypoints;
    map<string, Mat> descriptors;
    map<pair<string, string>, vector<DMatch>> matches;
    vector<Mat> Rs, Ts;
    vector<Mat> points3d;
    vector<Mat> tracks;
    vector<Vec3f> pointCloud;
    vector<Vec3b> pointCloudColor;
    Matx33f K_;

    const float MATCH_RATIO_THRESHOLD = 0.8f; // Nearest neighbor matching ratio
    const float
        PAIR_MATCH_SURVIVAL_RATE; // Ratio of surviving matches for a successful stereo match
    const bool visualize; // Show 3D visualization of the sprase cloud?
    const string saveMVS; // Save the reconstruction in MVS format for OpenMVS?
    const string saveCloud; // Save the reconstruction to a point cloud file?
    const bool saveDebugVisualizations; // Save debug visualizations from the reconstruction process

    const string TAG = "StructureFromMotion";
};

int main(int argc, char** argv)
{
    utils::logging::setLogLevel(utils::logging::LOG_LEVEL_DEBUG);

    cv::CommandLineParser parser(argc, argv,
        "{help h ? |       | help message}"
        "{@dir     | .     | directory with image files for reconstruction }"
        "{mrate    | 0.5   | Survival rate of matches to consider image pair success }"
        "{viz      | false | Visualize the sparse point cloud reconstruction? }"
        "{debug    | false | Save debug visualizations to files? }"
        "{mvs      |       | Save reconstruction to an .mvs file. Provide filename }"
        "{cloud    |       | Save reconstruction to a point cloud file (PLY, XYZ and OBJ). Provide "
        "filename}");

    if (parser.has("help")) {
        parser.printMessage();
        return 0;
    }

    StructureFromMotion sfm(parser.get<string>("@dir"), parser.get<float>("mrate"),
        parser.get<bool>("viz"), parser.get<string>("mvs"), parser.get<string>("cloud"),
        parser.get<bool>("debug"));
    sfm.runSfM();

    return 0;
}