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


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/function/refine_rec.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/arch/x86/avx/simd/function/rec_raw.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::double_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a00) const BOOST_NOEXCEPT
    {
      A0 a0 = refine_rec(a00, refine_rec(a00,refine_rec(a00, raw_(rec)(a00))));
      #ifndef BOOST_SIMD_NO_INFINITIES
      a0 = if_else(is_inf(a00),
                      bitwise_and(a00, Mzero<A0>()),
                      a0
                     );
      #endif
      #ifndef BOOST_SIMD_NO_DENORMALS
      auto is_den = is_less(bs::abs(a00), Smallestposval<A0>());
      return if_else(is_den,  bitwise_or(bitofsign(a00), Inf<A0>()), a0);
      #else
      auto is_den = is_eqz(a00);
      return if_else(is_den,  bitwise_or(a00, Inf<A0>()), a0);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::single_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a00) const BOOST_NOEXCEPT
    {
      A0 a0 = refine_rec(a00,refine_rec(a00, raw_(rec)(a00)));
      #ifndef BOOST_SIMD_NO_INFINITIES
      a0 = if_else(is_inf(a00),
                      bitwise_and(a00, Mzero<A0>()),
                      a0
                     );
      #endif
      #ifndef BOOST_SIMD_NO_DENORMALS
      auto is_den = is_less(bs::abs(a00), Smallestposval<A0>());
      return if_else(is_den,  bitwise_or(bitofsign(a00), Inf<A0>()), a0);
      #else
      auto is_den = is_eqz(a00);
      return if_else(is_den,  bitwise_or(a00, Inf<A0>()), a0);
      #endif
    }
  };

} } }

#endif


//
//  MeshImporter.cpp
//  sketchup_converter
//
//  Created by jahsia on 11/8/18.
//  Copyright © 2018 trisetra. All rights reserved.
//
#include <Eigen/Dense>
#include "MeshImporter.hpp"
#include <fstream>
#include <iostream>

