#ifndef SKYLARK_HASH_TRANSFORM_MIXED_HPP
#define SKYLARK_HASH_TRANSFORM_MIXED_HPP

#include <map>
#include <boost/serialization/map.hpp>

#include "../base/sparse_vc_star_matrix.hpp"

#if SKYLARK_HAVE_COMBBLAS
#include "../utility/external/combblas_comm_grid.hpp"
#endif
#include "../utility/external/elemental_comm_grid.hpp"

namespace skylark { namespace sketch {

//FIXME:
//  - Benchmark one-sided vs. col/row comm (or midpoint scheme):
//    Most likely the scheme depends on the output Elemental distribution,
//    here we use the same comm-scheme for all output types.
//  - Processing Sparse matrix in blocks?
//  - MPI-3 stuff, see: Enabling highly-scalable remote memory access
//    programming with MPI-3 one sided, R. Gerstenbergerm,  M. Besta, and
//    T. Hoefler.

/* Specialization: sparse_vc_star for input, distributed Elemental for output */
template <typename ValueType,
          El::Distribution ColDist,
          El::Distribution RowDist,
          template <typename> class IdxDistributionType,
          template <typename> class ValueDistribution>
struct hash_transform_t <
    base::sparse_vc_star_matrix_t<ValueType>,
    El::DistMatrix<ValueType, ColDist, RowDist>,
    IdxDistributionType,
    ValueDistribution > :
        public hash_transform_data_t<IdxDistributionType,
                                     ValueDistribution> {
    typedef El::Int index_type;
    typedef ValueType value_type;
    typedef base::sparse_vc_star_matrix_t<value_type> matrix_type;
    typedef El::DistMatrix< value_type, ColDist, RowDist > output_matrix_type;
    typedef hash_transform_data_t<IdxDistributionType,
                                  ValueDistribution> data_type;

    /**
     * Regular constructor
     */
    hash_transform_t (int N, int S, base::context_t& context)
        : data_type(N, S, context)
    {}

    /**
     * Copy constructor
     */
    hash_transform_t (
        hash_transform_t<
            matrix_type, output_matrix_type,
            IdxDistributionType, ValueDistribution>& other)
        : data_type(other)
    {}

    /**
     * Constructor from data
     */
    hash_transform_t (const data_type& other_data)
        : data_type(other_data)
    {}

    template <typename Dimension>
    void apply (const matrix_type &A, output_matrix_type &sketch_of_A,
                Dimension dimension) const {
        try {
            apply_impl (A, sketch_of_A, dimension);
        } catch(boost::mpi::exception e) {
            SKYLARK_THROW_EXCEPTION (
                base::mpi_exception()
                    << base::error_msg(e.what()) );
        } catch (std::bad_alloc e) {
            SKYLARK_THROW_EXCEPTION (
                base::skylark_exception()
                    << base::error_msg("bad_alloc: out of memory") );
        }
    }

private:
    /**
     * Apply the sketching transform that is described in by the sketch_of_A.
     *
     * FIXME: distribution depending schemes would be more efficient
     */
    template <typename Dimension>
    void apply_impl (const matrix_type &A,
        output_matrix_type &sketch_of_A,
        Dimension dist) const {

        typedef size_t offset_idx_t;

        boost::mpi::communicator comm = skylark::utility::get_communicator(A);

        const size_t rank = comm.rank();

        const size_t ncols = sketch_of_A.Width();

        size_t comm_size = comm.size();

        const int* A_indptr  = A.indptr();
        const int* A_indices = A.indices();
        const value_type *A_values = A.locked_values();

        std::vector< std::map<size_t, size_t> > array_offsets(comm_size);

        // pre-compute processor targets of local sketch application
        for(int i = 0; i < A.width(); i++) {
            for (int j = A_indptr[i]; j < A_indptr[i + 1]; j++) {

                // compute global row and column id, and compress in one
                // target position index
                const size_t pos = getPos(
                        A.global_row(A_indices[j]), i, ncols, dist);

                // compute target processor for this target index
                const size_t target_rank = utility::owner(
                        sketch_of_A, pos / ncols, pos % ncols);

                assert(target_rank < comm_size);

                // map the position to the next empty slot
                if(array_offsets[target_rank].count(pos) == 0) {
                     size_t next_pos = array_offsets[target_rank].size();
                     array_offsets[target_rank][pos] = next_pos;
                }
            }
        }

        // constructing array holding start/end indices for one-sided access
        std::vector<offset_idx_t> proc_start_idx(comm_size + 1, 0);
        for(size_t i = 1; i < comm_size + 1; ++i)
            proc_start_idx[i] = proc_start_idx[i-1] + array_offsets[i-1].size();

        const size_t my_num_values = proc_start_idx[comm_size];
        // total number of nnz that will result when applying sketch locally
        std::vector<index_type> indices(my_num_values, 0);
        std::vector<value_type> values(my_num_values, 0);

        // Apply sketch for all local values. Note that some of the resulting
        // values might end up on a different processor. The data structure
        // fills values (sorted by processor id) in one continuous array.
        // Subsequently, one-sided operations can be used to access values for
        // each processor.
        for(int i = 0; i < A.width(); i++) {
            for (int j = A_indptr[i]; j < A_indptr[i + 1]; j++) {

                // compute global row and column id, and compress in one
                // target position index
                const size_t pos = getPos(
                        A.global_row(A_indices[j]), i, ncols, dist);

                // compute target processor for this target index
                const size_t target_rank = utility::owner(
                        sketch_of_A, pos / ncols, pos % ncols);

                assert(target_rank < comm_size);

                // get offset in array for current element
                const size_t ar_idx = proc_start_idx[target_rank] +
                    array_offsets[target_rank][pos];

                assert(ar_idx < indices.size());
                indices[ar_idx] = pos;

                assert(ar_idx < values.size());
                values[ar_idx]  += A_values[j] *
                    data_type::getValue(A.global_row(A_indices[j]), i, dist);
            }
        }

        // tell MPI that we will not use locks
        MPI_Info info;
        MPI_Info_create(&info);
        MPI_Info_set(info, "no_locks", "true");

        MPI_Win start_offset_win, idx_win, val_win;

        MPI_Win_create(&proc_start_idx[0], sizeof(size_t) * (comm_size + 1),
                       sizeof(size_t), info, comm, &start_offset_win);

        MPI_Win_create(&indices[0], sizeof(index_type) * indices.size(),
                       sizeof(index_type), info, comm, &idx_win);

        MPI_Win_create(&values[0], sizeof(value_type) * values.size(),
                       sizeof(value_type), info, comm, &val_win);

        MPI_Info_free(&info);

        // Synchronize epoch, no subsequent put operations (read only) and no
        // preceding fence calls.
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOPRECEDE, start_offset_win);
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOPRECEDE, idx_win);
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOPRECEDE, val_win);

        // accumulate values from other procs
        for(size_t p = 0; p < comm_size; ++p) {

            // get the start/end offset
            std::vector<size_t> offset(2);
            MPI_Get(&(offset[0]), 2, boost::mpi::get_mpi_datatype<size_t>(),
                    p, rank, 2, boost::mpi::get_mpi_datatype<size_t>(),
                    start_offset_win);

            MPI_Win_fence(MPI_MODE_NOPUT, start_offset_win);
            size_t num_values = offset[1] - offset[0];

            // and fill indices/values.
            std::vector<index_type> add_idx(num_values);
            std::vector<value_type> add_val(num_values);
            MPI_Get(&(add_idx[0]), num_values,
                    boost::mpi::get_mpi_datatype<index_type>(), p, offset[0],
                    num_values, boost::mpi::get_mpi_datatype<index_type>(),
                    idx_win);

            MPI_Get(&(add_val[0]), num_values,
                    boost::mpi::get_mpi_datatype<value_type>(), p, offset[0],
                    num_values, boost::mpi::get_mpi_datatype<value_type>(),
                    val_win);

            MPI_Win_fence(MPI_MODE_NOPUT, idx_win);
            MPI_Win_fence(MPI_MODE_NOPUT, val_win);

            // finally, set data in local buffer
            for(size_t i = 0; i < num_values; ++i) {
                index_type lrow = sketch_of_A.LocalRow(add_idx[i] / ncols);
                index_type lcol = sketch_of_A.LocalCol(add_idx[i] % ncols);
                sketch_of_A.UpdateLocal(lrow, lcol, add_val[i]);
            }
        }

        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOSUCCEED, start_offset_win);
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOSUCCEED, idx_win);
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOSUCCEED, val_win);

        MPI_Win_free(&start_offset_win);
        MPI_Win_free(&idx_win);
        MPI_Win_free(&val_win);
    }

    inline index_type getPos(index_type rowid, index_type colid, size_t ncols,
        columnwise_tag) const {
        return colid + ncols * data_type::row_idx[rowid];
    }

    inline index_type getPos(index_type rowid, index_type colid, size_t ncols,
        rowwise_tag) const {
        return rowid * ncols + data_type::row_idx[colid];
    }
};

#if SKYLARK_HAVE_COMBBLAS
/* Specialization: SpParMat for input, distributed Elemental for output */
template <typename IndexType,
          typename ValueType,
          El::Distribution ColDist,
          El::Distribution RowDist,
          template <typename> class IdxDistributionType,
          template <typename> class ValueDistribution>
struct hash_transform_t <
    SpParMat<IndexType, ValueType, SpDCCols<IndexType, ValueType> >,
    El::DistMatrix<ValueType, ColDist, RowDist>,
    IdxDistributionType,
    ValueDistribution > :
        public hash_transform_data_t<IdxDistributionType,
                                     ValueDistribution> {
    typedef IndexType index_type;
    typedef ValueType value_type;
    typedef SpDCCols< index_type, value_type > col_t;
    typedef FullyDistVec< index_type, value_type> mpi_vector_t;
    typedef SpParMat< index_type, value_type, col_t > matrix_type;
    typedef El::DistMatrix< value_type, ColDist, RowDist > output_matrix_type;
    typedef hash_transform_data_t<IdxDistributionType,
                                  ValueDistribution> data_type;

    /**
     * Regular constructor
     */
    hash_transform_t (int N, int S, base::context_t& context) :
        data_type(N, S, context)
    {}

    /**
     * Copy constructor
     */
    hash_transform_t (
        hash_transform_t<
            matrix_type, output_matrix_type,
            IdxDistributionType, ValueDistribution>& other)
        : data_type(other)
    {}

    /**
     * Constructor from data
     */
    hash_transform_t (const data_type& other_data)
        : data_type(other_data)
    {}

    template <typename Dimension>
    void apply (const matrix_type &A, output_matrix_type &sketch_of_A,
                Dimension dimension) const {
        try {
            apply_impl (A, sketch_of_A, dimension);
        } catch(boost::mpi::exception e) {
            SKYLARK_THROW_EXCEPTION (
                base::mpi_exception()
                    << base::error_msg(e.what()) );
        } catch (std::string e) {
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg(e) );
        } catch (std::logic_error e) {
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg(e.what()) );
        } catch (std::bad_alloc e) {
            SKYLARK_THROW_EXCEPTION (
                base::skylark_exception()
                    << base::error_msg("bad_alloc: out of memory") );
        }
    }

private:
    /**
     * Apply the sketching transform that is described in by the sketch_of_A.
     */
    template <typename Dimension>
    void apply_impl (const matrix_type &A_,
        output_matrix_type &sketch_of_A,
        Dimension dist) const {

        // We are essentially doing a 'const' access to A, but the necessary,
        // 'const' option is missing from the interface
        matrix_type &A = const_cast<matrix_type&>(A_);

        const size_t rank = A.getcommgrid()->GetRank();

        // extract columns of matrix
        col_t &data = A.seq();

        const size_t ncols = sketch_of_A.Width();

        const size_t my_row_offset = utility::cb_my_row_offset(A);
        const size_t my_col_offset = utility::cb_my_col_offset(A);

        size_t comm_size = A.getcommgrid()->GetSize();
        std::vector< std::set<size_t> > proc_set(comm_size);

        // pre-compute processor targets of local sketch application
        for(typename col_t::SpColIter col = data.begcol();
            col != data.endcol(); col++) {
            for(typename col_t::SpColIter::NzIter nz = data.begnz(col);
                nz != data.endnz(col); nz++) {

                // compute global row and column id, and compress in one
                // target position index
                const index_type rowid = nz.rowid()  + my_row_offset;
                const index_type colid = col.colid() + my_col_offset;
                const size_t pos       = getPos(rowid, colid, ncols, dist);

                // compute target processor for this target index
                const size_t target = utility::owner(
                        sketch_of_A, pos / ncols, pos % ncols);

                if(proc_set[target].count(pos) == 0) {
                    assert(target < comm_size);
                    proc_set[target].insert(pos);
                }
            }
        }

        // constructing array holding start/end indices for one-sided access
        std::vector<index_type> proc_start_idx(comm_size + 1, 0);
        for(size_t i = 1; i < comm_size + 1; ++i)
            proc_start_idx[i] = proc_start_idx[i-1] + proc_set[i-1].size();

        // total number of nnz that will result when applying sketch locally
        std::vector<index_type> indicies(proc_start_idx[comm_size], 0);
        std::vector<value_type> values(proc_start_idx[comm_size], 0);

        // Apply sketch for all local values. Note that some of the resulting
        // values might end up on a different processor. The data structure
        // fills values (sorted by processor id) in one continuous array.
        // Subsequently, one-sided operations can be used to access values for
        // each processor.
        for(typename col_t::SpColIter col = data.begcol();
            col != data.endcol(); col++) {
            for(typename col_t::SpColIter::NzIter nz = data.begnz(col);
                nz != data.endnz(col); nz++) {

                // compute global row and column id, and compress in one
                // target position index
                const index_type rowid = nz.rowid()  + my_row_offset;
                const index_type colid = col.colid() + my_col_offset;
                const size_t pos       = getPos(rowid, colid, ncols, dist);

                // compute target processor for this target index
                const size_t proc = utility::owner(
                        sketch_of_A, pos / ncols, pos % ncols);

                // get offset in array for current element
                const size_t ar_idx = proc_start_idx[proc] +
                    std::distance(proc_set[proc].begin(), proc_set[proc].find(pos));

                indicies[ar_idx] = pos;
                values[ar_idx]  += nz.value() *
                                   data_type::getValue(rowid, colid, dist);
            }
        }

        // Creating windows for all relevant arrays
        boost::mpi::communicator comm = utility::get_communicator(A);

        // tell MPI that we will not use locks
        MPI_Info info;
        MPI_Info_create(&info);
        MPI_Info_set(info, "no_locks", "true");

        MPI_Win start_offset_win, idx_win, val_win;

        MPI_Win_create(&proc_start_idx[0], sizeof(size_t) * (comm_size + 1),
                       sizeof(size_t), info, comm, &start_offset_win);

        MPI_Win_create(&indicies[0], sizeof(index_type) * indicies.size(),
                       sizeof(index_type), info, comm, &idx_win);

        MPI_Win_create(&values[0], sizeof(value_type) * values.size(),
                       sizeof(value_type), info, comm, &val_win);

        MPI_Info_free(&info);

        // Synchronize epoch, no subsequent put operations (read only) and no
        // preceding fence calls.
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOPRECEDE, start_offset_win);
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOPRECEDE, idx_win);
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOPRECEDE, val_win);

        // accumulate values from other procs
        for(size_t p = 0; p < comm_size; ++p) {

            // get the start/end offset
            std::vector<size_t> offset(2);
            MPI_Get(&(offset[0]), 2, boost::mpi::get_mpi_datatype<size_t>(),
                    p, rank, 2, boost::mpi::get_mpi_datatype<size_t>(),
                    start_offset_win);

            MPI_Win_fence(MPI_MODE_NOPUT, start_offset_win);
            size_t num_values = offset[1] - offset[0];

            // and fill indices/values.
            std::vector<index_type> add_idx(num_values);
            std::vector<value_type> add_val(num_values);
            MPI_Get(&(add_idx[0]), num_values,
                    boost::mpi::get_mpi_datatype<index_type>(), p, offset[0],
                    num_values, boost::mpi::get_mpi_datatype<index_type>(),
                    idx_win);

            MPI_Get(&(add_val[0]), num_values,
                    boost::mpi::get_mpi_datatype<value_type>(), p, offset[0],
                    num_values, boost::mpi::get_mpi_datatype<value_type>(),
                    val_win);

            MPI_Win_fence(MPI_MODE_NOPUT, idx_win);
            MPI_Win_fence(MPI_MODE_NOPUT, val_win);

            // finally, set data in local buffer
            for(size_t i = 0; i < num_values; ++i) {
                index_type lrow = sketch_of_A.LocalRow(add_idx[i] / ncols);
                index_type lcol = sketch_of_A.LocalCol(add_idx[i] % ncols);
                sketch_of_A.UpdateLocal(lrow, lcol, add_val[i]);
            }
        }

        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOSUCCEED, start_offset_win);
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOSUCCEED, idx_win);
        MPI_Win_fence(MPI_MODE_NOPUT | MPI_MODE_NOSUCCEED, val_win);

        MPI_Win_free(&start_offset_win);
        MPI_Win_free(&idx_win);
        MPI_Win_free(&val_win);
    }

    inline index_type getPos(index_type rowid, index_type colid, size_t ncols,
        columnwise_tag) const {
        return colid + ncols * data_type::row_idx[rowid];
    }

    inline index_type getPos(index_type rowid, index_type colid, size_t ncols,
        rowwise_tag) const {
        return rowid * ncols + data_type::row_idx[colid];
    }
};