#define fmax std::numeric_limits<float>::max()
#define fmin std::numeric_limits<float>::min()
  ////////////// Mesh Operations //////////////
  void MeshImporter::add_positions(MeshSource* mesh, std::vector<float>&& src, std::vector<uint32_t>&& idx_buffer){
    mesh->pos = std::move(src);
    mesh->index = std::move(idx_buffer);
  }

  void MeshImporter::add_normals(MeshSource* mesh,std::vector<float>&& src, std::vector<uint32_t>&& ){
    mesh->normal = std::move(src);
  }

  void MeshImporter::add_tangent(MeshSource* mesh,std::vector<float>&& src, std::vector<uint32_t>&& ){

  }

  void MeshImporter::add_binormal(MeshSource* mesh,std::vector<float>&& src, std::vector<uint32_t>&& ){

  }

  //  idx of channel. 0,1,2,3 = RGBA, 0,1,2,-1 = RGB. specify -1 to ignore channel
  void MeshImporter::add_color(MeshSource* mesh,int r_idx, int g_idx, int b_idx, int a_idx, std::vector<float>&& src , std::vector<uint32_t>&& idx_buffer){

  }

  void MeshImporter::add_uv(MeshSource* mesh, uint32_t idx, std::vector<float>&& src, std::vector<uint32_t>&& ){
    mesh->uv = std::move(src);
  }

  // indicate number of vertcies per face
  void MeshImporter::add_face_descriptor(MeshSource* mesh,std::vector<uint8_t>&& vert_per_face){

  }
  // material_idx is used to access add_materials std::vector<std::shared_ptr<MaterialData>> material_data
  void MeshImporter::add_face_material_idx(MeshSource* mesh,std::vector<int32_t>&& material_idx){
    mesh->face_material_idx = std::move(material_idx);
  }

  ////////////// Node Operations //////////////
  void MeshImporter::add_tranform3x4(Node* node, std::vector<float>&& transform){
    node->matrix = Eigen::Matrix4f::Identity();
    std::vector<float> m = std::move(transform);
    node->matrix << m[0], m[1], m[2], 0.0f,
                    m[3], m[4], m[5], 0.0f,
                    m[6], m[7], m[8], 0.0f,
                    m[9], m[10],m[11], 1.0f;
  }

  void MeshImporter::add_mesh_to_node(Node* node, const MeshSource* MeshSource){
    node->mesh = MeshSource;
  }

  ////////////// Import Operations //////////////

  std::shared_ptr<Node>  MeshImporter::create_node(const Node* parent, const std::string& name){
    auto node = std::make_shared<Node>();
    node->parent = parent;
    _nodes.push_back(node);
    return _nodes.back();
  }
  std::shared_ptr<MeshSource> MeshImporter::create_mesh(const std::string& name){

    auto mesh = std::make_shared<MeshSource>();
    mesh->name = name;
    _mesh_sources.push_back(mesh);
    return _mesh_sources.back();
  }

  void MeshImporter::add_materials(const std::vector< std::shared_ptr<MaterialData> >& material_data){
    _materials.insert(_materials.begin(), material_data.begin(), material_data.end());
  }

  const std::shared_ptr<MaterialData> MeshImporter::get_material( int idx ){
    return _materials[idx];
  }

  int  MeshImporter::apply_material(MeshSource* mesh, const MaterialData* material){
    mesh->materials.push_back(material);
    return (int)(mesh->materials.size()-1);
  }

  void MeshImporter::add_mdl_path(const std::string& mdl_path){

  }

  void MeshImporter::add_texture_path(std::unordered_set<std::string>&& texts){
    _textures = std::move(texts);
  }

  void MeshImporter::serialize_to_file(const std::string& file_path, bool flattern, bool y_up, float rotatate_z){

    std::vector<Eigen::Vector3f> pos;
    std::vector<Eigen::Vector3f> normal;
    std::vector<float> uv;
    std::vector<uint32_t> indecies;

    Eigen::Matrix3f rot3f;
    rot3f = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) *
    Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
    Eigen::AngleAxisf(rotatate_z, Eigen::Vector3f::UnitZ());

    for(auto& node : _nodes){
      Eigen::Matrix4f temp = node->matrix.transpose();
      node->matrix = temp;
    }
    int node_c = 0;
    for(auto node : _nodes){
      if(node->parent){
        node->matrix = node->parent->matrix * node->matrix;
      }

      ++node_c;
      if(node->mesh){
        if(flattern){
          const Eigen::Vector3f* pos_src = (const Eigen::Vector3f*)node->mesh->pos.data();
          const Eigen::Vector3f* norm_src = (const Eigen::Vector3f*)node->mesh->normal.data();
          const int* index = (const int*)node->mesh->index.data();
          std::vector< Eigen::Vector3f> pos_temp(node->mesh->pos.size()/3);
          std::vector< Eigen::Vector3f> norm_temp(node->mesh->normal.size()/3);

          Eigen::Vector4f temp;
          Eigen::Vector3f ntemp;
          for(size_t i = 0; i < node->mesh->pos.size()/3; ++i){

            temp = Eigen::Vector4f::Ones();
            temp.head<3>() = pos_src[i];
            pos_temp[i] = (node->matrix*temp).head<3>();

            ntemp = norm_src[i];
            auto mat_3x3 = node->matrix.block<3,3>(0,0);
            ntemp = mat_3x3*ntemp;
            ntemp.normalize();
            norm_temp[i] = ntemp;
          }

          int offset = pos.size();
          pos.insert( pos.end(), pos_temp.begin(), pos_temp.end() );
          normal.insert( normal.end(),norm_temp.begin(), norm_temp.end() );
          uv.insert(uv.end(), node->mesh->uv.begin(), node->mesh->uv.end() );

          std::vector<uint32_t> index_temp = node->mesh->index;
          for(size_t i = 0; i < node->mesh->index.size(); ++i){
            index_temp[i] += offset;
          }
          indecies.insert(indecies.end(),index_temp.begin(), index_temp.end());

        }
      }
    }

    Eigen::Vector3f max = Eigen::Vector3f(fmin,fmin,fmin);
    Eigen::Vector3f min = Eigen::Vector3f(fmax,fmax,fmax);

    if(flattern){
      size_t vert_size = pos.size();
      Eigen::Vector3f mid_point = Eigen::Vector3f::Zero();

      for( Eigen::Vector3f& a_pos : pos ){
        a_pos = rot3f*a_pos;
      }
      for( Eigen::Vector3f& a_norm : normal ){
        a_norm = rot3f*a_norm;
      }

      for( const Eigen::Vector3f& a_pos : pos ){
        mid_point += a_pos;
      }
      mid_point = mid_point/(float)vert_size;
      if(!y_up)
        mid_point.z() = 0.0f;
      else
        mid_point.y() = 0.0f;

      for(  Eigen::Vector3f& a_pos : pos ){
        a_pos = a_pos - mid_point;

        max.x() = std::max(max.x(),a_pos.x());
        max.y() = std::max(max.y(),a_pos.y());
        max.z() = std::max(max.z(),a_pos.z());
        min.x() = std::min(min.x(),a_pos.x());
        min.y() = std::min(min.y(),a_pos.y());
        min.z() = std::min(min.z(),a_pos.z());
      }
      Eigen::Vector3f diff = max - min;
      float length = diff.norm();

      for(  Eigen::Vector3f& a_pos : pos ){
        a_pos = a_pos/length;
      }

      std::cout<< "max:" << max/length <<std::endl;
      std::cout<< "min:" << min/length <<std::endl;
    }

    std::ofstream outfile;
    outfile.open (file_path, std::ios::out | std::ios::trunc | std::ios::binary);
    outfile << 'T' << 'R' << 'I' << 'S';
    uint32_t uint_val = (uint32_t)(pos.size()*8);
    outfile.write((char*)(&uint_val), sizeof(uint32_t));

    for(size_t i = 0; i < pos.size(); ++i){
      outfile.write((char*)(&pos[i]), sizeof(Eigen::Vector3f));
      outfile.write((char*)(&normal[i]), sizeof(Eigen::Vector3f));
      outfile.write((char*)(&uv[i*2]), 2*sizeof(float));

    }

    uint_val = (uint32_t)indecies.size();
    outfile.write((char*)(&uint_val), sizeof(uint32_t));
    outfile.write((char*)indecies.data(), indecies.size()*sizeof(uint32_t));
    outfile.close();

    // write to PLY

    size_t lastindex = file_path.find_last_of(".");
    std::string rawname = file_path.substr(0, lastindex);
    std::string plyname = rawname + ".ply";

    outfile.open (plyname, std::ios::out | std::ios::trunc );
    outfile<<"ply"<<std::endl;
    outfile<<"format ascii 1.0"<<std::endl;
    outfile<<"element vertex "<< pos.size() <<std::endl;
    outfile<<"property float x" << std::endl;
    outfile<<"property float y" << std::endl;
    outfile<<"property float z" << std::endl;

    outfile<<"element face " << uint_val/3 <<std::endl;
    outfile<<"property list uchar int vertex_indices"<<std::endl;
    outfile<<"end_header" << std::endl;

    for(size_t i = 0; i < pos.size(); ++i){
      outfile<< pos[i].x() <<" "<< pos[i].y()<< " " << pos[i].z() << std::endl;
    }

    for(size_t i = 0; i < indecies.size()/3; ++i){
      outfile<< "3" <<" "<<indecies[i*3] <<" "<< indecies[i*3+1] << " " << indecies[i*3+2] << std::endl;
    }

    outfile.close();
  }


#include <ndt_registration/ndt_matcher_d2d_sc.h>
#include <ndt_registration/icp_matcher_p2p.h>
#include <ndt_generic/motion_model_3d.h>
#include <ndt_generic/io.h>
#include <ndt_map/pointcloud_utils.h>
#include <boost/program_options.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <ndt_generic/gnuplot-iostream.h>
#include <ndt_evaluation/registration_score_eval.h>

using namespace std;
namespace po = boost::program_options;

int main(int argc, char** argv)
{
  cout << "--------------------------------------------------" << endl;
  cout << "Creates a set of gnuplot files to visualize " << endl;
  cout << "different scores of matching functions" << std::endl;
  cout << "--------------------------------------------------" << endl;

  po::options_description desc("Allowed options");
  std::string gt_file;
  std::string odom_file;
  std::string base_name_pcd;
  Eigen::Vector3d transl;
  Eigen::Vector3d euler;
  double resolution;
  int idx1, idx2;
  int dimidx1, dimidx2;
  int offset_size;
  double incr_dist;
  double incr_ang;
  std::string out_file;
  // Simply to make it transparant to the fuser node.
  perception_oru::MotionModel2d::Params motion_params;
  int iter_step;
  int iters;
  std::string global_ndtmap_file;
  int node_idx;
  double mx,my;

  desc.add_options()
      ("help", "produce help message")
      ("gt_file", po::value<std::string>(&gt_file), "vehicle pose files in world frame")
      ("odom_file", po::value<std::string>(&odom_file)->default_value(std::string("")), "estimated sensor poses (from egomotion) to be used in SC in world frame")
      ("base_name_pcd", po::value<string>(&base_name_pcd)->default_value(std::string("")), "prefix for the .pcd files")
      ("out_file", po::value<string>(&out_file)->default_value(std::string("scores")), "prefix for the output files")
      ("idx1", po::value<int>(&idx1)->default_value(0), "'fixed' index of pose/pointcloud to be used")
      ("idx2", po::value<int>(&idx2)->default_value(1), "'moving' index of pose/pointcloud to be used")
      ("dimidx1", po::value<int>(&dimidx1)->default_value(0), "dimension index_x (what to plot on the X axis in gnuplot - 0,1,2,3,4,5 -> x,y,z,roll,pitch,yaw)")
      ("dimidx2", po::value<int>(&dimidx2)->default_value(1), "dimension index_y (what to plot on the Y axis in gnuplot - 0,1,2,3,4,5 -> x,y,z,roll,pitch,yaw)")

      ("x", po::value<double>(&transl[0])->default_value(0.), "sensor pose - translation vector x")
      ("y", po::value<double>(&transl[1])->default_value(0.), "sensor pose - translation vector y")
      ("z", po::value<double>(&transl[2])->default_value(0.), "sensor pose - translation vector z")
      ("ex", po::value<double>(&euler[0])->default_value(0.), "sensor pose - euler angle vector x")
      ("ey", po::value<double>(&euler[1])->default_value(0.), "sensor pose - euler angle vector y")
      ("ez", po::value<double>(&euler[2])->default_value(0.), "sensor pose - euler angle vector z")
      ("resolution", po::value<double>(&resolution)->default_value(1.), "resolution of the map")
      ("offset_size", po::value<int>(&offset_size)->default_value(100), "2*number of evaluated points per dimension")
      ("incr_dist", po::value<double>(&incr_dist)->default_value(0.01), "incremental steps for distance directions (x,y,z)")
      ("incr_ang", po::value<double>(&incr_ang)->default_value(0.002), "incremental steps for angular directions (R,P,Y)")

      ("Dd", po::value<double>(&motion_params.Dd)->default_value(1.), "forward uncertainty on distance traveled")
      ("Dt", po::value<double>(&motion_params.Dt)->default_value(1.), "forward uncertainty on rotation")
      ("Cd", po::value<double>(&motion_params.Cd)->default_value(1.), "side uncertainty on distance traveled")
      ("Ct", po::value<double>(&motion_params.Ct)->default_value(1.), "side uncertainty on rotation")
      ("Td", po::value<double>(&motion_params.Td)->default_value(1.), "rotation uncertainty on distance traveled")
      ("Tt", po::value<double>(&motion_params.Tt)->default_value(1.), "rotation uncertainty on rotation")
      ("use_score_d2d_sc", "if the d2d sc score should be used in the objective plot")
      ("save_eps", "if .eps should be generated")
      ("save_pcd", "if .pcd point cloud should be generated gt, odom, d2d, d2d_sc...")
      ("save_global_Ts", "if global transforms on registrations odom, etc. should be generate, for evaluation with ate.py, rpe.py")
      ("iter_all_poses", "iterate over all available poses")
      ("iter_step", po::value<int>(&iter_step)->default_value(1), "iter step size")
      ("iters", po::value<int>(&iters)->default_value(1), "if additional iters should be evaluated (idx1 + iter, idx2 + iter)")
      ("gnuplot", "if gnuplot should be invoked")
      ("draw_icp", "if icp results should be plotted")
      ("use_d2d_in_grid", "plot the d2d estimate in (0,0)")
      ("use_d2d_sc_in_grid", "plot the d2d_sc estimate in (0,0)")
      ("global_ndtmap_file", po::value<std::string>(&global_ndtmap_file)->default_value(""), "global map name to be registred towards")
      ("node_idx", po::value<int>(&node_idx)->default_value(0), "node index to be used in a graph map")
      ("mx", po::value<double>(&transl[0])->default_value(0.), "global map offset in x")
      ("my", po::value<double>(&transl[1])->default_value(0.), "global map offset in y")

      ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help"))
  {
    cout << desc << "\n";
    return 1;
  }
  if (odom_file.empty()) {
    cout << "odom_file not specified" << endl;
    return 1;
  }

  bool use_score_d2d_sc = vm.count("use_score_d2d_sc");
  bool save_eps = vm.count("save_eps");
  bool save_pcd = vm.count("save_pcd");
  bool save_global_Ts = vm.count("save_global_Ts");
  bool iter_all_poses = vm.count("iter_all_poses");
  bool use_gnuplot = vm.count("gnuplot");
  bool draw_icp = vm.count("skip_icp");
  bool use_d2d_in_grid = vm.count("use_d2d_in_grid");
  bool use_d2d_sc_in_grid = vm.count("use_d2d_sc_in_grid");
  bool to_global_map = !global_ndtmap_file.empty();

  if (use_d2d_sc_in_grid) {
    use_score_d2d_sc = true;
  }

  Eigen::Affine3d sensor_pose = ndt_generic::vectorsToAffine3d(transl,euler);
  std::cout << "Sensor pose used : " << ndt_generic::affine3dToStringRPY(sensor_pose) << std::endl;

  perception_oru::MotionModel3d::Params motion_params3d(motion_params);

  std::cout << "Motion params used : " << motion_params3d << std::endl;

  RegistrationScoreEval se(gt_file, odom_file, base_name_pcd, sensor_pose, motion_params);
  se.resolution = resolution;
  se.offset_size = offset_size;
  se.incr_dist = incr_dist;
  se.incr_ang = incr_ang;
  se.use_d2d_in_grid = use_d2d_in_grid;
  se.use_d2d_sc_in_grid = use_d2d_sc_in_grid;

  if (to_global_map) {
    // Should be using the graph map representation?
    Eigen::Affine3d map_pose;
    map_pose.setIdentity();
    map_pose.translation()[0] = mx;
    map_pose.translation()[1] = my;
    se.setGlobalMap(global_ndtmap_file, map_pose);
  }

  if (iter_all_poses) {
    iters = se.nbPoses();
  }

  if (iters > se.nbPoses()) {
    std::cerr << "[iters too large] # poses : " << se.nbPoses() << std::endl;
    exit(-1);
  }

  out_file +=  ".res" + ndt_generic::toString(resolution) + "Dd" + ndt_generic::toString(motion_params.Dd) + "Dt" + ndt_generic::toString(motion_params.Dt);

  int i = 0;
  while (i < iters) {
    int _idx1 = idx1 + i;
    int _idx2 = idx2 + i;
    std::cout << "computing scores" << std::endl;
    se.computeScoreSets(_idx1, _idx2, dimidx1, dimidx2);

    std::cout << "saving : " << out_file << std::endl;
    se.save2D(out_file + ".dat", dimidx1, dimidx2);
    se.savePoseEstInOdomFrame(out_file + ".T");

    if (save_pcd) {
      std::string out_file_pcd = out_file + ndt_generic::toString(_idx1) + "_" + ndt_generic::toString(_idx2);
      se.savePCD(out_file_pcd);
    }

    if (use_gnuplot) {
      // Show it using gnuplot
      Gnuplot gp;//(std::fopen("output.gnuplot", "wb"));

      if (save_eps) {
        std::string out_file_eps = out_file + ndt_generic::toString(_idx1) + "_" + ndt_generic::toString(_idx2) + "x" + ndt_generic::toString(dimidx1) + "y" + ndt_generic::toString(dimidx2) + "sc" + ndt_generic::toString(use_score_d2d_sc) + ".eps";
        std::cout << "saving : " << out_file_eps << std::endl;
        gp << "set terminal postscript eps size 3.5,2.62 enhanced color font 'Helvetica,12' lw 1\n";
        gp << "set output '" << out_file_eps << "'\n";
      }
      std::string title = "Objective score value ";
      if (use_score_d2d_sc) {
        title += "with SC";
      }
      else {
        title += "without SC";
      }
      title += " [" + ndt_generic::toString(dimidx1) + "," + ndt_generic::toString(dimidx2) + "]";
      title += " res: " + ndt_generic::toString(resolution);
      if (use_score_d2d_sc)  {
        title += " Dd: " + ndt_generic::toString(motion_params.Dd);
      }
      gp << "set title \"" << title << "\"\n";
      gp << "set key top right\n";
      gp << "set pm3d map\n";

      if (!draw_icp) {
        gp << "splot '-' with pm3d notitle, '-' with points pt 1 title 'GT',  '-' with points pt 4 title 'odom', '-' with points pt 2 title 'd2d', '-' with points pt 3 title 'd2d sc', '-' with points pt 2 title 'filter'\n";
        gp.send2d( se.getScoreSegments(dimidx1, dimidx2, use_score_d2d_sc));
        gp.send1d( se.getRelPoseGT(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseOdom(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseD2D(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseD2D_SC(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseFilter(dimidx1, dimidx2));
      }
      else {
        gp << "splot '-' with pm3d notitle, '-' with points pt 1 title 'GT', '-' with points pt 2 title 'd2d', '-' with points pt 3 title 'd2d sc', '-' with points pt 4 title 'icp', '-' with points pt 2 title 'filter', '-' with points pt 4 title 'icp filter'\n";
        gp.send2d( se.getScoreSegments(dimidx1, dimidx2, use_score_d2d_sc));
        gp.send1d( se.getRelPoseGT(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseD2D(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseD2D_SC(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseICP(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseFilter(dimidx1, dimidx2));
        gp.send1d( se.getRelPoseICPFilter(dimidx1, dimidx2));
      }
    }
    i += iter_step;
  }

  if (save_global_Ts && iters > 1) {
    // Saves all transforms
    std::string out_file_Ts = out_file + ".Ts";
    se.saveTsToEvalFiles(out_file_Ts);
    se.saveComputationTime(out_file + ".time");

  }
  std::cout << "done." << std::endl;
}


#include "ros/ros.h"
#include <boost/bind.hpp>
#include <cmath>
#include <nav_msgs/Odometry.h>
#include <o_msgs/Monitor.h>
#include <o_msgs/RoboClawStatus.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <nav_msgs/OccupancyGrid.h>

// TODO: Take distance arg and return vector of (x,y,angle) tuples of points within that distance in costmap

typedef enum LIDAR_SOURCE {
	NEATO,
	D435_LEFT,
	D435_right,
	NUMBER_LIDAR_SOURCES
} SCAN_SOURCE;

float merged_scans[360][NUMBER_LIDAR_SOURCES];

    // float lowestDist = 1e8;
    // int left_count = 0;
    // int right_count = 0;
    // bool obstacle_left = false;
    // bool obstacle_center = false;
    // bool obstacle_right = false;
    // geometry_msgs::Point32 left_point;
    // geometry_msgs::Point32 center_point;
    // geometry_msgs::Point32 right_point;
    // left_point.x = left_point.y = left_point.z = 0.0;
    // center_point.x = center_point.y = center_point.z = 0.0;
    // right_point.x = right_point.y = right_point.z = 0.0;

    // visualization_msgs::Marker marker;
    // marker.header.frame_id = "scan";
    // marker.header.stamp = ros::Time::now();
    // marker.ns = "closestNamespace";
    // marker.id = 0;
    // marker.type = shape;
    // marker.action = visualization_msgs::Marker::DELETEALL;
    // marker.lifetime = ros::Duration();
    // marker_pub.publish(marker);

    // float angle;
    // int i;
    // for (i = 0, angle = laser_msg->angle_min;
    //      i < laser_msg->ranges.size();
    //      ++i, angle += laser_msg->angle_increment) {
    //     float range = laser_msg->ranges[i];
    //     if ((range < laser_msg->range_min) || (range > laser_msg->range_max)) continue;

    //     if (range < lowestDist) {
    //         lowestDist = range;
    //     }

    //     if (range < kHAZARD_DISTANCE) {
    //         geometry_msgs::Point32 marker_point;
    //         marker_point.x = cos(angle) * range;
    //         marker_point.y = sin(angle) * range;
    //         marker_point.z = 0;
    //         ROS_INFO("angle: %fr (%fd), range: %f, index: %d, x: %f, y: %f",
    //                  angle,
    //                  angles::to_degrees(angle),
    //                  range,
    //                  i,
    //                  marker_point.x,
    //                  marker_point.y);
    //         addMarker(marker_point, i);
    //     }
    // }
void laserCallback(const sensor_msgs::LaserScanConstPtr& laser_msg) {
}

inline int mapIndex(const nav_msgs::OccupancyGrid::ConstPtr& costmap_msg, int i, int j) {
    int result = i + (j * costmap_msg->info.width);
    return result;
}

void costmapCallback(const nav_msgs::OccupancyGrid::ConstPtr& costmap_msg) {
	ROS_INFO("[roam_node] width: %d"
			 ", height: %d"
			 ", resolution: %7.4f"
			 ", origin: [%7.4f, %7.4f, %7.4f]",
		 	 costmap_msg->info.width,
			 costmap_msg->info.height,
			 costmap_msg->info.resolution,
			 costmap_msg->info.origin.position.x,
			 costmap_msg->info.origin.position.y,
			 costmap_msg->info.origin.position.z);

    std::size_t d_x = -10000;
    std::size_t d_y = -10000;
    double min_dist = 10000;
    geometry_msgs::PoseStamped new_pt;
    geometry_msgs::PoseStamped pt;

    pt.header.frame_id = "base_link";
    pt.pose.position.x = 0;
    pt.pose.position.y = 0;
    pt.pose.position.z = 0;
    pt.pose.orientation.w = 1;
    pt.pose.orientation.x = 0;
    pt.pose.orientation.y = 0;
    pt.pose.orientation.z = 0;

    tf::TransformListener tf_l(ros::Duration(10));

    try {
        tf_l.waitForTransform(costmap_msg->header.frame_id,
                              pt.header.frame_id,
                              ros::Time(0),
                              ros::Duration(1));
        tf_l.transformPose(costmap_msg->header.frame_id, pt, new_pt);
    } catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
    }

    double pt_x = new_pt.pose.position.x;
    double pt_y = new_pt.pose.position.y;
    double pt_th = tf::getYaw(new_pt.pose.orientation);
    std::size_t i_midpoint = costmap_msg->info.width / 2;
    std::size_t j_midpoint = costmap_msg->info.height / 2;

    // getting minimum distance
    for (std::size_t j = 0; j < costmap_msg->info.height; j++) {
        for (std::size_t i = 0; i < costmap_msg->info.width; i++) {
            double cost = costmap_msg->data[mapIndex(costmap_msg, i, j)];
            if(cost == 100) {
                // convert to world position
                double d2_sq = ((i - i_midpoint) * (i - i_midpoint)) + ((j - j_midpoint) * (j - j_midpoint));
                double d2_dist = sqrt(d2_sq * 1.0) * costmap_msg->info.resolution;
                // if (d2_dist < 1.0) {
                //     ROS_INFO("[roam_node] i: %ld, j: %ld"
                //              ", d2_sq: %7.4f, d2_dist: %7.4f"
                //              ", old min_dist: %7.4f"
                //              ", new min: %s"
                //              , i, j
                //              , d2_sq, d2_dist
                //              , min_dist
                //              , (d2_dist < min_dist) ? "TRUE" : "false"
                //     );
                // }
                if (d2_dist < min_dist) {
                    min_dist = d2_dist;
                    d_x = i;
                    d_y = j;
                }
            }
        }
    }

    double angle = std::atan2(d_y - (j_midpoint * 1.0), d_x - (i_midpoint * 1.0));

    ROS_INFO("roam_node] min_dist: %7.4f"
             ", at x: %ld (%7.4f)"
             ", y: %ld (%7.4f)"
             ", angle: %7.4f"
             ", j_midpoint %ld, j_midpoint: %ld"
             ", d_y - j_midpoint: %7.4f, d_x - i_midpoint: %7.4f"
             , min_dist
             , d_x, (d_x * costmap_msg->info.resolution) + costmap_msg->info.origin.position.x
             , d_y, (d_y * costmap_msg->info.resolution) + costmap_msg->info.origin.position.y
             , angle
             , j_midpoint, i_midpoint
             , d_y - (j_midpoint * 1.0), d_x - (i_midpoint * 1.0)
    );
    for (std::size_t jx = costmap_msg->info.height; jx > 0 ; jx--) {
        char line[costmap_msg->info.width];
        for (std::size_t ix = 0; ix < costmap_msg->info.width; ix++) {
            double cost = costmap_msg->data[mapIndex(costmap_msg, ix, jx - 1)];
            line[ix] = (cost == 100) ? '*' : '.';
        }

        if (jx == j_midpoint) line[i_midpoint] = '@';
        if (jx == d_y) line[d_x] = 'X';
        ROS_INFO("%c %3ld: %s", (jx == d_y)? '>' : ' ', jx, line);
    }
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "roam_node");
	ros::NodeHandle nh;

    ros::Subscriber sub_d435_left = nh.subscribe<sensor_msgs::LaserScan>("/plaser_scan_left", 1, laserCallback);
    ros::Subscriber sub_d435_right = nh.subscribe<sensor_msgs::LaserScan>("plaser_scan_right", 1, laserCallback);
    ros::Subscriber sub_neato = nh.subscribe<sensor_msgs::LaserScan>("/o/scan", 1, laserCallback);

	ros::Subscriber foo = nh.subscribe<nav_msgs::OccupancyGrid>("/move_base/local_costmap/costmap", 1, costmapCallback);

 	ROS_INFO("[roam_node] starting loop");
    ros::Rate r(10);
	while (ros::ok()) {
        ros::spinOnce();
        r.sleep();
	}

	ROS_INFO("[roam_node] shutdown");
}


#pragma once

#include <boost/multiprecision/cpp_int.hpp>

namespace futurehead
{
using uint128_t = boost::multiprecision::uint128_t;
using uint256_t = boost::multiprecision::uint256_t;
using uint512_t = boost::multiprecision::uint512_t;
// SI dividers
//TO-CHANGE
futurehead::uint128_t const Gxrb_ratio = futurehead::uint128_t ("1000000000000000000000000000000000"); // 10^33
futurehead::uint128_t const Mxrb_ratio = futurehead::uint128_t ("1000000000000000000000000000000"); // 10^30
futurehead::uint128_t const kxrb_ratio = futurehead::uint128_t ("1000000000000000000000000000"); // 10^27
futurehead::uint128_t const xrb_ratio = futurehead::uint128_t ("1000000000000000000000000"); // 10^24
futurehead::uint128_t const raw_ratio = futurehead::uint128_t ("1"); // 10^0

class uint128_union
{
public:
	uint128_union () = default;
	/**
	 * Decode from hex string
	 * @warning Aborts at runtime if the input is invalid
	 */
	uint128_union (std::string const &);
	uint128_union (uint64_t);
	uint128_union (futurehead::uint128_t const &);
	bool operator== (futurehead::uint128_union const &) const;
	bool operator!= (futurehead::uint128_union const &) const;
	bool operator< (futurehead::uint128_union const &) const;
	bool operator> (futurehead::uint128_union const &) const;
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	void encode_dec (std::string &) const;
	bool decode_dec (std::string const &, bool = false);
	bool decode_dec (std::string const &, futurehead::uint128_t);
	std::string format_balance (futurehead::uint128_t scale, int precision, bool group_digits) const;
	std::string format_balance (futurehead::uint128_t scale, int precision, bool group_digits, const std::locale & locale) const;
	futurehead::uint128_t number () const;
	void clear ();
	bool is_zero () const;
	std::string to_string () const;
	std::string to_string_dec () const;
	union
	{
		std::array<uint8_t, 16> bytes;
		std::array<char, 16> chars;
		std::array<uint32_t, 4> dwords;
		std::array<uint64_t, 2> qwords;
	};
};
static_assert (std::is_nothrow_move_constructible<uint128_union>::value, "uint128_union should be noexcept MoveConstructible");

// Balances are 128 bit.
class amount : public uint128_union
{
public:
	using uint128_union::uint128_union;
};
class raw_key;
class uint256_union
{
public:
	uint256_union () = default;
	/**
	 * Decode from hex string
	 * @warning Aborts at runtime if the input is invalid
	 */
	uint256_union (std::string const &);
	uint256_union (uint64_t);
	uint256_union (futurehead::uint256_t const &);
	void encrypt (futurehead::raw_key const &, futurehead::raw_key const &, uint128_union const &);
	uint256_union & operator^= (futurehead::uint256_union const &);
	uint256_union operator^ (futurehead::uint256_union const &) const;
	bool operator== (futurehead::uint256_union const &) const;
	bool operator!= (futurehead::uint256_union const &) const;
	bool operator< (futurehead::uint256_union const &) const;
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	void encode_dec (std::string &) const;
	bool decode_dec (std::string const &);

	void clear ();
	bool is_zero () const;
	std::string to_string () const;
	futurehead::uint256_t number () const;

	union
	{
		std::array<uint8_t, 32> bytes;
		std::array<char, 32> chars;
		std::array<uint32_t, 8> dwords;
		std::array<uint64_t, 4> qwords;
		std::array<uint128_union, 2> owords;
	};
};
static_assert (std::is_nothrow_move_constructible<uint256_union>::value, "uint256_union should be noexcept MoveConstructible");

class link;
class root;
class hash_or_account;

// All keys and hashes are 256 bit.
class block_hash final : public uint256_union
{
public:
	using uint256_union::uint256_union;
	operator futurehead::link const & () const;
	operator futurehead::root const & () const;
	operator futurehead::hash_or_account const & () const;
};

class public_key final : public uint256_union
{
public:
	using uint256_union::uint256_union;

	std::string to_node_id () const;
	bool decode_node_id (std::string const & source_a);
	void encode_account (std::string &) const;
	std::string to_account () const;
	bool decode_account (std::string const &);

	operator futurehead::link const & () const;
	operator futurehead::root const & () const;
	operator futurehead::hash_or_account const & () const;
};

class wallet_id : public uint256_union
{
	using uint256_union::uint256_union;
};

// These are synonymous
using account = public_key;

class hash_or_account
{
public:
	hash_or_account () = default;
	hash_or_account (uint64_t value_a);

	bool is_zero () const;
	void clear ();
	std::string to_string () const;
	bool decode_hex (std::string const &);
	bool decode_account (std::string const &);
	std::string to_account () const;

	operator futurehead::block_hash const & () const;
	operator futurehead::account const & () const;
	operator futurehead::uint256_union const & () const;

	bool operator== (futurehead::hash_or_account const &) const;
	bool operator!= (futurehead::hash_or_account const &) const;

	union
	{
		std::array<uint8_t, 32> bytes;
		futurehead::uint256_union raw; // This can be used when you don't want to explicitly mention either of the types
		futurehead::account account;
		futurehead::block_hash hash;
	};
};

// A link can either be a destination account or source hash
class link final : public hash_or_account
{
public:
	using hash_or_account::hash_or_account;
};

// A root can either be an open block hash or a previous hash
class root final : public hash_or_account
{
public:
	using hash_or_account::hash_or_account;

	futurehead::block_hash const & previous () const;
};

class private_key : public uint256_union
{
public:
	using uint256_union::uint256_union;
};

// The seed or private key
class raw_key final
{
public:
	~raw_key ();
	void decrypt (futurehead::uint256_union const &, futurehead::raw_key const &, uint128_union const &);
	bool operator== (futurehead::raw_key const &) const;
	bool operator!= (futurehead::raw_key const &) const;
	futurehead::private_key const & as_private_key () const;
	futurehead::uint256_union data;
};
class uint512_union
{
public:
	uint512_union () = default;
	uint512_union (futurehead::uint256_union const &, futurehead::uint256_union const &);
	uint512_union (futurehead::uint512_t const &);
	bool operator== (futurehead::uint512_union const &) const;
	bool operator!= (futurehead::uint512_union const &) const;
	futurehead::uint512_union & operator^= (futurehead::uint512_union const &);
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	void clear ();
	bool is_zero () const;
	futurehead::uint512_t number () const;
	std::string to_string () const;

	union
	{
		std::array<uint8_t, 64> bytes;
		std::array<uint32_t, 16> dwords;
		std::array<uint64_t, 8> qwords;
		std::array<uint256_union, 2> uint256s;
	};
};
static_assert (std::is_nothrow_move_constructible<uint512_union>::value, "uint512_union should be noexcept MoveConstructible");

class signature : public uint512_union
{
public:
	using uint512_union::uint512_union;
};

class qualified_root : public uint512_union
{
public:
	using uint512_union::uint512_union;

	futurehead::block_hash const & previous () const
	{
		return reinterpret_cast<futurehead::block_hash const &> (uint256s[0]);
	}
	futurehead::root const & root () const
	{
		return reinterpret_cast<futurehead::root const &> (uint256s[1]);
	}
};

futurehead::signature sign_message (futurehead::raw_key const &, futurehead::public_key const &, futurehead::uint256_union const &);
futurehead::signature sign_message (futurehead::raw_key const &, futurehead::public_key const &, uint8_t const *, size_t);
bool validate_message (futurehead::public_key const &, futurehead::uint256_union const &, futurehead::signature const &);
bool validate_message (futurehead::public_key const &, uint8_t const *, size_t, futurehead::signature const &);
bool validate_message_batch (unsigned const char **, size_t *, unsigned const char **, unsigned const char **, size_t, int *);
futurehead::private_key deterministic_key (futurehead::raw_key const &, uint32_t);
futurehead::public_key pub_key (futurehead::private_key const &);

/* Conversion methods */
std::string to_string_hex (uint64_t const);
bool from_string_hex (std::string const &, uint64_t &);

/**
 * Convert a double to string in fixed format
 * @param precision_a (optional) use a specific precision (default is the maximum)
 */
std::string to_string (double const, int const precision_a = std::numeric_limits<double>::digits10);

namespace difficulty
{
	uint64_t from_multiplier (double const, uint64_t const);
	double to_multiplier (uint64_t const, uint64_t const);
}
}

namespace std
{
template <>
struct hash<::futurehead::uint256_union>
{
	size_t operator() (::futurehead::uint256_union const & data_a) const
	{
		return data_a.qwords[0] + data_a.qwords[1] + data_a.qwords[2] + data_a.qwords[3];
	}
};
template <>
struct hash<::futurehead::account>
{
	size_t operator() (::futurehead::account const & data_a) const
	{
		return hash<::futurehead::uint256_union> () (data_a);
	}
};
template <>
struct hash<::futurehead::block_hash>
{
	size_t operator() (::futurehead::block_hash const & data_a) const
	{
		return hash<::futurehead::uint256_union> () (data_a);
	}
};
template <>
struct hash<::futurehead::private_key>
{
	size_t operator() (::futurehead::private_key const & data_a) const
	{
		return hash<::futurehead::uint256_union> () (data_a);
	}
};
template <>
struct hash<::futurehead::root>
{
	size_t operator() (::futurehead::root const & data_a) const
	{
		return hash<::futurehead::uint256_union> () (data_a);
	}
};
template <>
struct hash<::futurehead::wallet_id>
{
	size_t operator() (::futurehead::wallet_id const & data_a) const
	{
		return hash<::futurehead::uint256_union> () (data_a);
	}
};
template <>
struct hash<::futurehead::uint256_t>
{
	size_t operator() (::futurehead::uint256_t const & number_a) const
	{
		return number_a.convert_to<size_t> ();
	}
};
template <>
struct hash<::futurehead::uint512_union>
{
	size_t operator() (::futurehead::uint512_union const & data_a) const
	{
		return hash<::futurehead::uint256_union> () (data_a.uint256s[0]) + hash<::futurehead::uint256_union> () (data_a.uint256s[1]);
	}
};
template <>
struct hash<::futurehead::qualified_root>
{
	size_t operator() (::futurehead::qualified_root const & data_a) const
	{
		return hash<::futurehead::uint512_union> () (data_a);
	}
};
}