/* Specialization: SpParMat for input, Local Elemental output */
template <typename IndexType,
          typename ValueType,
          template <typename> class IdxDistributionType,
          template <typename> class ValueDistribution>
struct hash_transform_t <
    SpParMat<IndexType, ValueType, SpDCCols<IndexType, ValueType> >,
    El::Matrix<ValueType>,
    IdxDistributionType,
    ValueDistribution > :
        public hash_transform_data_t<IdxDistributionType,
                                     ValueDistribution> {
    typedef IndexType index_type;
    typedef ValueType value_type;
    typedef SpDCCols< index_type, value_type > col_t;
    typedef FullyDistVec< index_type, value_type> mpi_vector_t;
    typedef SpParMat< index_type, value_type, col_t > matrix_type;
    typedef El::Matrix< value_type > output_matrix_type;
    typedef hash_transform_data_t<IdxDistributionType,
                                  ValueDistribution> data_type;

    /**
     * Regular constructor
     */
    hash_transform_t (int N, int S, base::context_t& context) :
        data_type(N, S, context) {

    }

    /**
     * Copy constructor
     */
    template <typename InputMatrixType,
              typename OutputMatrixType>
    hash_transform_t (hash_transform_t<InputMatrixType,
                                       OutputMatrixType,
                                       IdxDistributionType,
                                       ValueDistribution>& other) :
        data_type(other) {}

    /**
     * Constructor from data
     */
    hash_transform_t (hash_transform_data_t<IdxDistributionType,
                                            ValueDistribution>& other_data) :
        data_type(other_data) {}

    template <typename Dimension>
    void apply (const matrix_type &A, output_matrix_type &sketch_of_A,
                Dimension dimension) const {
        try {
            apply_impl (A, sketch_of_A, dimension);
        } catch(boost::mpi::exception e) {
            SKYLARK_THROW_EXCEPTION (
                base::mpi_exception()
                    << base::error_msg(e.what()) );
        } catch (std::string e) {
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg(e) );
        } catch (std::logic_error e) {
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg(e.what()) );
        } catch (std::bad_alloc e) {
            SKYLARK_THROW_EXCEPTION (
                base::skylark_exception()
                    << base::error_msg("bad_alloc: out of memory") );
        }
    }

private:
    /**
     * Apply the sketching transform that is described in by the sketch_of_A.
     */
    template <typename Dimension>
    void apply_impl (const matrix_type &A_,
        output_matrix_type &sketch_of_A,
        Dimension dist) const {

        // We are essentially doing a 'const' access to A, but the necessary,
        // 'const' option is missing from the interface
        matrix_type &A = const_cast<matrix_type&>(A_);

        const size_t rank = A.getcommgrid()->GetRank();

        // extract columns of matrix
        col_t &data = A.seq();

        const size_t my_row_offset = utility::cb_my_row_offset(A);
        const size_t my_col_offset = utility::cb_my_col_offset(A);

        int n_res_cols = A.getncol();
        int n_res_rows = A.getnrow();
        data_type::get_res_size(n_res_rows, n_res_cols, dist);

        // Apply sketch for all local values. Subsequently, all values are
        // gathered on processor 0 and the local matrix is populated.
        typedef std::map<index_type, value_type> col_values_t;
        col_values_t col_values;
        for(typename col_t::SpColIter col = data.begcol();
            col != data.endcol(); col++) {
            for(typename col_t::SpColIter::NzIter nz = data.begnz(col);
                nz != data.endnz(col); nz++) {

                index_type rowid = nz.rowid()  + my_row_offset;
                index_type colid = col.colid() + my_col_offset;

                const value_type value =
                    nz.value() * data_type::getValue(rowid, colid, dist);
                data_type::finalPos(rowid, colid, dist);
                col_values[colid * n_res_rows + rowid] += value;
            }
        }

        std::vector< std::map<index_type, value_type > > result;
        boost::mpi::gather(utility::get_communicator(A), col_values, result, 0);

        if(rank == 0) {
            typedef typename std::map<index_type, value_type>::iterator itr_t;
            for(size_t i = 0; i < result.size(); ++i) {
                itr_t proc_itr = result[i].begin();
                for(; proc_itr != result[i].end(); proc_itr++) {
                    int row = proc_itr->first % n_res_rows;
                    int col = proc_itr->first / n_res_rows;
                    sketch_of_A.Update(row, col, proc_itr->second);
                }
            }
        }
    }
};

/* Specialization: SpParMat for input, Elemental[* / *] output */
template <typename IndexType,
          typename ValueType,
          template <typename> class IdxDistributionType,
          template <typename> class ValueDistribution>
struct hash_transform_t <
    SpParMat<IndexType, ValueType, SpDCCols<IndexType, ValueType> >,
    El::DistMatrix<ValueType, El::STAR, El::STAR>,
    IdxDistributionType,
    ValueDistribution > :
        public hash_transform_data_t<IdxDistributionType,
                                     ValueDistribution> {
    typedef IndexType index_type;
    typedef ValueType value_type;
    typedef SpDCCols< index_type, value_type > col_t;
    typedef FullyDistVec< index_type, value_type> mpi_vector_t;
    typedef SpParMat< index_type, value_type, col_t > matrix_type;
    typedef El::DistMatrix< value_type, El::STAR, El::STAR > output_matrix_type;
    typedef hash_transform_data_t<IdxDistributionType,
                                  ValueDistribution> data_type;

    /**
     * Regular constructor
     */
    hash_transform_t (int N, int S, base::context_t& context) :
        data_type(N, S, context) {

    }

    /**
     * Copy constructor
     */
    template <typename InputMatrixType,
              typename OutputMatrixType>
    hash_transform_t (hash_transform_t<InputMatrixType,
                                       OutputMatrixType,
                                       IdxDistributionType,
                                       ValueDistribution>& other) :
        data_type(other) {}

    /**
     * Constructor from data
     */
    hash_transform_t (hash_transform_data_t<IdxDistributionType,
                                            ValueDistribution>& other_data) :
        data_type(other_data) {}

    template <typename Dimension>
    void apply (const matrix_type &A, output_matrix_type &sketch_of_A,
                Dimension dimension) const {
        try {
            apply_impl (A, sketch_of_A, dimension);
        } catch(boost::mpi::exception e) {
            SKYLARK_THROW_EXCEPTION (
                base::mpi_exception()
                    << base::error_msg(e.what()) );
        } catch (std::string e) {
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg(e) );
        } catch (std::logic_error e) {
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg(e.what()) );
        } catch (std::bad_alloc e) {
            SKYLARK_THROW_EXCEPTION (
                base::skylark_exception()
                    << base::error_msg("bad_alloc: out of memory") );
        }
    }

private:
    /**
     * Apply the sketching transform that is described in by the sketch_of_A.
     */
    template <typename Dimension>
    void apply_impl (const matrix_type &A_,
        output_matrix_type &sketch_of_A,
        Dimension dist) const {

        // We are essentially doing a 'const' access to A, but the necessary,
        // 'const' option is missing from the interface
        matrix_type &A = const_cast<matrix_type&>(A_);

        const size_t rank = A.getcommgrid()->GetRank();

        // extract columns of matrix
        col_t &data = A.seq();

        const size_t my_row_offset = utility::cb_my_row_offset(A);
        const size_t my_col_offset = utility::cb_my_col_offset(A);

        int n_res_cols = A.getncol();
        int n_res_rows = A.getnrow();
        data_type::get_res_size(n_res_rows, n_res_cols, dist);

        // Apply sketch for all local values. Subsequently, all values are
        // gathered on all processor and the "local" matrix is populated.
        typedef std::map<index_type, value_type> col_values_t;
        col_values_t col_values;
        for(typename col_t::SpColIter col = data.begcol();
            col != data.endcol(); col++) {
            for(typename col_t::SpColIter::NzIter nz = data.begnz(col);
                nz != data.endnz(col); nz++) {

                index_type rowid = nz.rowid()  + my_row_offset;
                index_type colid = col.colid() + my_col_offset;

                const value_type value =
                    nz.value() * data_type::getValue(rowid, colid, dist);
                data_type::finalPos(rowid, colid, dist);
                col_values[colid * n_res_rows + rowid] += value;
            }
        }

        std::vector< std::map<index_type, value_type > > result;
        boost::mpi::all_gather(
                utility::get_communicator(A), col_values, result);

        typedef typename std::map<index_type, value_type>::iterator itr_t;
        for(size_t i = 0; i < result.size(); ++i) {
            itr_t proc_itr = result[i].begin();
            for(; proc_itr != result[i].end(); proc_itr++) {
                int row = proc_itr->first % n_res_rows;
                int col = proc_itr->first / n_res_rows;
                sketch_of_A.Update(row, col, proc_itr->second);
            }
        }
    }
};
#endif

} } /** namespace skylark::sketch */

#endif // SKYLARK_HASH_TRANSFORM_MIXED_HPP


//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
// license.txt (http://opensource.org/licenses/MIT)

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <cmath>
#include <map>

#include "models/IntersectionDrivingModel.hpp"
#include "DriverUpdateParams.hpp"
#include "Driver.hpp"
#include "util/Utils.hpp"

using namespace std;
using namespace sim_mob;

MITSIM_IntDriving_Model::MITSIM_IntDriving_Model() :
intersectionAttentivenessFactorMin(0), intersectionAttentivenessFactorMax(0), minimumGap(0), impatienceFactor(0)
{
    modelType = IntModelType::Int_Model_MITSIM;
}

MITSIM_IntDriving_Model::MITSIM_IntDriving_Model(DriverUpdateParams& params) :
intersectionAttentivenessFactorMin(0), intersectionAttentivenessFactorMax(0), minimumGap(0), impatienceFactor(0)
{
    modelType = IntModelType::Int_Model_MITSIM;
    readDriverParameters(params);
}

MITSIM_IntDriving_Model::~MITSIM_IntDriving_Model()
{
}

double MITSIM_IntDriving_Model::getIntersectionAttentivenessFactorMin() const
{
    return intersectionAttentivenessFactorMin;
}

double MITSIM_IntDriving_Model::getIntersectionAttentivenessFactorMax() const
{
    return intersectionAttentivenessFactorMax;
}

double MITSIM_IntDriving_Model::getImpatienceFactor() const
{
    return impatienceFactor;
}

void MITSIM_IntDriving_Model::readDriverParameters(DriverUpdateParams &params)
{
    string modelName = "general_driver_model";
    string critical_gap_addon;
    bool isAMOD = false;

    //Check if the vehicle is autonomous
    if (params.driver->getParent()->amodId != "-1")
    {
        isAMOD = true;
    }

    //Get the parameter manager instance for the respective type of vehicle (normal or AMOD)
    ParameterManager *parameterMgr = ParameterManager::Instance(isAMOD);

    //Read the parameter values
    parameterMgr->param(modelName, "intersection_attentiveness_factor_min", intersectionAttentivenessFactorMin, 1.0);
    parameterMgr->param(modelName, "intersection_attentiveness_factor_max", intersectionAttentivenessFactorMax, 3.0);
    parameterMgr->param(modelName, "minimum_gap", minimumGap, 0.0);
    parameterMgr->param(modelName, "critical_gap_addon", critical_gap_addon, string("0.0 2.5"));
    parameterMgr->param(modelName, "impatience_factor", impatienceFactor, 0.2);

    //Vector to store the tokenized parameters
    std::vector<string> gapAddonParams;

    //Index
    int index = 0;

    //Tokenize the gap add-on parameters
    boost::trim(critical_gap_addon);
    boost::split(gapAddonParams, critical_gap_addon, boost::is_any_of(" "), boost::token_compress_on);

    //Convert into numeric form
    vector<string>::iterator itStr = gapAddonParams.begin();
    while (itStr != gapAddonParams.end())
    {
        double res = 0;

        try
        {
            res = boost::lexical_cast<double>(itStr->c_str());
        }
        catch (boost::bad_lexical_cast&)
        {
            std::stringstream str;
            str << __func__ << ": Could not covert " << *itStr << " to type double.";
            throw std::runtime_error(str.str());
        }

        criticalGapAddOn[index] = res;

        ++index;
        ++itStr;
    }
}

double MITSIM_IntDriving_Model::calcBrakeToStopAcc(double distance, DriverUpdateParams &params)
{
    double acc = 0;

    if (distance > Math::DOUBLE_EPSILON)
    {
        //v^2 = u^2 + 2as (Equation of motion)
        //So, a = (v^2 - u^2) / 2s
        //v = final velocity, u = initial velocity
        //a = acceleration, s = displacement
        double sqCurrVel = params.perceivedFwdVelocity * params.perceivedFwdVelocity;

        acc = -sqCurrVel / distance * 0.5;

        if (acc <= params.normalDeceleration)
        {
            acc = params.normalDeceleration;
        }
        else
        {
            double dt = params.nextStepSize;
            double vt = params.perceivedFwdVelocity * dt;
            double a = dt * dt;
            double b = 2.0 * vt - params.normalDeceleration * a;
            double c = sqCurrVel + 2.0 * params.normalDeceleration * (distance - vt);
            double d = b * b - 4.0 * a * c;

            if (!(d < 0 || a <= 0.0))
            {
                acc = (sqrt(d) - b) / a * 0.5;
            }
        }
    }
    else
    {
        double dt = params.nextStepSize;
        acc = (dt > 0.0) ? -(params.perceivedFwdVelocity) / dt : params.maxDeceleration;
    }

    //Make sure the value is bounded
    if (acc > params.maxAcceleration)
    {
        acc = params.maxAcceleration;
    }

    if (acc < params.maxDeceleration)
    {
        acc = params.maxDeceleration;
    }

    return acc;
}

double MITSIM_IntDriving_Model::calcCrawlingAcc(double distance, DriverUpdateParams& params)
{
    //Acceleration to slowly crawl towards the conflict point
    return 2 * ((distance / 2) - params.perceivedFwdVelocity * params.nextStepSize) / (params.nextStepSize * params.nextStepSize);
}

double MITSIM_IntDriving_Model::calcArrivalTime(double distance, DriverUpdateParams& params)
{
    double arrivalTime = -1, acceleration = 0, finalVel = 0;

    //The final velocity is limited by the turning speed, so calculate the acceleration required to
    //achieve the final velocity
    //v^2 = u^2 + 2as
    //So, a = (v^2 - u^2) / (2s)

    //Get the speed limit and convert it to m/s
    finalVel = 1; //currTurning->getTurningSpeed() / 3.6;

    //Calculate the acceleration
    acceleration = ((finalVel * finalVel) - (params.perceivedFwdVelocity * params.perceivedFwdVelocity)) / (2 * distance);

    //We know s = ut + (1/2)at^2
    //To find the time required, we rearrange the equation as follows:
    //(1/2)at^2 + ut - s = 0    This is a quadratic equation AX^2 + BX + C = 0 and we can solve for t
    //A = (1/2)a; B = u; C = -s

    if (acceleration == 0 && params.perceivedFwdVelocity != 0)
    {
        //Acceleration is 0, so we have a linear relation : ut - s = 0
        arrivalTime = distance / params.perceivedFwdVelocity;
    }
    else
    {
        //As it is a quadratic equation, we will have two solutions
        double sol1 = 0, sol2 = 0;

        //The discriminant (b^2 - 4ac)
        double discriminant = (params.perceivedFwdVelocity * params.perceivedFwdVelocity) - (2 * acceleration * (-distance));

        if (discriminant >= 0)
        {
            //Calculate the solutions
            sol1 = (-params.perceivedFwdVelocity - sqrt(discriminant)) / acceleration;
            sol2 = (-params.perceivedFwdVelocity + sqrt(discriminant)) / acceleration;

            //As time can be negative, return the solution that is a positive value
            if (sol1 >= 0 && sol2 >= 0)
            {
                arrivalTime = min(sol1, sol2);
            }
            else if (sol1 >= 0)
            {
                arrivalTime = sol1;
            }
            else if (sol2 >= 0)
            {
                arrivalTime = sol2;
            }
        }
    }

    return arrivalTime;
}

double MITSIM_IntDriving_Model::makeAcceleratingDecision(DriverUpdateParams &params)
{
    params.intDebugStr.clear();
    std::stringstream debugStr;
    double acc = params.maxAcceleration;
    const double vehicleLength = params.driver->getVehicleLength();
    const TurningGroup *currTurningGroup = currTurning->getTurningGroup();

    //Reduce the reaction time in intersection
    params.reactionTimeCounter = params.reactionTimeCounter * Utils::generateFloat(intersectionAttentivenessFactorMin, intersectionAttentivenessFactorMax);

    //Safety margin distance in front of the vehicle (half a vehicle length seems a reasonable margin)
    const double safeDist = 1.5 * vehicleLength;
    double distToStopLine = params.driver->getDistToIntersection() - safeDist;

    debugStr << ";t:" << params.now.ms() << ";dSL:" << distToStopLine;

    //Check if we've stopped close enough to the stop line
    if (distToStopLine <= 1 && params.perceivedFwdVelocity <= 0.1)
    {
        params.hasStoppedForStopSign = true;
        debugStr << ";StpSgn-Dne";
    }

    //We have to stop for the stop sign
    if (currTurningGroup->getRule() == TurningGroupRule::TURNING_GROUP_RULE_STOP_SIGN && !params.hasStoppedForStopSign && distToStopLine <= 5)
    {
        double brakingAcc = calcBrakeToStopAcc(distToStopLine, params);

        if (acc > brakingAcc && params.perceivedFwdVelocity != 0)
        {
            acc = brakingAcc;
            params.driver->setYieldingToInIntersection(-1);
        }

        debugStr << ";StpSgn";
        return acc;
    }

    //The turning conflicts for the current turning
    const vector<TurningConflict *> &conflicts = currTurning->getConflictsOnPath();

    //Select the nearest conflict ahead of us (the vector is sorted according to the distance from the start of the turning,
    //we may have crossed some)
    for (vector<TurningConflict *>::const_iterator itConflicts = conflicts.begin(); itConflicts != conflicts.end(); ++itConflicts)
    {
        const TurningConflict *conflict = *itConflicts;
        bool isGapRejected = false;

        debugStr << ";Cfl:" << conflict->getConflictId();

        //The priority of the turnings in the conflict
        //0 - equal priority, 1 - first turning has priority, 2 - second turning has priority
        int priority = conflict->getPriority();

        bool isFirstTurning = (conflict->getFirstTurning() == currTurning) ? true : false;

        //The distance to conflict point from the start of the turning
        double cfltDistFrmTurning = 0;

        //The distance to conflict point from our position
        double distToConflict = 0;

        //Check which turning in the conflict has the higher priority. If our turning has higher priority,
        //ignore all vehicles in the conflict as they will slow down for us
        if (isFirstTurning)
        {
            //Our turning has priority, or equal priority but other turning has
            //stop sign, ignore the vehicles in this conflict
            if (priority == 1 ||
                    (priority == 0 && currTurningGroup->getRule() == TurningGroupRule::TURNING_GROUP_RULE_NO_STOP_SIGN &&
                    conflict->getSecondTurning()->getTurningGroup()->getRule() == TurningGroupRule::TURNING_GROUP_RULE_STOP_SIGN))
            {
                continue;
            }

            //Get the distance to conflict point
            cfltDistFrmTurning = conflict->getFirstConflictDistance();
        }
        else
        {
            //Our turning has priority, or equal priority but other turning has
            //stop sign, ignore the vehicles in this conflict
            if (priority == 2 ||
                    (priority == 0 && currTurningGroup->getRule() == TurningGroupRule::TURNING_GROUP_RULE_NO_STOP_SIGN &&
                    conflict->getFirstTurning()->getTurningGroup()->getRule() == TurningGroupRule::TURNING_GROUP_RULE_STOP_SIGN))
            {
                continue;
            }

            //Get the distance to conflict point
            cfltDistFrmTurning = conflict->getSecondConflictDistance();
        }

        //Calculate the distance to conflict point from current position

        //If the vehicle is approaching the intersection rather than already in it, add the distance to the end
        //of the segment to the distance to conflict
        if (params.isApproachingIntersection)
        {
            distToConflict = cfltDistFrmTurning + params.driver->getDistToIntersection();
        }
        else
        {
            //Vehicle is in the intersection, so subtract the distance covered on the turning
            distToConflict = cfltDistFrmTurning - params.driver->getDistCoveredOnCurrWayPt();
        }

        debugStr << ";DstCfl:" << distToConflict;

        //The distance at which the vehicle needs to come to a stand-still when yielding
        double stoppingDist = 0;

        //If the turning has a stop sign, we stop just before the intersection
        if (currTurningGroup->getRule() == TurningGroupRule::TURNING_GROUP_RULE_STOP_SIGN)
        {
            stoppingDist = distToStopLine;
        }
        else
        {
            stoppingDist = distToConflict - safeDist;
        }

        debugStr << ";StpDis:" << stoppingDist;

        //If we're yet to reach the conflict, calculate the time required for us and the conflict driver
        //else go to the next conflict
        if (distToConflict > vehicleLength)
        {
            //Time taken to reach conflict by current driver
            double timeToConflict = calcArrivalTime(abs(distToConflict), params);

            if (timeToConflict == -1)
            {
                if (params.perceivedFwdVelocity > 1.0)
                {
                    timeToConflict = abs(distToConflict) / params.perceivedFwdVelocity;
                }
                else
                {
                    timeToConflict = 0;
                }
            }

            debugStr << ";Time2Cfl:" << timeToConflict;

            //Calculate the critical gap
            //It is the max of: minimumGap and the difference between the
            //criticalGap for the turning conflict and a random value
            double criticalGap = conflict->getCriticalGap();

            //Add a random add-on value
            criticalGap += Utils::nRandom(criticalGapAddOn[0], criticalGapAddOn[1]);

            //Reduce by impatience on if equal priority
            if (priority == 0)
            {
                criticalGap -= (params.impatienceTimer * impatienceFactor);
            }

            criticalGap = max(criticalGap, minimumGap);

            debugStr << ";CGap:" << criticalGap;

            //The map entry to the conflict and list of NearestVehicles on the it
            map<const TurningConflict *, std::set<NearestVehicle, compare_NearestVehicle> >::iterator itConflictVehicles = params.conflictVehicles.find(conflict);

            //Check if we have found an entry in the map
            if (itConflictVehicles != params.conflictVehicles.end())
            {
                debugStr << ";#veh:" << itConflictVehicles->second.size();

                //Iterator to the list of nearest vehicles on the list
                std::set<NearestVehicle, compare_NearestVehicle>::iterator itNearestVehicles = itConflictVehicles->second.begin();

                //Look for the vehicle reaching the conflict point - the list is sorted according to the distance from the conflict
                //point - distance less than 0 means yet to reach the conflict and greater than 0 means crossed the conflict point
                for (; itNearestVehicles != itConflictVehicles->second.end(); ++itNearestVehicles)
                {
                    Driver *drv = const_cast<Driver *> (itNearestVehicles->driver);
                    DriverUpdateParams &paramsOtherDriver = drv->getParams();

                    debugStr << ";Veh:" << paramsOtherDriver.parentId;
                    debugStr << ";DisCfl:" << itNearestVehicles->distance;

                    //If a driver is already yielding to us, scan the next conflict
                    if (itNearestVehicles->driver->getYieldingToInIntersection() == params.parentId)
                    {
                        debugStr << ";Yldng:";
                        break;
                    }

                    if (itNearestVehicles->distance >= -itNearestVehicles->driver->getVehicleLength() / 2 && itNearestVehicles->distance < 0.0)
                    {
                        //Other vehicle is blocking the conflict
                        debugStr << ";StpPtCrsd:";
                        isGapRejected = true;
                    }
                    else if (itNearestVehicles->distance < -itNearestVehicles->driver->getVehicleLength())
                    {
                        //The vehicle is yet to arrive at the conflict point

                        //Time taken to reach conflict point by incoming driver
                        double timeToConflictOtherDriver = 0;

                        //Speed of the incoming vehicle (m/s))
                        double speed = itNearestVehicles->driver->getVehicle()->getVelocity();

                        if (speed > 1.0)
                        {
                            //Negate the distance while calculating the time
                            timeToConflictOtherDriver = abs(itNearestVehicles->distance) / speed;
                        }

                        //The gap between the drivers
                        double gap = abs(timeToConflictOtherDriver - timeToConflict);

                        debugStr << ";T2Cfl:" << timeToConflictOtherDriver << ";gap:" << gap;

                        //The gap was accepted, but we need to check if there's enough space after the conflict
                        //point. A vehicle with higher priority might collide with us if we can't go past
                        //the conflict point
                        if (gap >= criticalGap && timeToConflictOtherDriver != 0 && timeToConflict != 0)
                        {
                            debugStr << ";gp>=cgap";

                            //Distance of forward vehicle from the conflict point
                            double distAheadOfConflict = DBL_MAX;

                            if (params.nvFwd.driver)
                            {
                                //The forward driver exists, reject the gap if he's not far enough ahead of the conflict
                                distAheadOfConflict = params.nvFwd.distance - distToConflict;
                            }
                            else if (params.nvFwdNextLink.driver)
                            {
                                //There is no forward driver, but there is one after the intersection
                                distAheadOfConflict = params.nvFwdNextLink.distance - distToConflict;
                            }

                            //Reject the gap if we don't have at least 5 vehicle length of space
                            if (distAheadOfConflict < (5 * vehicleLength))
                            {
                                isGapRejected = true;
                            }
                        }
                        else if (timeToConflict != 0 && timeToConflictOtherDriver == 0)
                        {
                            //Other driver has stopped
                            debugStr << ";Time2Cfl!=0&&T2Cfl!=0";

                            //Assume the vehicle starts moving with the same speed as us

                            //Assumed time calculated based on our speed
                            double assumedTimeToConflict = abs(itNearestVehicles->distance) / params.currSpeed;

                            debugStr << ";AsmdT2Cfl:" << assumedTimeToConflict;

                            //Assumed gap
                            double assumedGap = abs(assumedTimeToConflict - timeToConflict);

                            //Check if the gap is accepted
                            if (assumedGap <= criticalGap && itNearestVehicles->driver->getYieldingToInIntersection() == -1)
                            {
                                debugStr << ";AsmdGap-Rjct";
                                isGapRejected = true;
                            }
                            else
                            {
                                debugStr << ";AsmdGap-Accpt";
                            }
                        }
                        else if (timeToConflict == 0 && timeToConflictOtherDriver != 0)
                        {
                            //We have stopped, crawl till we can better judge the gap
                            debugStr << ";Time2Cfl==0&&T2Cfl!=0";

                            if (distToConflict > abs(itNearestVehicles->distance) && itNearestVehicles->driver->getYieldingToInIntersection() == -1)
                            {
                                //If we're stationary, crawl to conflict point
                                double crawlAcc = calcCrawlingAcc(stoppingDist, params);

                                if (acc > crawlAcc)
                                {
                                    acc = crawlAcc;
                                    params.driver->setYieldingToInIntersection(paramsOtherDriver.parentId);

                                    debugStr << ";Crawl:" << crawlAcc << ";YldVeh:" << paramsOtherDriver.parentId;
                                }
                                else
                                {
                                    debugStr << ";XCrawl:" << crawlAcc;
                                    isGapRejected = true;
                                }
                            }
                        }
                        else if (timeToConflict == 0 && timeToConflictOtherDriver == 0)
                        {
                            //Both vehicles have stopped, break deadlock
                            debugStr << ";Time2Cfl==0&&T2Cfl==0";

                            //Compare the distances, if the one nearer to the conflict can go through
                            if (distToConflict > abs(itNearestVehicles->distance) && itNearestVehicles->driver->getYieldingToInIntersection() == -1)
                            {
                                debugStr << ";OthClser";
                                isGapRejected = true;
                            }
                            else
                            {
                                debugStr << ";IClsr";
                            }
                        }
                        else
                        {
                            //All other cases, reject gap
                            isGapRejected = true;
                        }
                    }

                    //If the gap has been rejected (slow down and stop)
                    if (isGapRejected)
                    {
                        //Calculate the deceleration required to stop before conflict
                        double brakingAcc = calcBrakeToStopAcc(stoppingDist, params);

                        debugStr << ";Gap-Rjct" << ";brake:" << brakingAcc;

                        //If this deceleration is smaller than the one we have previously, use this
                        if (acc > brakingAcc)
                        {
                            acc = brakingAcc;
                            params.driver->setYieldingToInIntersection(paramsOtherDriver.parentId);

                            debugStr << ";YldVeh:" << paramsOtherDriver.parentId << "Gap-Rjct";
                        }
                    }
                }
            }
        }
        else
        {
            debugStr << ";XdCflt" << conflict->getConflictId() << ";dist:" << distToConflict;
        }
    }

    params.intDebugStr = debugStr.str();
    return acc;
}


#ifndef PARMCB_SPTREES_HPP_
#define PARMCB_SPTREES_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/throw_exception.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/serialization/vector.hpp>

#include <parmcb/detail/lex_dijkstra.hpp>
#include <parmcb/detail/util.hpp>

#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>

#include <memory>
#include <stack>
#include <functional>

namespace parmcb {

    template<class Graph, class WeightMap> class SPNode;
    template<class Graph, class WeightMap> class SPTree;
    template<class Graph, class WeightMap, class T> struct SPSubtree;
    template<class Graph, class WeightMap, bool ParallelUsingTBB> class SPTrees;
    template<class Graph, class WeightMap> class CandidateCycle;
    template<class Graph> struct SerializableCandidateCycle;
    template<class Graph, class WeightMap> struct SerializableMinOddCycle;
    template<class Graph, class WeightMap> struct SerializableMinOddCycleMinOp;

    template<class Graph, class WeightMap>
    class SPNode {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        SPNode() :
                _vertex(), _parity(false), _weight(WeightType()), _pred(), _has_pred(false) {
        }

        SPNode(Vertex vertex, WeightType weight) :
                _vertex(vertex), _parity(false), _weight(WeightType()), _pred(), _has_pred(false) {
        }

        SPNode(Vertex vertex, WeightType weight, const Edge &pred) :
                _vertex(vertex), _parity(false), _weight(weight), _pred(pred), _has_pred(true) {
        }

        void add_child(std::shared_ptr<SPNode<Graph, WeightMap>> c) {
            _children.push_back(c);
        }

        std::vector<std::shared_ptr<SPNode<Graph, WeightMap>>>& children() {
            return _children;
        }

        Vertex& vertex() {
            return _vertex;
        }

        bool& parity() {
            return _parity;
        }

        WeightType& weight() {
            return _weight;
        }

        const Edge& pred() {
            return _pred;
        }

        bool has_pred() {
            return _has_pred;
        }

    private:
        Vertex _vertex;
        bool _parity;
        WeightType _weight;
        Edge _pred;
        bool _has_pred;
        std::vector<std::shared_ptr<SPNode<Graph, WeightMap>>> _children;
    };

    template<class Graph, class WeightMap, class T>
    struct SPSubtree {
        T info;
        std::shared_ptr<SPNode<Graph, WeightMap>> root;

        SPSubtree(T info, std::shared_ptr<SPNode<Graph, WeightMap>> root) :
                info(info), root(root) {
        }
    };

    template<class Graph, class WeightMap>
    class SPTree {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        SPTree(std::size_t id, const Graph &g, const VertexIndexMapType& index_map, const WeightMap &weight_map, const Vertex &source) :
                _id(id), _g(g), _weight_map(weight_map), _index_map(index_map), _source(
                        source), _tree_node_map(boost::num_vertices(g)), _first_in_path(boost::num_vertices(g)) {
            initialize();
        }

        void update_parities(const std::set<Edge> &edges) {
            std::stack<SPSubtree<Graph, WeightMap, bool>> stack;
            stack.emplace(false, _root);

            while (!stack.empty()) {
                SPSubtree<Graph, WeightMap, bool> r = stack.top();
                stack.pop();

                r.root->parity() = r.info;
                for (auto c : r.root->children()) {
                    bool is_signed = edges.find(c->pred()) != edges.end();
                    stack.emplace(SPSubtree<Graph, WeightMap, bool> { static_cast<bool>(r.info ^ is_signed), c });
                }
            }
        }

        std::shared_ptr<SPNode<Graph, WeightMap>> node(const Vertex &v) const {
            return _tree_node_map[_index_map[v]];
        }

        const Vertex& source() const {
            return _source;
        }

        const Graph& graph() const {
            return _g;
        }

        const std::size_t id() const {
            return _id;
        }

        const Vertex first(const Vertex &v) {
            auto vindex = _index_map[v];
            return _first_in_path[vindex];
        }

        template<class EdgeIterator>
        std::vector<CandidateCycle<Graph, WeightMap>> create_candidate_cycles(EdgeIterator begin,
                EdgeIterator end) const {
            // collect tree edges
            std::set<Edge> tree_edges;
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(_g); vi != viend; ++vi) {
                auto v = *vi;
                auto vindex = _index_map[v];
                std::shared_ptr<SPNode<Graph, WeightMap>> n = _tree_node_map[vindex];
                if (n != nullptr && n->has_pred()) {
                    tree_edges.insert(n->pred());
                }
            }

            // loop over (non-tree) provided edges and create candidate cycles
            std::vector<CandidateCycle<Graph, WeightMap>> cycles;
            for (EdgeIterator it = begin; it != end; it++) {
                Edge e = *it;
                if (tree_edges.find(e) != tree_edges.end()) {
                    continue;
                }

                // non-tree edge
                std::shared_ptr<SPNode<Graph, WeightMap>> v = node(boost::source(e, _g));
                if (v == nullptr) {
                    continue;
                }
                std::shared_ptr<SPNode<Graph, WeightMap>> u = node(boost::target(e, _g));
                if (u == nullptr) {
                    continue;
                }

                if (_first_in_path[_index_map[v->vertex()]] == _first_in_path[_index_map[u->vertex()]]) {
                    // shortest paths start with the same vertex, discard
                    continue;
                }

                WeightType cycle_weight = boost::get(_weight_map, e) + v->weight() + u->weight();
                cycles.emplace_back(_id, e, cycle_weight);
            }
            return cycles;
        }

        std::vector<CandidateCycle<Graph, WeightMap>> create_candidate_cycles() const {
            auto itPair = boost::edges(_g);
            return create_candidate_cycles(itPair.first, itPair.second);
        }

        std::vector<SerializableCandidateCycle<Graph>> create_serializable_candidate_cycles(
                const ForestIndex<Graph> &forest_index) {
            // collect tree edges
            std::set<Edge> tree_edges;
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(_g); vi != viend; ++vi) {
                auto v = *vi;
                auto vindex = _index_map[v];
                std::shared_ptr<SPNode<Graph, WeightMap>> n = _tree_node_map[vindex];
                if (n != nullptr && n->has_pred()) {
                    tree_edges.insert(n->pred());
                }
            }

            // loop over all non-tree edges and create candidate cycles
            std::vector<SerializableCandidateCycle<Graph>> cycles;
            for (const auto &e : boost::make_iterator_range(boost::edges(_g))) {
                if (tree_edges.find(e) != tree_edges.end()) {
                    continue;
                }

                // non-tree edge
                std::shared_ptr<SPNode<Graph, WeightMap>> v = node(boost::source(e, _g));
                if (v == nullptr) {
                    continue;
                }
                std::shared_ptr<SPNode<Graph, WeightMap>> u = node(boost::target(e, _g));
                if (u == nullptr) {
                    continue;
                }

                if (_first_in_path[_index_map[v->vertex()]] == _first_in_path[_index_map[u->vertex()]]) {
                    // shortest paths start with the same vertex, discard
                    continue;
                }

                cycles.emplace_back(_source, forest_index(e));
            }

            return cycles;
        }

    private:
        const std::size_t _id;
        const Graph &_g;
        const WeightMap &_weight_map;
        const VertexIndexMapType &_index_map;
        const Vertex _source;

        /*
         * Shortest path tree root
         */
        std::shared_ptr<SPNode<Graph, WeightMap>> _root;
        /*
         * Map from vertex to shortest path tree node
         */
        std::vector<std::shared_ptr<SPNode<Graph, WeightMap>>> _tree_node_map;
        /*
         * First vertex in shortest path from root to a vertex.
         */
        std::vector<Vertex> _first_in_path;

        void initialize() {
            // run shortest path
            std::vector<WeightType> dist(boost::num_vertices(_g), (std::numeric_limits<WeightType>::max)());
            boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, WeightType>, Vertex, WeightType&> dist_map(
                    parmcb::detail::VertexIndexFunctor<Graph, WeightType>(dist, _index_map));
            std::vector<std::tuple<bool, Edge>> pred(boost::num_vertices(_g), std::make_tuple(false, Edge()));
            boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, std::tuple<bool, Edge>>, Vertex,
                    std::tuple<bool, Edge>&> pred_map(
                    parmcb::detail::VertexIndexFunctor<Graph, std::tuple<bool, Edge> >(pred, _index_map));
            lex_dijkstra(_g, _weight_map, _source, dist_map, pred_map);

            // create tree nodes and mapping
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(_g); vi != viend; ++vi) {
                auto v = *vi;
                auto vindex = _index_map[v];
                auto p = boost::get(pred_map, v);
                if (v == _source) {
                    _tree_node_map[vindex] = std::shared_ptr<SPNode<Graph, WeightMap>>(
                            new SPNode<Graph, WeightMap>(v, dist[vindex]));
                    _root = _tree_node_map[vindex];
                } else if (std::get<0>(p)) {
                    Edge e = std::get<1>(p);
                    _tree_node_map[vindex] = std::shared_ptr<SPNode<Graph, WeightMap>>(
                            new SPNode<Graph, WeightMap>(v, dist[vindex], e));
                }
            }

            // link tree nodes
            for (boost::tie(vi, viend) = boost::vertices(_g); vi != viend; ++vi) {
                auto v = *vi;
                auto p = boost::get(pred_map, v);
                if (std::get<0>(p)) {
                    auto e = std::get<1>(p);
                    auto u = boost::opposite(e, v, _g);
                    auto vindex = _index_map[v];
                    auto uindex = _index_map[u];
                    _tree_node_map[uindex]->add_child(_tree_node_map[vindex]);
                }
            }

            // compute first in path
            compute_first_in_path();
        }

        void compute_first_in_path() {
            std::stack<SPSubtree<Graph, WeightMap, Vertex>> stack;
            stack.emplace(_source, _root);

            while (!stack.empty()) {
                SPSubtree<Graph, WeightMap, Vertex> r = stack.top();
                stack.pop();

                if (r.root == _root) {
                    auto v = r.root->vertex();
                    auto vindex = _index_map[v];
                    _first_in_path[vindex] = v;
                    for (auto c : r.root->children()) {
                        stack.emplace(SPSubtree<Graph, WeightMap, Vertex> { static_cast<Vertex>(c->vertex()), c });
                    }
                } else {
                    auto v = r.root->vertex();
                    auto vindex = _index_map[v];
                    _first_in_path[vindex] = r.info;
                    for (auto c : r.root->children()) {
                        stack.emplace(SPSubtree<Graph, WeightMap, Vertex> { static_cast<Vertex>(r.info), c });
                    }
                }
            }
        }

    };

    template<class Graph, class WeightMap>
    class CandidateCycle {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        CandidateCycle(std::size_t tree, const Edge &e, WeightType weight) :
                _tree(tree), _e(e), _weight(weight) {
        }

        CandidateCycle(const CandidateCycle &c) :
                _tree(c._tree), _e(c._e), _weight(c._weight) {
        }

        CandidateCycle& operator=(const CandidateCycle &other) {
            if (this != &other) {
                _tree = other._tree;
                _e = other._e;
                _weight = other._weight;
            }
            return *this;
        }

        std::size_t tree() const {
            return _tree;
        }

        const Edge& edge() const {
            return _e;
        }

        const WeightType& weight() const {
            return _weight;
        }

    private:
        std::size_t _tree;
        Edge _e;
        WeightType _weight;
    };

    template<class Graph>
    struct SerializableCandidateCycle {
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename ForestIndex<Graph>::size_type Edge;

        SerializableCandidateCycle() {
        }

        SerializableCandidateCycle(Vertex v, Edge e) :
                v(v), e(e) {
        }

        template<typename Archive>
        void serialize(Archive &ar, const unsigned) {
            ar & v;
            ar & e;
        }

        Vertex v;
        Edge e;
    };

    template<class Graph, class WeightMap>
    struct SerializableMinOddCycle {
        typedef typename ForestIndex<Graph>::size_type Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        SerializableMinOddCycle() :
                exists(false) {
        }

        SerializableMinOddCycle(std::vector<Edge> edges, WeightType weight, bool exists) :
                edges(edges), weight(weight), exists(exists) {
        }

        SerializableMinOddCycle(const SerializableMinOddCycle<Graph, WeightMap> &c) :
                edges(c.edges), weight(c.weight), exists(c.exists) {
        }

        SerializableMinOddCycle<Graph, WeightMap>& operator=(const SerializableMinOddCycle<Graph, WeightMap> &other) {
            if (this != &other) {
                edges = other.edges;
                weight = other.weight;
                exists = other.exists;
            }
            return *this;
        }

        template<typename Archive>
        void serialize(Archive &ar, const unsigned) {
            ar & edges;
            ar & weight;
            ar & exists;
        }

        std::vector<Edge> edges;
        WeightType weight;
        bool exists;
    };

    template<class Graph, class WeightMap>
    struct SerializableMinOddCycleMinOp {

        const SerializableMinOddCycle<Graph, WeightMap>& operator()(
                const SerializableMinOddCycle<Graph, WeightMap> &lhs,
                const SerializableMinOddCycle<Graph, WeightMap> &rhs) const {
            if (!lhs.exists || !rhs.exists) {
                if (lhs.exists) {
                    return lhs;
                } else {
                    return rhs;
                }
            }
            // both valid, compare
            if (lhs.weight < rhs.weight) {
                return lhs;
            }
            return rhs;
        }

    };

    template<class Graph, class WeightMap>
    class CandidateCycleToSerializableConverter {
    public:
        CandidateCycleToSerializableConverter(const std::vector<parmcb::SPTree<Graph, WeightMap>> &trees,
                const ForestIndex<Graph> &forest_index) :
                trees(trees), forest_index(forest_index) {
        }

        SerializableCandidateCycle<Graph> operator()(const CandidateCycle<Graph, WeightMap> &cycle) const {
            return SerializableCandidateCycle<Graph>(trees.at(cycle.tree()).source(), forest_index(cycle.edge()));
        }

    private:
        const std::vector<parmcb::SPTree<Graph, WeightMap>> &trees;
        const ForestIndex<Graph> &forest_index;
    };

    template<class Graph, class WeightMap>
    class CandidateCycleBuilder {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        CandidateCycleBuilder(const Graph &g, const WeightMap &weight_map) :
                g(g), weight_map(weight_map) {
        }

        std::tuple<std::set<Edge>, WeightType, bool> operator()(const std::vector<parmcb::SPTree<Graph, WeightMap>> &trees,
                const CandidateCycle<Graph, WeightMap> &c, const std::set<Edge> &signed_edges, bool use_weight_limit,
                WeightType weight_limit) const {

            std::shared_ptr<SPNode<Graph, WeightMap>> v = trees[c.tree()].node(boost::source(c.edge(), g));
            std::shared_ptr<SPNode<Graph, WeightMap>> u = trees[c.tree()].node(boost::target(c.edge(), g));

            Edge e = c.edge();
            if (v->parity() ^ u->parity() ^ (signed_edges.find(e) != signed_edges.end())) {
                // odd cycle, validate
                bool valid = true;
                WeightType cycle_weight = boost::get(weight_map, e);
                std::set<Edge> result;
                result.insert(e);

                if (use_weight_limit && cycle_weight > weight_limit) {
                    return std::make_tuple(std::set<Edge> { }, 0.0, false);
                }

                // first part
                Vertex w = boost::source(c.edge(), g);
                std::shared_ptr<SPNode<Graph, WeightMap>> ws = trees[c.tree()].node(w);
                while (ws->has_pred()) {
                    Edge a = ws->pred();
                    if (result.insert(a).second == false) {
                        valid = false;
                        break;
                    }
                    cycle_weight += boost::get(weight_map, a);
                    if (use_weight_limit && cycle_weight > weight_limit) {
                        valid = false;
                        break;
                    }
                    w = boost::opposite(a, w, g);
                    ws = trees[c.tree()].node(w);
                }

                if (!valid) {
                    return std::make_tuple(std::set<Edge> { }, 0.0, false);
                }

                // second part
                w = boost::target(c.edge(), g);
                ws = trees[c.tree()].node(w);
                while (ws->has_pred()) {
                    Edge a = ws->pred();
                    if (result.insert(a).second == false) {
                        valid = false;
                        break;
                    }
                    cycle_weight += boost::get(weight_map, a);
                    if (use_weight_limit && cycle_weight > weight_limit) {
                        valid = false;
                        break;
                    }
                    w = boost::opposite(a, w, g);
                    ws = trees[c.tree()].node(w);
                }

                if (!valid) {
                    return std::make_tuple(std::set<Edge> { }, 0.0, false);
                }

                return std::make_tuple(result, cycle_weight, true);
            }
            return std::make_tuple(std::set<Edge> { }, 0.0, false);
        }

    private:
        const Graph &g;
        const WeightMap &weight_map;
    };

    template<class Graph, class WeightMap, bool ParallelUsingTBB>
    class ShortestOddCycleLookup {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        ShortestOddCycleLookup(const Graph &g, const WeightMap &weight_map,
                std::vector<parmcb::SPTree<Graph, WeightMap>> &trees,
                std::vector<parmcb::CandidateCycle<Graph, WeightMap>> &cycles, bool sorted_cycles) :
                g(g), weight_map(weight_map), candidate_cycle_builder(g, weight_map), trees(trees), cycles(cycles), sorted_cycles(
                        sorted_cycles) {
        }

        std::tuple<std::set<Edge>, WeightType, bool> operator()(const std::set<Edge> &edges) {
            return compute_shortest_odd_cycle(edges);
        }

    private:

        template<bool is_tbb_enabled = ParallelUsingTBB>
        std::tuple<std::set<Edge>, WeightType, bool> compute_shortest_odd_cycle(const std::set<Edge> &edges,
                typename std::enable_if<!is_tbb_enabled>::type* = 0) {

            for (std::size_t i = 0; i < trees.size(); i++) {
                trees[i].update_parities(edges);
            }

            std::tuple<std::set<Edge>, WeightType, bool> min;

            for (CandidateCycle<Graph, WeightMap> c : cycles) {
                std::tuple<std::set<Edge>, WeightType, bool> cc = candidate_cycle_builder(trees, c, edges,
                        std::get<2>(min), std::get<1>(min));

                if (std::get<2>(cc)) {
                    if (sorted_cycles) {
                        return cc;
                    }

                    if (!std::get<2>(min)) {
                        min = cc;
                    } else {
                        if (std::get<1>(cc) < std::get<1>(min)) {
                            min = cc;
                        }
                    }
                }
            }
            return min;
        }

        template<bool is_tbb_enabled = ParallelUsingTBB>
        std::tuple<std::set<Edge>, WeightType, bool> compute_shortest_odd_cycle(const std::set<Edge> &edges,
                typename std::enable_if<is_tbb_enabled>::type* = 0) {

            tbb::parallel_for(tbb::blocked_range<std::size_t>(0, trees.size()),
                    [&](const tbb::blocked_range<std::size_t> &r) {
                        for (std::size_t i = r.begin(); i != r.end(); ++i) {
                            trees[i].update_parities(edges);
                        }
                    });

            std::less<WeightType> compare = std::less<WeightType>();
            typedef std::tuple<std::set<Edge>, WeightType, bool> cycle_t;
            auto cycle_min = [compare](const cycle_t &c1, const cycle_t &c2) {
                if (!std::get<2>(c1) || !std::get<2>(c2)) {
                    if (std::get<2>(c1)) {
                        return c1;
                    } else {
                        return c2;
                    }
                }
                // both valid, compare
                if (!compare(std::get<1>(c2), std::get<1>(c1))) {
                    return c1;
                }
                return c2;
            };

            return tbb::parallel_reduce(tbb::blocked_range<std::size_t>(0, cycles.size()),
                    std::make_tuple(std::set<Edge>(), (std::numeric_limits<WeightType>::max)(), false),
                    [&](tbb::blocked_range<std::size_t> r, auto running_min) {
                        for (std::size_t i = r.begin(); i < r.end(); i++) {
                            auto c = cycles[i];
                            auto cc = candidate_cycle_builder(trees, c, edges, std::get<2>(running_min),
                                    std::get<1>(running_min));
                            if (std::get<2>(cc)) {
                                if (!std::get<2>(running_min) || compare(std::get<1>(cc), std::get<1>(running_min))) {
                                    running_min = cc;
                                }
                            }
                        }
                        return running_min;
                    },
                    cycle_min);
        }

        const Graph &g;
        const WeightMap &weight_map;
        const CandidateCycleBuilder<Graph, WeightMap> candidate_cycle_builder;
        std::vector<parmcb::SPTree<Graph, WeightMap>> &trees;
        std::vector<parmcb::CandidateCycle<Graph, WeightMap>> &cycles;
        bool sorted_cycles;
    };

} // parmcb

#endif