#include <boost/program_options.hpp>
#include <frovedis.hpp>
#include <frovedis/matrix/standard_scaler.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

template <class T>
void do_scale(const string& input, const string& output,
              bool with_mean, bool with_std, bool sample_stddev,
              bool binary) {
  rowmajor_matrix<T> mat;
  time_spent t(DEBUG);
  if (binary) {
    mat = make_rowmajor_matrix_loadbinary<T>(input);
  } else {
    mat = make_rowmajor_matrix_load<T>(input);
  }
  t.show("load matrix: ");
  auto ss = frovedis::standard_scaler<T>(with_mean, with_std, sample_stddev);
  ss.fit(mat);
  t.show("fit matrix: ");
  auto scaled_matrix = ss.transform(mat);
  t.show("transform matrix: ");
  binary ? scaled_matrix.savebinary(output) : scaled_matrix.save(output);
  t.show("save scaled matrix: ");
}

int main(int argc, char* argv[]) {
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  // clang-format off
  opt.add_options()("help,h", "print help")
    ("input,i", value<string>(),"input matrix")
    ("output,o", value<string>(), "output scaled matrix")
    ("double","for double type input (default)")
    ("binary,b", "use binary input/output");
  // clang-format on
  variables_map argmap;
  store(command_line_parser(argc, argv).options(opt).allow_unregistered().run(),
        argmap);
  notify(argmap);

  string input, output;
  bool with_mean = true;
  bool with_std = true;
  bool sample_stddev = false;
  bool binary = false;

  if (argmap.count("help")) {
    cerr << opt << endl;
    exit(1);
  }

  if (argmap.count("input")) {
    input = argmap["input"].as<string>();
  } else {
    cerr << "input is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if (argmap.count("output")) {
    output = argmap["output"].as<string>();
  } else {
    cerr << "output is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("double"))
    do_scale<double>(input, output, with_mean, with_std, sample_stddev, binary);
  else
    do_scale<float>(input, output, with_mean, with_std, sample_stddev, binary);
}


//
//  Copyright Toon Knapen and Kresimir Fresl 2003
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_BINDINGS_BLAS_BLAS2_HPP
#define BOOST_BINDINGS_BLAS_BLAS2_HPP

#include <boost/numeric/bindings/blas/blas2_overloads.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/transpose.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits.hpp>
#include <cassert>

namespace boost { namespace numeric { namespace bindings { namespace blas {

  // y <- alpha * op (A) * x + beta * y
  // op (A) == A || A^T || A^H
  // ! CAUTION this function assumes that all matrices involved are column-major matrices
  template < typename matrix_type, typename vector_type_x, typename vector_type_y, typename value_type >
  void gemv(const char TRANS,
            const value_type& alpha,
            const matrix_type &a,
            const vector_type_x &x,
            const value_type& beta,
            vector_type_y &y
            )
  {
    // precondition: matrix_type must be dense or dense_proxy
    /* not all compilers can handle the traits
    BOOST_STATIC_ASSERT( ( boost::is_same< typename mtraits::matrix_structure,
                                           boost::numeric::bindings::traits::general_t
                           >::value ) ) ;
    */

    const integer_t m = traits::matrix_size1( a ) ;
    const integer_t n = traits::matrix_size2( a ) ;
    assert ( traits::vector_size( x ) >= (TRANS == traits::NO_TRANSPOSE ? n : m) ) ;
    assert ( traits::vector_size( y ) >= (TRANS == traits::NO_TRANSPOSE ? m : n) ) ;
    const integer_t lda = traits::leading_dimension( a ) ;
    const integer_t stride_x = traits::vector_stride( x ) ;
    const integer_t stride_y = traits::vector_stride( y ) ;

    const value_type *a_ptr = traits::matrix_storage( a ) ;
    const value_type *x_ptr = traits::vector_storage( x ) ;
    value_type *y_ptr = traits::vector_storage( y ) ;

    detail::gemv( TRANS, m, n, alpha, a_ptr, lda, x_ptr, stride_x, beta, y_ptr, stride_y );
  }

  // A <- alpha * x * trans(y) ( outer product ), alpha, x and y are real-valued
  // ! CAUTION this function assumes that all matrices involved are column-major matrices
  template < typename vector_type_x, typename vector_type_y, typename value_type, typename matrix_type >
  void ger( const value_type& alpha,
            const vector_type_x &x,
            const vector_type_y &y,
            matrix_type &a
            )
  {
    // precondition: matrix_type must be dense or dense_proxy
    /* not all compilers can handle the traits
    BOOST_STATIC_ASSERT( ( boost::is_same< typename mtraits::matrix_structure,
                                           boost::numeric::bindings::traits::general_t
                           >::value ) ) ;
    */

    const integer_t m = traits::matrix_size1( a ) ;
    const integer_t n = traits::matrix_size2( a ) ;
    assert ( traits::vector_size( x ) <= m ) ;
    assert ( traits::vector_size( y ) <= n ) ;
    const integer_t lda = traits::leading_dimension( a ) ;
    const integer_t stride_x = traits::vector_stride( x ) ;
    const integer_t stride_y = traits::vector_stride( y ) ;

    const value_type *x_ptr = traits::vector_storage( x ) ;
    const value_type *y_ptr = traits::vector_storage( y ) ;
    value_type *a_ptr = traits::matrix_storage( a ) ;

    detail::ger( m, n, alpha, x_ptr, stride_x, y_ptr, stride_y, a_ptr, lda );
  }
/*
  // A <- alpha * x * trans(y) ( outer product ), alpha, x and y are complex-valued
  template < typename vector_type_x, typename vector_type_y, typename value_type, typename matrix_type >
  void geru( const value_type& alpha,
             const vector_type_x &x,
             const vector_type_y &y,
             matrix_type &a
             )
  {
    // precondition: matrix_type must be dense or dense_proxy
//    not all compilers can handle the traits
//    BOOST_STATIC_ASSERT( ( boost::is_same< typename mtraits::matrix_structure,
//                                           boost::numeric::bindings::traits::general_t
//                           >::value ) ) ;

//    BOOST_STATIC_ASSERT( ( boost::is_same< x.value_type(), FEMTown::Complex() >::value ) ) ;
    const integer_t m = traits::matrix_size1( a ) ;
    const integer_t n = traits::matrix_size2( a ) ;
    assert ( traits::vector_size( x ) <= m ) ;
    assert ( traits::vector_size( y ) <= n ) ;
    const integer_t lda = traits::leading_dimension( a ) ;
    const integer_t stride_x = traits::vector_stride( x ) ;
    const integer_t stride_y = traits::vector_stride( y ) ;

    const value_type *x_ptr = traits::vector_storage( x ) ;
    const value_type *y_ptr = traits::vector_storage( y ) ;
    value_type *a_ptr = traits::matrix_storage( a ) ;

    detail::geru( m, n, alpha, x_ptr, stride_x, y_ptr, stride_y, a_ptr, lda );
  }
*/
  /*
  // y <- alpha * A * x + beta * y
  template < typename matrix_type, typename vector_type_x, typename vector_type_y >
  void gemv(const typename traits::matrix_traits<matrix_type>::value_type &alpha,
            const matrix_type &a,
            const vector_type_x &x,
            const typename traits::vector_traits<vector_type_y>::value_type &beta,
            vector_type_y &y
            )
  {
    gemv( traits::NO_TRANSPOSE, alpha, a, x, beta, y );
  }

  // y <- A * x
  template < typename matrix_type, typename vector_type_x, typename vector_type_y >
  void gemv(const matrix_type &a, const vector_type_x &x, vector_type_y &y)
  {
    typedef typename traits::matrix_traits<matrix_type>::value_type val_t;
    gemv( traits::NO_TRANSPOSE, (val_t) 1, a, x, (val_t) 0, y );
  }
  */

}}}}

#endif // BOOST_BINDINGS_BLAS_BLAS2_HPP


// Filename: insert_scope.cpp

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

using namespace mtl;

class world_matrix
{
    typedef mat::inserter<compressed2D<double>, update_plus<double> > inserter_type;
  public:
    world_matrix(unsigned nrows, unsigned ncols) : A(nrows, ncols), ins(0) {}

    void add_entry(unsigned row, unsigned col, double value)
    {  (*ins)[row][col] << value;    }

    void start_insertion() // must be called before first insertion
    { if (!ins) ins= new inserter_type(A); }

    void finish_insertion() // must be called before first usage
    {
	if (ins) {
	    delete ins;
	    ins= 0;
	}
    }

    friend inline std::ostream& operator<<(std::ostream& os, const world_matrix& w)
    { return os << w.A; }

  private:
    compressed2D<double>      A;
    inserter_type*            ins;
};

int main(int, char**)
{
    world_matrix              A(3, 3);

    A.start_insertion();
    A.add_entry(0, 0, 2.0);
    A.add_entry(1, 2, 0.5);
    A.add_entry(2, 1, 3.0);
    A.finish_insertion();

    std::cout << "A is \n" << A;  // we can access A now

    return 0;
}


//
// Created by yche on 12/17/17.
//
#include "sling.h"

#include <boost/format.hpp>

#include "tbb/parallel_sort.h"

#include "ground_truth/stat.h"
#include "ground_truth/graph_yche.h"
#include "ground_truth/yche_serialization.h"
#include "util/log.h"
#include "util/util.h"

const double Sling::BACKEPS = 7.28e-4; //EPS / 23.;
const double Sling::K = 10.;

int Sling::NUMTHREAD = std::thread::hardware_concurrency();
//int Sling::NUMTHREAD = 1;

using namespace std::chrono;

void Sling::init() {
    first = new bool[g->n];
    second = new bool[g->n];
    memset(first, 0, sizeof(bool) * g->n);
    memset(second, 0, sizeof(bool) * g->n);
    for (int u = 0; u < g->n; ++u) {
        if (!(first[u] = (g->inedge[u].size() <= K / BACKEPS))) {
            second[u] = false;
            continue;
        }
        int cnt = 0;
        for (auto itr = g->inedge[u].begin(); itr != g->inedge[u].end(); ++itr) {
            cnt += g->inedge[*itr].size();
        }
        second[u] = (cnt <= K / BACKEPS);
    }
    sqrtct[0] = 1.;
    for (int i = 1; i < 20; ++i)
        sqrtct[i] = sqrtc * sqrtct[i - 1];
}

//########################### indexing 1st: Diagonal Correction Matrix ####################################
double Sling::calcDi(int i, double eps, bool &early, int &R, int tid) {
    R = 0;
    early = true;
    if (g->inedge[i].size() == 0) return 1.;
    if (g->inedge[i].size() == 1) {
        if (g->inedge[i][0] == i) return 1.;
        else return 1. - c;
    }
    return calcDi_1(i, eps, early, R, tid);
}

double Sling::calcDi_1(int i, double eps, bool &early, int &R, int tid) {
    const auto &in_i = g->inedge[i];
    const vector<int> *inedge = g->inedge;
    const int isize = in_i.size();
    eps = eps * (double) isize / (double) (isize - 1) / c;
//    int Rs = 14. / 3. / eps * 2. * log(2 * g->n) / log(2.71828);
//    constexpr double failure_probability = 0.01;

    int Rs = 14. / 3. / eps * log(4.0 * g->n / failure_probability) / log(2.71828);
    int X = 0;
    double cc = c * (1l << 32);
    for (; R < Rs; ++R) {
        int rx = gen.rand(tid) % isize;
        int ry = gen.rand(tid) % (isize - 1);
        if (ry >= rx) ++ry;
        int x = in_i[rx];
        int y = in_i[ry];
        do {
            if (inedge[x].empty() || inedge[y].empty())
                break;
            x = inedge[x][gen.rand(tid) % inedge[x].size()];
            y = inedge[y][gen.rand(tid) % inedge[y].size()];
            if (x == y) {
                ++X;
                break;
            }
        } while (gen.rand(tid) < cc);
    }
    if (X / (double) R <= eps) {
        return 1. - c * c * (X / (double) R) * (double) (isize - 1) / (double) isize - c / (double) isize;
    }
    double upp = X / (double) R + sqrt(eps * X / (double) R);
//    int Rl = (2 * upp + 2. / 3. * eps) / (eps * eps) * log(2 * g->n) / log(2.71828);
    int Rl = (2 * upp + 2. / 3. * eps) / (eps * eps) * log(4.0 * g->n / failure_probability) / log(2.71828);
    for (; R < Rl; ++R) {
        int rx = gen.rand(tid) % isize;
        int ry = gen.rand(tid) % (isize - 1);
        if (ry >= rx) ++ry;
        int x = in_i[rx];
        int y = in_i[ry];
        do {
            if (inedge[x].empty() || inedge[y].empty())
                break;
            x = inedge[x][gen.rand(tid) % inedge[x].size()];
            y = inedge[y][gen.rand(tid) % inedge[y].size()];
            if (x == y) {
                ++X;
                break;
            }
        } while (gen.rand(tid) < cc);
    }
    early = false;
    return 1. - c * c * (X / (double) R) * (double) (isize - 1) / (double) isize - c / (double) isize;
}

void Sling::t_calcD(double eps, mutex *lock, int *cursor, int tid) {
    int i, u;
    while (true) {
        lock->lock();
        i = *cursor;
        u = ((*cursor) += BLOCKSIZE);
        lock->unlock();
        if (i >= g->n)
            return;
        if (u > g->n) u = g->n;
        for (; i < u; ++i) {
            int RWCNT;
            bool early;
            double di = calcDi(i, eps / c, early, RWCNT, tid);
            d[i] = di;
        }
    }
}

void __Sling_t_calcD(Sling *sim, double eps, mutex *lock, int *cursor, int tid) {
    sim->t_calcD(eps, lock, cursor, tid);
}

void Sling::calcD(double eps) {
    if (d != NULL) delete[] d;
    d = new double[g->n];
    int cursor = 0;
    mutex lock;
    vector<thread> threads;
    for (int i = 0; i < NUMTHREAD - 1; ++i)
        threads.emplace_back(__Sling_t_calcD, this, eps, &lock, &cursor, i);
    t_calcD(eps, &lock, &cursor, NUMTHREAD - 1);
    for (int t = 0; t < NUMTHREAD - 1; ++t)
        threads[t].join();
    d_bar = 0.;
    for (int i = 0; i < g->n; ++i)
        d_bar += d[i];
    d_bar /= g->n;
}

//########################### indexing 2nd: Backward Propagation ####################################
map<pair<int, int>, double, PairCmp> Sling::pushback(int u, double eps, int tid) {
    map<pair<int, int>, double, PairCmp> p;
    gmap<int, double> pt;
    gmap<int, double> ptt;
    gset<int> s;
    deque<int> q;
    deque<int> qq;

    int t = 0;
    pt[u] = 1.;
    q.push_back(u);

    while (!q.empty()) {
        int v = q.front();
        double puvt = pt[v];;
        p[make_pair(v, t)] = puvt;
        for (unsigned i = 0; i < g->edge[v].size(); ++i) {
            int vv = g->edge[v][i];
            double x = ptt[vv] += puvt * sqrtc / (double) g->inedge[vv].size();
            if (x > eps)// || (t < 2 && x > eps / K))
            {
                if (s.find(vv) == s.end()) {
                    qq.push_back(vv);
                    s.insert(vv);
                }
            }
        }
        q.pop_front();
        if (q.empty()) {
            s.clear();
            pt.swap(ptt);
            ptt.clear();
            q.swap(qq);
            qq.clear();
            ++t;
        }
    }
    p.erase(make_pair(u, 0));
    return p;
}

void Sling::t_backward(double eps, mutex *tasklock, int *cursor, int tid, mutex *plock) {
    int s, t;
//    vector<tuple<int, int, int, double>> vec;
    while (true) {
//        vec.clear();
        tasklock->lock();
        s = *cursor;
        t = ((*cursor) += BLOCKSIZE);
        tasklock->unlock();

        if (s >= g->n) return;
        if (t > g->n) t = g->n;

        for (int v = s; v < t; ++v) {
            auto pv = pushback(v, eps, tid);
            for (auto itr = pv.begin(); itr != pv.end(); ++itr) {
                if (itr->second >= eps || (itr->first.second < 2 && itr->second >= eps / K)) {
                    int u = itr->first.first;
                    int t = itr->first.second;
                    double value = itr->second;
                    if (t == 1 && first[u]) continue;
                    if (t == 2 && second[u]) continue;

//                    p.push_back(make_tuple(u, t, v, value));
                    con_vec_p.push_back(make_tuple(u, t, v, value));
//                    con_vec_p.emplace_back(u, t, v, value);
                }
            }
        }
    }
}

void __Sling_t_backward(Sling *sim, double eps, mutex *tasklock, int *cursor, int tid, mutex *plock) {
    sim->t_backward(eps, tasklock, cursor, tid, plock);
}

void Sling::backward(double eps) {
//    Sling::NUMTHREAD = 1;
    log_info("Backward Multi Threading: %d", Sling::NUMTHREAD);
    int plockNum = (g->n - 1) / BLOCKSIZE + 1;
    auto *plock = new mutex[plockNum];
    mutex tasklock;
    int cursor = 0;
//    if (!p.empty()) { p.clear(); }
//    p.reserve(2339768660l);

    if (!con_vec_p.empty()) { con_vec_p.clear(); }
//    con_vec_p.reserve(2339768660l);
    con_vec_p.reserve(233976866l);

    vector<thread> threads;
    for (int i = 0; i < NUMTHREAD - 1; ++i)
        threads.emplace_back(__Sling_t_backward, this, eps, &tasklock, &cursor, i, plock);
    t_backward(eps, &tasklock, &cursor, NUMTHREAD - 1, plock);
    for (int t = 0; t < NUMTHREAD - 1; ++t)
        threads[t].join();
    delete[] plock;
    cerr << "sort" << endl;
//    sort(p.begin(), p.end(), cmpTuple);
    tbb::parallel_sort(con_vec_p.begin(), con_vec_p.end(), cmpTuple);
    cerr << "sort finished" << endl;
    pstart.resize(g->n + 1);
    pstart[0] = 0;
    int x = 1;
    for (long long i = 0; i < con_vec_p.size(); ++i) {
        if (std::get<0>(con_vec_p[i]) >= x) {
            for (; x <= std::get<0>(con_vec_p[i]); ++x)
                pstart[x] = i;
        }
    }
    for (; x <= g->n; ++x)
        pstart[x] = con_vec_p.size();
    p = vector<tuple<int, int, int, double>>{begin(con_vec_p), end(con_vec_p)};
}

///----------------------------- 1st: single pair
double Sling::simrank(int u, int v) {
    assert(!p.empty() && d != nullptr);
    if (u == v) return 1.;
    google::sparse_hash_map<pair<int, int>, double, PairHash> pu;
    gmap<pair<int, int>, double, PairHash> pv;
    gmap<pair<int, int>, double, PairHash> ppu;
    gmap<pair<int, int>, double, PairHash> ppv;
    for (long long i = pstart[u]; i < pstart[u + 1]; ++i) {
        pu[make_pair(std::get<1>(p[i]), std::get<2>(p[i]))] = std::get<3>(p[i]);
    }
    for (long long i = pstart[v]; i < pstart[v + 1]; ++i) {
        pv[make_pair(std::get<1>(p[i]), std::get<2>(p[i]))] = std::get<3>(p[i]);
    }

    int cnt = 0;
    int thr = 1. / sqrt(BACKEPS);
    for (long long i = pstart[u]; i < pstart[u + 1] && cnt < thr; ++i) {
        int t = std::get<1>(p[i]);
        int x = std::get<2>(p[i]);
        if (g->inedge[x].size() > thr || g->inedge[x].empty()) continue;
        ++cnt;
        for (auto xitr = g->inedge[x].begin(); xitr != g->inedge[x].end(); ++xitr) {
            if (pu.find(make_pair(t + 1, *xitr)) == pu.end())
                ppu[make_pair(t + 1, *xitr)] += std::get<3>(p[i]) * c / g->inedge[x].size();
        }
    }
    cnt = 0;
    for (long long i = pstart[v]; i < pstart[v + 1] && cnt < thr; ++i) {
        int t = std::get<1>(p[i]);
        int x = std::get<2>(p[i]);
        if (g->inedge[x].size() > thr || g->inedge[x].empty()) continue;
        ++cnt;
        for (auto xitr = g->inedge[x].begin(); xitr != g->inedge[x].end(); ++xitr) {
            if (pv.find(make_pair(t + 1, *xitr)) == pv.end())
                ppv[make_pair(t + 1, *xitr)] += std::get<3>(p[i]) * c / g->inedge[x].size();
        }
    }
    for (auto itr = ppu.begin(); itr != ppu.end(); ++itr) {
        pu[itr->first] = itr->second;
    }
    for (auto itr = ppv.begin(); itr != ppv.end(); ++itr) {
        pv[itr->first] = itr->second;
    }
    if (first[u]) {
        for (auto itr = g->inedge[u].begin(); itr != g->inedge[u].end(); ++itr) {
            pu[make_pair(1, *itr)] = sqrtc / g->inedge[u].size();
        }
    }
    if (second[u]) {
        for (auto uitr = g->inedge[u].begin(); uitr != g->inedge[u].end(); ++uitr) {
            int v = *uitr;
            for (auto vitr = g->inedge[v].begin(); vitr != g->inedge[v].end(); ++vitr) {
                pu[make_pair(2, *vitr)] += c / g->inedge[u].size() / g->inedge[v].size();
            }
        }
    }
    if (first[v]) {
        for (auto itr = g->inedge[v].begin(); itr != g->inedge[v].end(); ++itr) {
            pv[make_pair(1, *itr)] = sqrtc / g->inedge[v].size();
        }
    }
    if (second[v]) {
        for (auto vitr = g->inedge[v].begin(); vitr != g->inedge[v].end(); ++vitr) {
            int vv = *vitr;
            for (auto vitr = g->inedge[vv].begin(); vitr != g->inedge[vv].end(); ++vitr) {
                pv[make_pair(2, *vitr)] += c / g->inedge[v].size() / g->inedge[vv].size();
            }
        }
    }
    double sim = 0.;
    for (auto uitr = pu.begin(); uitr != pu.end(); ++uitr) {
        auto vitr = pv.find(uitr->first);
        if (vitr == pv.end())
            continue;
        sim += uitr->second * d[uitr->first.second] * vitr->second;
    }
    return sim;
}

// TODO
///----------------------------- 2nd: single source
vector<double> Sling::simrank(int u) {
    assert(!p.empty() && d != NULL);
    map<pair<int, int>, double, PairCmp> pu;
    for (long long i = pstart[u]; i < pstart[u + 1]; ++i)
        pu[make_pair(std::get<1>(p[i]), std::get<2>(p[i]))] = std::get<3>(p[i]);
    gmap<pair<int, int>, double, PairHash> ppu;
    int cnt = 0;
    int thr = 1. / sqrt(BACKEPS);
    for (long long i = pstart[u]; i < pstart[u + 1] && cnt < thr; ++i) {
        int t = std::get<1>(p[i]);
        int x = std::get<2>(p[i]);
        if (g->inedge[x].size() > thr || g->inedge[x].empty()) continue;
        ++cnt;
        for (auto xitr = g->inedge[x].begin(); xitr != g->inedge[x].end(); ++xitr) {
            if (pu.find(make_pair(t + 1, *xitr)) == pu.end())
                ppu[make_pair(t + 1, *xitr)] += std::get<3>(p[i]) * c / g->inedge[x].size();
        }
    }
    for (auto itr = ppu.begin(); itr != ppu.end(); ++itr) {
        pu[itr->first] = itr->second;
    }
    if (second[u]) {
        for (auto uitr = g->inedge[u].begin(); uitr != g->inedge[u].end(); ++uitr) {
            int v = *uitr;
            for (auto vitr = g->inedge[v].begin(); vitr != g->inedge[v].end(); ++vitr) {
                pu[make_pair(2, *vitr)] += c / g->inedge[u].size() / g->inedge[v].size();
            }
        }
    }
    if (first[u]) {
        for (auto itr = g->inedge[u].begin(); itr != g->inedge[u].end(); ++itr) {
            pu[make_pair(1, *itr)] = sqrtc / g->inedge[u].size();
        }
    }
    vector<double> sim(g->n, 0.);
    vector<double> q;
    vector<double> qq;
    deque<int> s;
    deque<int> ss;
    for (int t = 1;; ++t) {
        auto start = pu.lower_bound(make_pair(t, 0));
        if (start == pu.end())
            break;
        auto end = pu.lower_bound(make_pair(t + 1, 0));
        if (start == end) continue;
        q.clear();
        q.resize(g->n, 0.);
        qq.clear();
        qq.resize(g->n, 0.);
        s.clear();
        ss.clear();
        int TTT = g->n / 10;
        for (; start != end; ++start) {
            q[start->first.second] = start->second * d[start->first.second];
            s.push_back(start->first.second);
        }
        double eps = sqrtct[t] * BACKEPS;
        for (int l = 1; l < t; ++l) {
            if (s.size() > TTT) {
                for (int u = 0; u < g->n; ++u) {
                    if (q[u] <= eps) continue;
                    for (auto vitr = g->edge[u].begin(); vitr != g->edge[u].end(); ++vitr) {
                        int v = *vitr;
                        qq[v] += sqrtc * q[u] / (double) g->inedge[v].size();
                    }
                }
                q.swap(qq);
                qq.clear();
                qq.resize(g->n, 0.);
            } else {
                for (auto uitr = s.begin(); uitr != s.end(); ++uitr) {
                    int u = *uitr;
                    for (auto vitr = g->edge[u].begin(); vitr != g->edge[u].end(); ++vitr) {
                        int v = *vitr;
                        double inc = sqrtc * q[u] / (double) g->inedge[v].size();
                        double x = qq[v] += inc;
                        if (x > eps && x <= eps + inc) {
                            ss.push_back(v);
                        }
                    }
                }
                q.swap(qq);
                qq.clear();
                qq.resize(g->n, 0.);
                s.swap(ss);
                ss.clear();
            }
        }
        if (s.size() > TTT) {
            for (int u = 0; u < g->n; ++u) {
                if (q[u] <= eps) continue;
                for (auto vitr = g->edge[u].begin(); vitr != g->edge[u].end(); ++vitr) {
                    int v = *vitr;
                    sim[v] += sqrtc * q[u] / (double) g->inedge[v].size();
                }
            }
        } else {
            for (auto uitr = s.begin(); uitr != s.end(); ++uitr) {
                int u = *uitr;
                for (auto vitr = g->edge[u].begin(); vitr != g->edge[u].end(); ++vitr) {
                    int v = *vitr;
                    sim[v] += sqrtc * q[u] / (double) g->inedge[v].size();
                }
            }
        }
    }

    sim[u] = 1.;
    return sim;
}

string Sling::get_file_path_base() {
    exec(string("mkdir -p " + SLING_INDEX_DIR).c_str());
    string file_path =
            SLING_INDEX_DIR + "/" + boost::str(boost::format("RLP_%s-%.3f-%.6f-%.6f") % g_name % c % eps_d % theta);
    return file_path;
}

void Sling::build_or_load_index() {
    string d_file_path = get_file_path_base() + ".d";
    string p_file_path = get_file_path_base() + ".p";
    string pstart_file_path = get_file_path_base() + ".pstart";
    if (file_exists(d_file_path) && file_exists(p_file_path) && file_exists(pstart_file_path)) {
        log_info("indexing exists.......");
        YcheSerializer serializer;
        log_info("d_file_path: %s", d_file_path.c_str());
        log_info("p_file_path: %s", p_file_path.c_str());
        FILE *pFile = fopen(d_file_path.c_str(), "r");
        size_t tmp;
        serializer.read_array_into_ref(pFile, d, tmp);
        log_info("d size: %s", to_string(tmp).c_str());
        fclose(pFile);

        FILE *pFile2 = fopen(p_file_path.c_str(), "r");
        serializer.read_tuple_vec(pFile2, p);
        fclose(pFile2);
        log_info("p size: %s", to_string(p.size()).c_str());

        FILE *pFile3 = fopen(pstart_file_path.c_str(), "r");
        serializer.read_vec(pFile3, pstart);
        fclose(pFile3);
    } else {
        //  build the index
        cout << "indexing..." << endl;

        auto tmp_start = std::chrono::high_resolution_clock::now();
        calcD(eps_d);
        auto tmp_end = std::chrono::high_resolution_clock::now();

        cout << "finish calcD " << float(duration_cast<microseconds>(tmp_end - tmp_start).count()) / (pow(10, 6))
             << " s\n";
        tmp_start = std::chrono::high_resolution_clock::now();
        backward(theta);
        tmp_end = std::chrono::high_resolution_clock::now();

        cout << "finish backward " << float(duration_cast<microseconds>(tmp_end - tmp_start).count()) / (pow(10, 6))
             << " s\n";
        cout << "mem size:" << getValue() << endl;

        cout << "store index......." << endl;
        // store indexing d
        YcheSerializer serializer;
        FILE *pFile = fopen(d_file_path.c_str(), "wb");
        serializer.write_array(pFile, d, static_cast<size_t>(g->n));
        fclose(pFile);
        // store indexing p
        FILE *pFile2 = fopen(p_file_path.c_str(), "wb");
        serializer.write_tuple_vec(pFile2, p);
        fclose(pFile2);
        // store indexing pstart

        FILE *pFile3 = fopen(pstart_file_path.c_str(), "wb");
        serializer.write_vec(pFile3, pstart);
        fclose(pFile2);
    }
}


#include "ni/layers/mapneighadjacency.h"

#include "ni/layers/depthsegmentation.h"

#include "elm/core/exception.h"
#include "elm/core/graph/graphattr.h"
#include "elm/core/layerconfig.h"
#include "elm/core/signal.h"
#include "elm/ts/layerattr_.h"

using namespace std;
using namespace cv;
using namespace elm;
using namespace ni;

/** Define parameters, non-integral defaults and any remaining I/O keys
  */
// paramters

// defaults
const int MapNeighAdjacency::DEFAULT_LABEL_UNASSIGNED   = 0;

#include <boost/assign/list_of.hpp>
template <>
elm::MapIONames LayerAttr_<MapNeighAdjacency>::io_pairs = boost::assign::map_list_of
        ELM_ADD_INPUT_PAIR(detail::BASE_SINGLE_INPUT_FEATURE_LAYER__KEY_INPUT_STIMULUS)
        ELM_ADD_OUTPUT_PAIR(detail::BASE_MATOUTPUT_LAYER__KEY_OUTPUT_RESPONSE)
        ;

MapNeighAdjacency::~MapNeighAdjacency()
{
}

MapNeighAdjacency::MapNeighAdjacency()
    : base_FeatureTransformationLayer()
{
    Clear();
}

void MapNeighAdjacency::Clear()
{
    m_ = Mat1f();
}

void MapNeighAdjacency::Reset(const LayerConfig &config)
{
    Reconfigure(config);
}

void MapNeighAdjacency::Reconfigure(const LayerConfig &config)
{
    if(config.Params().size() > 0) {

        ELM_THROW_KEY_ERROR("MapNeighAdjacency does not expect any paramters.");
    }
}

void MapNeighAdjacency::Activate(const Signal &signal)
{
    Mat1f map = signal.MostRecentMat1f(name_input_); // weighted gradient after thresholding

    GraphAttr seg_graph(map, map > DEFAULT_LABEL_UNASSIGNED);

    seg_graph.AdjacencyMat(m_);
}


#ifndef MATHLEXER_HPP
#define MATHLEXER_HPP

#include <iostream>
#include <memory>
#include <sstream>
#include <vector>

#include <boost/lexical_cast.hpp>

namespace simple_interpreter {

struct Token
{
   enum Type
   {
      integer,
      plus,
      minus,
      lparen,
      rparen
   } type;

   std::string text;

   Token(const Type t, const std::string& str) : type(t), text(str)
   {
   }

   friend std::ostream&
   operator<<(std::ostream& os, const Token& token)
   {
      os << "`" << token.text << "`";
      return os;
   }
};

std::vector< Token >
lex(const std::string text)
{
   std::vector< Token > tokens;
   for (size_t charIdx = 0; charIdx < text.size(); ++charIdx)
   {
      switch (text[charIdx])
      {
         case '+':
            tokens.emplace_back(Token::plus, "+");
            break;
         case '-':
            tokens.emplace_back(Token::minus, "-");
            break;
         case '(':
            tokens.emplace_back(Token::lparen, "(");
            break;
         case ')':
            tokens.emplace_back(Token::rparen, ")");
            break;
         default:
            if (std::isdigit(text[charIdx]))
            {
               std::ostringstream buffer;
               buffer << text[charIdx];
               for (size_t i = charIdx + 1; i < text.size(); ++i)
               {
                  if (std::isdigit(text[i]))
                  {
                     buffer << text[i];
                     ++charIdx;
                  }
                  else
                  {
                     break;
                  }
               }

               tokens.emplace_back(Token::integer, buffer.str());
            }
            else
            {
               throw std::runtime_error{std::string("error: Unexpected token: `") + text[charIdx] + '`'};
            }
      }
   }

   return tokens;
}

struct Element
{
   virtual int
   eval() const = 0;
};

struct Integer : Element
{
   int value;

   Integer(const int newVal) : value(newVal)
   {
   }

   int
   eval() const override
   {
      return value;
   }
};

struct BinaryOperation : Element
{
   enum Type
   {
      addition,
      subtraction
   } type;

   std::shared_ptr< Element > lhs, rhs;

   int
   eval() const override
   {
      switch (type)
      {
         case addition:
            return lhs->eval() + rhs->eval();
         case subtraction:
            return lhs->eval() - rhs->eval();
      }
      return 0;
   }
};

std::shared_ptr< Element >
parse(const std::vector< Token >& tokens)
{
   auto rootOp = std::make_shared< BinaryOperation >();

   bool haveLhs{false};

   for (size_t tokenIdx = 0; tokenIdx < tokens.size(); ++tokenIdx)
   {
      const auto& token = tokens[tokenIdx];
      switch (token.type)
      {
         case Token::integer:
         {
            const int value = boost::lexical_cast< int >(token.text);
            auto integer = std::make_shared< Integer >(value);
            if (!haveLhs)
            {
               rootOp->lhs = integer;
               haveLhs = true;
            }
            else
            {
               rootOp->rhs = integer;
            }
            break;
         }
         case Token::minus:
         {
            rootOp->type = BinaryOperation::subtraction;
            break;
         }
         case Token::plus:
         {
            rootOp->type = BinaryOperation::addition;
            break;
         }
         case Token::lparen:
         {
            bool rparenFound{false};
            size_t rparenIdx = tokenIdx + 1;
            for (; rparenIdx < tokens.size(); ++rparenIdx)
            {
               if (tokens[rparenIdx].type == Token::rparen)
               {
                  rparenFound = true;
                  break;
               }
               else if (tokens[rparenIdx].type == Token::lparen)
               {
                  throw std::runtime_error{"error: Nested parens not supported yet"};
               }
            }
            if (rparenFound)
            {
               std::vector< Token > subExpression{tokens.begin() + tokenIdx + 1, tokens.begin() + rparenIdx};
               auto element = parse(subExpression);
               if (!haveLhs)
               {
                  rootOp->lhs = element;
                  haveLhs = true;
               }
               else
               {
                  rootOp->rhs = element;
               }
               tokenIdx = rparenIdx;
            }
            else
            {
               throw std::runtime_error{"error: Missing `)`"};
            }
            break;
         }
         case Token::rparen:
         {
            throw std::runtime_error{"error: `)` should be ignored"};
            break;
         }
      }
   }

   return rootOp;
}

} // namespace simple_interpreter

#endif // MATHLEXER_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/correct.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/multi/algorithms/correct.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/multi/io/wkt/wkt.hpp>

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

template <typename Geometry>
void test_geometry(std::string const& wkt, std::string const& expected)
{
    Geometry geometry;

    bg::read_wkt(wkt, geometry);
    bg::correct(geometry);

    std::ostringstream out;
    out << bg::wkt(geometry);

    BOOST_CHECK_EQUAL(out.str(), expected);
}

template <typename P>
void test_all()
{
    typedef bg::model::multi_polygon<bg::model::polygon<P> > cw_type;
    std::string cw_mp =
            "MULTIPOLYGON(((0 0,0 1,1 1,1 0,0 0)))";
    test_geometry<cw_type>(cw_mp, cw_mp);

    test_geometry<cw_type>("MULTIPOLYGON(((0 0,1 0,1 1,0 1,0 0)))",
        cw_mp);
}

int test_main( int , char* [] )
{
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


#include <hdf5.h>
#include <omp.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <Eigen/Sparse>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <functional>
#include <iomanip>
#include <iostream>
#include <stdexcept>

#include "bte_config.h"
#include "aux/filtered_range.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "base/numbers.hpp"
#include "collision_tensor/assembly/gain.hpp"
#include "collision_tensor/collision_tensor_factory.hpp"
#include "collision_tensor/collision_tensor_galerkin.hpp"
#include "collision_tensor/time_stepping/rk4.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "post_processing/energy.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/utility/utility.hpp"

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

typedef SpectralBasisFactoryKS basis_factory_t;

int main(int argc, char* argv[])
{
  boltzmann::Timer<> timer;
  double beta, dt;
  int nsteps;
  string tensor_file;
  bool adaptiveL, verbose;

  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      ("beta,b", po::value<double>(&beta)->default_value(2), "beta")
      ("dt,t", po::value<double>(&dt)->default_value(0.001), "delta t")
      ("nsteps,n", po::value<int>(&nsteps)->default_value(200), "#timesteps")
      ("adapt,a", po::value<bool>(&adaptiveL)->default_value(false), "use adaptive L-range")
      ("ct,T", po::value<string>(&tensor_file),
      "path to tensor file \n the files `spectral_basis.desc` and \n `spectral_basis_test.desc` "
      "must reside in the same directory")
      ("init,i", po::value<string>()->required(), "coefficients hdf5 in Polar Laguerre basis")
      ("ifdata", po::value<string>()->default_value("coeffs"), " path to data in HDF5 file `if`")
      ("verbose,v", po::value<bool>(&verbose)->default_value(false), "verbose output");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  std::string version_id = GIT_SHA1;
  cout << "VersionID: " << version_id << "@" << GIT_BNAME << std::endl;
  cout << "CMD:\n";
  for (int i = 0; i < argc; ++i) {
    cout << argv[i] << "\t";
  }
  cout << "\n\n\n";

  cout << "Command line parameters::\n"
       << right << setw(15) << "beta" << setw(12) << beta << endl
       << right << setw(15) << "dt" << setw(12) << dt << "\n\n\n";

  typedef typename basis_factory_t::basis_type basis_type;
  typedef boost::filesystem::path path_t;

  // load spectral basis
  basis_type trial_basis;
  path_t tensor_fpath(tensor_file.c_str());
  string trial_basis_fname = (tensor_fpath.parent_path() / path_t("spectral_basis.desc")).string();
  basis_factory_t::create(trial_basis, trial_basis_fname);

  // read tensor from file
  timer.start();
  CollisionTensorGalerkin ct(trial_basis);
  ct.read_hdf5(tensor_file.c_str());
  print_timer(timer.stop(), "read collision tensor from file");
  cout << "------------------------------\n";

  const int N = trial_basis.n_dofs();
  Eigen::VectorXd coeffs(N);
  string fname = vm["init"].as<string>();
  hid_t h5_init = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
  eigen2hdf::load(h5_init, vm["ifdata"].as<string>(), coeffs);

  cout << "---------- MOMENTSs ----------\n";
  Mass mass;
  mass.init(trial_basis);
  double m0 = mass.compute(coeffs.data());
  cout << "\t mass = " << m0 << endl;

  Energy energy;
  energy.init(trial_basis);
  energy.compute(coeffs.data());

  Momentum momentum;
  momentum.init(trial_basis);
  momentum.compute(coeffs.data());
  cout << "\t energy = " << energy.compute(coeffs.data()) << endl;

  // ------------------------------------------------------------
  // TIME STEPPING
  typedef Eigen::VectorXd vec_t;
  vec_t out(N);
  hg::RK4<> rk4(N);
  const double tol = 1e-10;
  auto find_relevant_range = [&](const double* solution) {
    const int L = spectral::get_max_l(trial_basis);
    typedef typename basis_type::elem_t elem_t;
    typedef typename basis_factory_t::fa_type angular_elem_t;
    // start from l=Lmax until  coefficients are above threshold
    const int llower_bound = 2;
    int lupper_bound = 2;
    typename elem_t::Acc::template get<angular_elem_t> get_xir;
    for (int l = L; l > llower_bound; --l) {
      std::function<bool(const elem_t&)> pred = [&](const elem_t& e) {
        return (get_xir(e).get_id().l == l);
      };
      // get current l-range of spectral basis
      auto range = filtered_range(trial_basis.begin(), trial_basis.end(), pred);
      bool is_below_tre = true;
      for (auto it = std::get<0>(range); it != std::get<1>(range); ++it) {
        unsigned int idx = trial_basis.get_dof_index(it->get_id());
        if (std::abs(solution[idx]) > tol) {
          is_below_tre = false;
          break;
        }
      }
      if (!is_below_tre) {
        lupper_bound = l;
        break;
      }
    }
    // look for the upper-bound iterator in trial_basis
    std::function<bool(int, const elem_t& e)> comp = [&](int l, const elem_t& e) {
      return l < get_xir(e).get_id().l;
    };
    auto it_max = std::upper_bound(trial_basis.begin(), trial_basis.end(), lupper_bound, comp);
    if (verbose) cout << "Adpative Basis: L_max = " << lupper_bound << endl;
    // returns nmax => relevant range is (0, nmax)
    return (it_max - trial_basis.begin());
  };

  hid_t file, gdata;
  file = H5Fcreate("coefficients.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  gdata = H5Gcreate1(file, "data", 0);
  eigen2hdf::save(gdata, "0", coeffs);
  H5Fflush(file, H5F_SCOPE_GLOBAL);

  double t = 0;
  for (int i = 0; i < nsteps; ++i) {
    auto coeffsn = coeffs;
    timer.start();
    auto f = [&](double* dst, const double* src) {
      if (adaptiveL) {
        int nmax = find_relevant_range(coeffs.data());
        if (verbose) cout << "Adaptive nmax = " << nmax << endl;
        ct.apply_adaptive(dst, src, nmax);
      } else {
        ct.apply(dst, src);
      }
    };

    rk4.apply(coeffsn.data(), coeffs.data(), f, dt);
    ct.project(coeffsn.data(), coeffs.data());
    print_timer(timer.stop(), "RK4");
    t += dt;
    timer.start();
    coeffs = coeffsn;
    double m = mass.compute(coeffs.data());
    if(std::isnan(m)) {
      H5Gclose(gdata);
      H5Fclose(file);
      throw std::runtime_error("blow up...");
    }
    double e = energy.compute(coeffs.data());
    auto mom = momentum.compute(coeffs.data());
    print_timer(timer.stop(), "compute moments");

    cout << "::MOMENTS::\t" << setw(8) << i + 1 << setw(20) << setprecision(10) << t << setw(30)
         << setprecision(20) << scientific << m << setw(30) << setprecision(20) << scientific << e
         << setw(30) << setprecision(20) << scientific << mom[0] << setw(30) << setprecision(20)
         << scientific << mom[1] << endl;

    // save results to HDF5
    eigen2hdf::save(gdata, boost::lexical_cast<string>(i + 1), coeffs);
    H5Fflush(file, H5F_SCOPE_GLOBAL);
  }

  H5Gclose(gdata);
  H5Fclose(file);

  return 0;
}


#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>
#include <chrono>
#include <queue>

using namespace Eigen;

typedef struct {
  long n;
  long m;
  unsigned int *rowOffsets;
  unsigned int *adj;
  long n_coarse;
  long m_coarse;
  unsigned int *rowOffsetsCoarse;
  unsigned int *adjCoarse;
  int *coarseID;
  double *eweights;
} graph_t;

template <typename T>
std::string to_string_with_precision(const T a_value, const int n = 8) {

  std::ostringstream out;
  out << std::setprecision(n) << a_value;
  return out.str();
}

static int
vu_cmpfn_inc(const void *a, const void *b) {

  int *av = ((int *) a);
  int *bv = ((int *) b);
  if (*av > *bv)
    return 1;
  if (*av < *bv)
    return -1;
  if (*av == *bv) {
    if (av[1] > bv[1])
      return 1;
    if (av[1] < bv[1])
      return -1;
  }
  return 0;
}

static int
simpleCoarsening(graph_t *g, int coarseningType) {

  if (coarseningType == 0)
    return 0;

  int num_coarsening_rounds_max = 100;
  int coarse_graph_nmax = 1000;

  int *cID = (int *) malloc((g->n)*sizeof(int));
  assert(cID != NULL);
  int *toMatch = (int *) malloc((g->n)*sizeof(int));
  assert(toMatch != NULL);

#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (long i=0; i<g->n; i++) {
    cID[i] = i;
    toMatch[i] = 1;
  }

  int coarse_vert_count = g->n;
  int num_rounds = 0;
  while ((coarse_vert_count > coarse_graph_nmax) &&
         (num_rounds < num_coarsening_rounds_max)) {
    num_rounds++;
    int num_matched = 0;
    for (int i=0; i<g->n; i++) {
      int u = i;
      while (cID[u] != u) {
        cID[u] = cID[cID[u]];
        u = cID[u];
      }
      if (toMatch[u] == 1) {
        for (unsigned int j=g->rowOffsets[u];
                          j<g->rowOffsets[u+1]; j++) {
          int v = g->adj[j];
          while (v != cID[v]) {
            cID[v] = cID[cID[v]];
            v = cID[v];
          }
          if (v == u) {
            continue;
          }
          if (toMatch[v] == 1) {
            if (u < v) {
              cID[u] = u;
              cID[v] = u;
            } else {
              cID[u] = v;
              cID[v] = v;
            }
            toMatch[u] = toMatch[v] = 0;
            num_matched += 2;
            break;
          }
        }
      }
    }

    int num_unmatched = coarse_vert_count - num_matched;
    int new_coarse_vert_count = num_matched/2 + num_unmatched;
    fprintf(stderr, "prev count: %d, matched: %d, new count: %d\n",
             coarse_vert_count, num_matched, new_coarse_vert_count);
             coarse_vert_count = new_coarse_vert_count;

    for (int i=0; i<g->n; i++) {
      toMatch[i] = 1;
    }
  }
  fprintf(stderr, "num rounds: %d\n", num_rounds);

  int *coarse_edges = (int *) malloc(2 * g->m * sizeof(int));
  assert(coarse_edges != NULL);

  /* set correct IDs */
  for (int i=0; i<g->n; i++) {
    int u = cID[i];
    while (u != cID[u]) {
      u = cID[cID[u]];
    }
    cID[i] = u;
  }

  int *vertIDs = (int *) malloc(g->n * sizeof(int));
  assert(vertIDs != NULL);
  for (int i=0; i<g->n; i++) {
    vertIDs[i] = -1;
  }

  int new_id = 0;
  for (int i=0; i<g->n; i++) {
    if (cID[i] == i) {
      vertIDs[i] = new_id++;
    }
  }
  assert(new_id == coarse_vert_count);
  for (int i=0; i<g->n; i++) {
    if (vertIDs[i] == -1) {
      vertIDs[i] = vertIDs[cID[i]];
    }
  }

  long ecount = 0;
  for (int i=0; i<g->n; i++) {
    int u = vertIDs[i];
    for (unsigned int j=g->rowOffsets[i]; j<g->rowOffsets[i+1]; j++) {
      int v = vertIDs[g->adj[j]];
      coarse_edges[ecount++] = u;
      coarse_edges[ecount++] = v;
      // fprintf(stderr, "%d %d\n", u, v);
    }
  }
  ecount = ecount/2;
  qsort(coarse_edges, g->m, 2*sizeof(int), vu_cmpfn_inc);

  /* count the number of coarse edges */
  int m_coarse = 1;
  int prev_u = coarse_edges[0];
  int prev_v = coarse_edges[1];

  for (int i=1; i<ecount; i++) {
    int curr_u = coarse_edges[2*i];
    int curr_v = coarse_edges[2*i+1];
    if ((curr_u != prev_u) || (curr_v != prev_v)) {
      m_coarse++;
      prev_u = curr_u;
      prev_v = curr_v;
    }
  }

  fprintf(stderr, "m_coarse %d\n", m_coarse);

  /* Allocate coarse edge weights array */
  double *eweights;
  eweights = (double *) malloc(m_coarse * sizeof(double));
  assert(eweights != NULL);
  for (int i=0; i<m_coarse; i++) {
    eweights[i] = 1.0;
  }

  unsigned int *rowOffsetsCoarse;
  rowOffsetsCoarse = (unsigned int *)
    malloc((coarse_vert_count+1)*sizeof(unsigned int));
  assert(rowOffsetsCoarse != NULL);
  for (int i=0; i<coarse_vert_count+1; i++) {
    rowOffsetsCoarse[i] = 0;
  }

  unsigned int *adjCoarse;
  adjCoarse = (unsigned int *) malloc(m_coarse*sizeof(unsigned int));
  assert(adjCoarse != NULL);

  /* update coarse edge weights */
  m_coarse = 1;
  // eweights[0] = 1.0;
  prev_u = coarse_edges[0];
  prev_v = coarse_edges[1];
  adjCoarse[0] = prev_v;
  rowOffsetsCoarse[prev_u+1]++;

  for (int i=1; i<ecount; i++) {
    int curr_u = coarse_edges[2*i];
    int curr_v = coarse_edges[2*i+1];
    if ((curr_u != prev_u) || (curr_v != prev_v)) {
      m_coarse++;
      adjCoarse[m_coarse-1] = curr_v;
      // eweights[m_coarse] = 1.0;
      rowOffsetsCoarse[curr_u+1]++;
      prev_u = curr_u;
      prev_v = curr_v;
    } else {
      eweights[m_coarse-1] += 1.0;
    }
  }

  for (int i=1; i<=coarse_vert_count; i++) {
    rowOffsetsCoarse[i] += rowOffsetsCoarse[i-1];
    // fprintf(stderr, "%u ", rowOffsetsCoarse[i]);
  }

  /*
  fprintf(stderr, "printing coarse graph:\n");
  for (int i=0; i<coarse_vert_count; i++) {
    for (int j=rowOffsetsCoarse[i]; j<rowOffsetsCoarse[i+1]; j++) {
      fprintf(stderr, "[%u %u %lf] ", i, adjCoarse[j], eweights[j]);
    }
  }
  */

  /*
  for (int i=0; i<ecount/2; i++) {
    fprintf(stderr, "%d %d\n", coarse_edges[2*i], coarse_edges[2*i+1]);
  }
  */

  free(cID);
  free(toMatch);
  free(coarse_edges);

  g->coarseID = vertIDs;
  g->n_coarse = coarse_vert_count;
  g->m_coarse = m_coarse;
  g->eweights = eweights;
  g->adjCoarse = adjCoarse;
  g->rowOffsetsCoarse = rowOffsetsCoarse;

  if (coarseningType == 1)
    return 0;

  // Optionally write to CSR and MTX files
  std::cout << "Writing csr and mtx files to current directory\n";
  FILE *writeBinaryPtr = fopen( "graph_coarse.csr", "wb");
  if (writeBinaryPtr == NULL) {
    fprintf(stderr, "could not open csr file for writing\n");
    exit(1);
  }

  FILE *outfp_mtx = fopen( "graph_coarse.mtx", "w");
  if (outfp_mtx == NULL) {
    fprintf(stderr, "could not open mtx file for writing\n");
    exit(1);
  }

  long N = g->n_coarse;
  unsigned int *rowOffsetsCoarse_noloops = (unsigned int *)
  malloc((N+1)*sizeof(unsigned int));
  assert(rowOffsetsCoarse_noloops != NULL);
  for (long i=0; i<N+1; i++) {
    rowOffsetsCoarse_noloops[i] = 0;
  }

  long num_self_loops = 0;
  for (long i=0; i<g->n_coarse; i++) {
    for (unsigned int j=g->rowOffsetsCoarse[i];
                      j<g->rowOffsetsCoarse[i+1]; j++) {
      unsigned int v = g->adjCoarse[j];
      if (((unsigned int) i) == v) {
        num_self_loops++;
      } else {
        rowOffsetsCoarse_noloops[i+1]++;
      }
    }
  }

  for (long i=1; i<N+1; i++) {
    rowOffsetsCoarse_noloops[i] += rowOffsetsCoarse_noloops[i-1];
  }

  long M = g->m_coarse - num_self_loops;
  std::cout << "edge count after loop removal: " << M << std::endl;

  fprintf(outfp_mtx, "%%%%MatrixMarket matrix coordinate pattern symmetric\n");
  fprintf(outfp_mtx, "%ld %ld %ld\n", N, N, M);

  unsigned int *adjCoarse_noloops = (unsigned int *)
    malloc(M*sizeof(unsigned int));
  assert(adjCoarse_noloops != NULL);

  long ec = 0;
  for (long i=0; i<g->n_coarse; i++) {
    for (unsigned int j=g->rowOffsetsCoarse[i];
                      j<g->rowOffsetsCoarse[i+1]; j++) {
      unsigned int v = g->adjCoarse[j];
      if (((unsigned int) i) == v) {
        num_self_loops++;
      } else {
        adjCoarse_noloops[ec++] = v;
        fprintf(outfp_mtx, "%ld %u\n", i+1, v+1);
      }
    }
  }
  assert(ec == M);
  fclose(outfp_mtx);

  long undirected = 1;
  long graph_type = 0;
  long one_indexed = 0;
  long verification_graph = 0;

  fwrite ( &N, sizeof(long), 1, writeBinaryPtr );
  fwrite ( &M, sizeof(long), 1, writeBinaryPtr );
  fwrite ( &undirected, sizeof(long), 1, writeBinaryPtr );
  fwrite ( &graph_type , sizeof(long), 1, writeBinaryPtr );
  fwrite ( &one_indexed , sizeof(long), 1, writeBinaryPtr );
  fwrite ( &verification_graph , sizeof(long), 1, writeBinaryPtr );

  fwrite ( rowOffsetsCoarse_noloops, sizeof(unsigned int), (N+1), writeBinaryPtr );
  fwrite ( adjCoarse_noloops, sizeof(unsigned int), M, writeBinaryPtr );

  fclose( writeBinaryPtr );

  free(rowOffsetsCoarse_noloops);
  free(adjCoarse_noloops);

  return 0;
}

static int
loadToMatrix(SparseMatrix<double,RowMajor>& M, VectorXd& degrees,
    graph_t *g, int coarseningType) {

  typedef Triplet<double> T;
  std::vector<T> tripletList;

  if (coarseningType == 0) {
    tripletList.reserve(g->m);

    for (int i=0; i<g->n; i++) {
      tripletList.push_back(T(i,i,0.5));
      degrees(i) = g->rowOffsets[i+1]-g->rowOffsets[i];
      double nzv = 1/(2.0*(g->rowOffsets[i+1]-g->rowOffsets[i]));
      for (unsigned int j=g->rowOffsets[i]; j<g->rowOffsets[i+1]; j++) {
        unsigned int v = g->adj[j];
        tripletList.push_back(T(i, v, nzv));
      }
    }
    M.setFromTriplets(tripletList.begin(), tripletList.end());
  } else {

    for (int i=0; i<g->n_coarse; i++) {
      double degree_i = 0;
      for (unsigned int j=g->rowOffsetsCoarse[i]; j<g->rowOffsetsCoarse[i+1]; j++) {
      degree_i += g->eweights[j];
    }
    degrees(i) = degree_i;
    // std::cout << degrees(i) << " ";
    }

    tripletList.reserve(g->m_coarse);

    for (long i=0; i<g->n_coarse; i++) {
      double diag_val = 0;
      double inv_2deg = 1/(2.0*degrees(i));
      for (unsigned int j=g->rowOffsetsCoarse[i]; j<g->rowOffsetsCoarse[i+1]; j++) {
        unsigned int v = g->adjCoarse[j];
        if (v == ((unsigned int) i)) {
          diag_val = g->eweights[j]*inv_2deg;
        } else {
          tripletList.push_back(T(i, v, g->eweights[j]*inv_2deg));
        }
      }
      tripletList.push_back(T(i,i,diag_val+0.5));
    }
    M.setFromTriplets(tripletList.begin(), tripletList.end());
  }

  return 0;
}

static VectorXd
bfs(unsigned int *row,
    unsigned int *col,
    long N, long M,
    unsigned int start) {
  VectorXd columnOfMatrix(N);

  unsigned int s = start;
  int *visited = (int *) malloc (sizeof(int) * N);
  memset(visited, 0, sizeof(unsigned int) * N);
  std::queue<unsigned int> Q;
  Q.push(s);
  visited[s] = 1;
  columnOfMatrix(s) = 0;

  while(!Q.empty()) {
    unsigned int h = Q.front();
    Q.pop();
    for (unsigned int j=row[h]; j<row[h+1]; j++) {
      s = col[j];
      if (!visited[s]) {
        visited[s] = 1;
        Q.push(s);
        columnOfMatrix(s) = columnOfMatrix(h) + 1;
      }
    }
  }

  free(visited);
  return columnOfMatrix;
}

static int
HDE(SparseMatrix<double,RowMajor>& M, graph_t *g,
    VectorXd& degrees,
    VectorXd& secondVec, VectorXd& thirdVec) {

  auto startTimerPart = std::chrono::high_resolution_clock::now();

  // Create Laplacian
  long n = g->n;
  long m = g->m;
  typedef Triplet<double> T;
  std::vector<T> LTripletList;
  LTripletList.reserve(g->m);
  for (int i=0; i<g->n; i++) {
    LTripletList.push_back(T(i,i,degrees(i)));
    for (unsigned int j=g->rowOffsets[i]; j<g->rowOffsets[i+1]; j++) {
      unsigned int v = g->adj[j];
      LTripletList.push_back(T(i,v, -1.0));
    }
  }
  SparseMatrix<double,RowMajor> L(n,n);
  L.setFromTriplets(LTripletList.begin(), LTripletList.end());
  auto endTimerPart = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elt = endTimerPart - startTimerPart;
  std::cout << "Laplacian load time: " << elt.count() << " s." << std::endl;

  // HDE Initialize
  startTimerPart = std::chrono::high_resolution_clock::now();
  VectorXi min_dist(g->n);
  min_dist.setOnes();
  min_dist = min_dist * INT_MAX;

  int maxM = 50;
  VectorXd tmp(n);
  tmp.setOnes();
  tmp.normalize();
  MatrixXd dist(n,maxM+1);
  MatrixXd dist_bak(n,maxM);
  dist.col(0) = tmp;

  int start_idx = 0;
  for (int run_count=1; run_count<=maxM; run_count++) {
    dist.col(run_count) = bfs(g->rowOffsets, g->adj, n, m, start_idx);

    int max = -1;
    for (long i=0; i<n; i++) {
      if (dist.col(run_count)(i) < min_dist[i]) {
        min_dist[i] = dist.col(run_count)(i);
      }
      if (min_dist[i] > max) {
        max = min_dist[i];
        start_idx = i;
      }
    }
    dist.col(run_count).normalize();
  }

  int j = 1;
  for (int run_count=0; run_count<maxM; run_count++) {

    for (int k=0; k<j; k++) {
      // D-orthogonalize
#if 1
      VectorXd dnormvec(n);
      dnormvec = dist.col(k).cwiseProduct(degrees);
      double multplr_denom = dist.col(k).dot(dnormvec);
      double multplr_num = dist.col(j).dot(dnormvec);
      dist.col(j) = dist.col(j) - (multplr_num * dist.col(k))/multplr_denom;
#endif
#if 0
      double multplr = dist.col(j).dot(dist.col(k));
      dist.col(j) = dist.col(j) - multplr * dist.col(k);
#endif
    }

    double normdist = dist.col(j).norm();
    if (normdist < 0.001) {
      std::cout << "discarding vec " << j << ", normdist " << normdist << std::endl;
      j--;
    } else {
      // std::cout << "j " << j << ", normdist " << normdist << std::endl;
      dist.col(j).normalize();
    }

    dist_bak.col(j-1) = dist.col(j);
    j++;
  }

  MatrixXd LX(n, maxM);
  LX = L * dist_bak;

  MatrixXd XtLX(maxM, maxM);
  XtLX = dist_bak.transpose() * LX;

  SelfAdjointEigenSolver<MatrixXd> es(XtLX);
  MatrixXd init_vecs(n, 2);
  init_vecs = dist_bak * es.eigenvectors().leftCols(2).real();

  endTimerPart = std::chrono::high_resolution_clock::now();
  elt = endTimerPart - startTimerPart;
  std::cout << "HDE Initialization time " << elt.count() << " s." << std::endl;
  secondVec = init_vecs.col(0);
  thirdVec  = init_vecs.col(1);

  return 0;
}

static int
powerIterationKoren(SparseMatrix<double,RowMajor>& M,
    VectorXd& degrees, double eps, VectorXd& firstVec,
    VectorXd& secondVec, VectorXd& thirdVec,
    int coarseningType, char *inputFilename) {

  // double eps = 1e-9;
  std::cout << "Using eps " << eps << " for second eigenvector" << std::endl;

  if (coarseningType > 0) {
    std::cout << "Using coarsened graph" << std::endl;
  }

  int n = M.rows();

  VectorXd uk_hat(n);

  // Intialized vectors are passed to function
  uk_hat = secondVec;
  // uk_hat.setRandom();
  // uk_hat.normalize();

  VectorXd uk(n);

  // For D-orthonormalization
  VectorXd firstVecD(n);
  firstVecD = firstVec.cwiseProduct(degrees);
  double mult1_denom = firstVec.dot(firstVecD);

  VectorXd residual(n);

  int num_iterations1 = 0;

  auto startTimerPart = std::chrono::high_resolution_clock::now();
  while (1) {

    uk = uk_hat;

    // D-orthonormalize
    double mult1_num = uk.dot(firstVecD);
    uk = uk - (mult1_num/mult1_denom)*firstVec;

    // Do matrix-vector product
    uk_hat = M*uk;
    uk_hat.normalize();

    num_iterations1++;

    // double residual_norm1 =
    //  residual.lpNorm<Infinity>()/(uk_hat.maxCoeff()-uk_hat.minCoeff());

#if 0
    double residual_dot = uk.dot(uk_hat);
    if (residual_dot >= (1-eps)) {
      break;
    }
#endif

    residual = uk-uk_hat;
    double residual_norm = residual.norm();
    // std::cout << residual_norm << std::endl;

    if (residual_norm < eps) {
      break;
    }
  }

  std::cout << "Num iterations for second eigenvector: " <<
    num_iterations1 << std::endl;

  // Save this eigenvector
  secondVec = uk_hat;
  auto endTimerPart = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elt = endTimerPart - startTimerPart;
  std::cout << "Second eigenvector computation time: " << elt.count() << " s." << std::endl;

  eps = 2.0*eps;
  std::cout << "Using eps " << eps << " for third eigenvector" << std::endl;

  // For D-orthonormalization
  VectorXd secondVecD(n);
  secondVecD = secondVec.cwiseProduct(degrees);
  double mult2_denom = secondVec.dot(secondVecD);

  // Initialized vectors are passed to function
  uk_hat = thirdVec;
  // uk_hat.setRandom();
  // uk_hat.normalize();

  startTimerPart = std::chrono::high_resolution_clock::now();
  int num_iterations2 = 0;
  while (1) {

    uk = uk_hat;

    // D-orthonormalize
    double mult1_num = uk.dot(firstVecD);
    uk = uk - (mult1_num/mult1_denom)*firstVec;
    double mult2_num = uk.dot(secondVecD);
    uk = uk - (mult2_num/mult2_denom)*secondVec;

    // Do matrix-vector product
    uk_hat = M*uk;
    uk_hat.normalize();

    num_iterations2++;

#if 0
    double residual_dot = uk.dot(uk_hat);
    if (residual_dot >= (1-eps)) {
      break;
    }
#endif

    residual = uk-uk_hat;
    double residual_norm = residual.norm();
    // std::cout << " " << residual_norm << std::endl;

    if (residual_norm < eps) {
      break;
    }

  }
  std::cout << "Num iterations for third eigenvector: " <<
    num_iterations2 << std::endl;

  // Save this eigenvector as well
  thirdVec = uk_hat;
  endTimerPart = std::chrono::high_resolution_clock::now();
  elt = endTimerPart - startTimerPart;
  std::cout << "Third eigenvector computation time: " << elt.count() << " s." << std::endl;

  std::cout << "Dot products of eigenvectors: "
  << firstVec.dot(secondVec) << " "
  << firstVec.dot(thirdVec) << " " << secondVec.dot(thirdVec) <<
  std::endl;

  return 0;

}

static int
RefineTutte(SparseMatrix<double,RowMajor>& M,
    VectorXd& secondVec, VectorXd& thirdVec,
    int numSmoothing) {

  std::cout << "Number of smoothing rounds: " << numSmoothing << std::endl;
  auto startTimerPart = std::chrono::high_resolution_clock::now();
  VectorXd unitVec(M.cols());;
  unitVec.setOnes();

  SparseMatrix<double,RowMajor> M2 = 2*M;
  // M2.diagonal() -= unitVec;
  M2.diagonal().setZero();

  for (int i=0; i<numSmoothing; i++) {
    secondVec = M2*secondVec;
    thirdVec = M2*thirdVec;
  }
  auto endTimerPart = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elt = endTimerPart - startTimerPart;
  std::cout << "RefineTutte Time: " << elt.count() << " s." << std::endl;

  return 0;
}

static int
writeCoords(SparseMatrix<double,RowMajor>& M,
    VectorXd& firstVec, VectorXd& secondVec, VectorXd& thirdVec,
    int coarseningType, int doHDE, int refineType,
    double eps, char *inputFilename) {

  // Write coordinates to file
  std::ofstream fout;
  std::string coordFilename(inputFilename);
  coordFilename += "_c" + std::to_string(coarseningType)
      + "_h" + std::to_string(doHDE)
      + "_r" + std::to_string(refineType)
      + "_eps" + to_string_with_precision(eps) + ".nxyz";
  std::cout << "Writing coordinates to file " << coordFilename << std::endl;
  fout.open(coordFilename);
  int n = M.cols();
  for (int i=0; i<n; i++) {
    fout << secondVec(i) << "," << thirdVec(i) << std::endl;
  }
  fout.close();

  /* print eigenvalues */
  double firstEigenVal = (M*firstVec).cwiseQuotient(firstVec).mean();
  double firstEigenValMin =
    (M*firstVec).cwiseQuotient(firstVec).minCoeff();
  double firstEigenValMax =
    (M*firstVec).cwiseQuotient(firstVec).maxCoeff();
  double secondEigenVal = (M*secondVec).cwiseQuotient(secondVec).mean();
  double secondEigenValMin =
    (M*secondVec).cwiseQuotient(secondVec).minCoeff();
  double secondEigenValMax =
    (M*secondVec).cwiseQuotient(secondVec).maxCoeff();
  double thirdEigenVal = (M*thirdVec).cwiseQuotient(thirdVec).mean();
  double thirdEigenValMin =
    (M*thirdVec).cwiseQuotient(thirdVec).minCoeff();
  double thirdEigenValMax =
    (M*thirdVec).cwiseQuotient(thirdVec).maxCoeff();

  std::cout << "First Eigenvalue  (mean, min, max): " << firstEigenVal
    << " " << firstEigenValMin << " " << firstEigenValMax << std::endl;
  std::cout << "Second Eigenvalue (mean, min, max): " << secondEigenVal
    << " " << secondEigenValMin << " " << secondEigenValMax << std::endl;
  std::cout << "Third Eigenvalue  (mean, min, max): " << thirdEigenVal
    << " " << thirdEigenValMin << " " << thirdEigenValMax << std::endl;

  return 0;
}

int main(int argc, char **argv) {

  if (argc != 5) {
    std::cout << "Usage: "<< argv[0] << " <csr filename> "
    "<0/1/2 (none,coarsen and continue,coarsen+stop)> <0/1 (HDE)> "
    "<0/1/2/3 (none,Koren,Tutte,Koren+Tutte)> "
    << std::endl;
  return 1;
  }

  char *inputFilename = argv[1];

  int coarseningType = atoi(argv[2]);
  if (coarseningType == 1) {
    std::cout << "Coarsening graph and continuing" << std::endl;
  } else if (coarseningType == 2) {
    std::cout << "Coarsening and stopping" << std::endl;
  } else {
    coarseningType = 0;
  }

  int doHDE = atoi(argv[3]);
  if (doHDE) {
    std::cout << "Running High-dimensional embedding" << std::endl;
    coarseningType = 0;
  } else {
    doHDE = 0;
  }

  int refineType = atoi(argv[4]);
  if (refineType == 0) {
    std::cout << "No eigenvector computation or refinement" << std::endl;
  } else if (refineType == 1) {
    std::cout << "Computing eigenvectors using Koren's algorithm" << std::endl;
  } else if (refineType == 2) {
    std::cout << "Refining coordinates using Tutte's algorithm" << std::endl;
  } else if (refineType == 3) {
    std::cout << "Eigenvectors followed by Tutte refinement" << std::endl;
  }

  // Read CSR file
  auto startTimer = std::chrono::high_resolution_clock::now();
  auto startTimerPart = std::chrono::high_resolution_clock::now();
  FILE *infp = fopen(inputFilename, "rb");
  if (infp == NULL) {
    std::cout << "Error: Could not open input file. Exiting ..." <<
    std::endl;
    return 1;
  }
  long n, m;
  long rest[4];
  unsigned int *rowOffsets, *adj;
  fread(&n, 1, sizeof(long), infp);
  fread(&m, 1, sizeof(long), infp);
  fread(rest, 4, sizeof(long), infp);
  rowOffsets = (unsigned int *) malloc (sizeof(unsigned int) * (n+1));
  assert(rowOffsets != NULL);
  adj = (unsigned int *) malloc (sizeof(unsigned int) * m);
  assert(adj != NULL);
  fread(rowOffsets, n+1, sizeof(unsigned int), infp);
  fread(adj, m, sizeof(unsigned int), infp);
  fclose(infp);
  auto endTimerPart = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elt = endTimerPart - startTimerPart;
  std::cout << "CSR read time: " << elt.count() << " s." << std::endl;
  std::cout << "Num edges: " << m/2 << ", vertices: " << n << std::endl;

  graph_t g;
  g.n = n; g.m = m;
  g.rowOffsets = rowOffsets;
  g.adj = adj;

  simpleCoarsening(&g, coarseningType);

  VectorXd secondVecc;
  VectorXd thirdVecc;

  if (coarseningType > 0) {
    long n_coarse = g.n_coarse;
    SparseMatrix<double,RowMajor> Mc(n_coarse,n_coarse);
    VectorXd degreesc(n_coarse);
    loadToMatrix(Mc, degreesc, &g, coarseningType);
    VectorXd firstVecc(n_coarse);
    firstVecc.setOnes();
    firstVecc.normalize();
    secondVecc.resize(n_coarse);
    thirdVecc.resize(n_coarse);
    secondVecc.setRandom();
    if (secondVecc(0) < 0) {
      secondVecc = -secondVecc;
    }
    secondVecc.normalize();
    thirdVecc.setRandom();
    if (thirdVecc(0) < 0) {
      thirdVecc = -thirdVecc;
    }
    thirdVecc.normalize();
    double epsc = 1e-9;
    powerIterationKoren(Mc, degreesc, epsc,
      firstVecc, secondVecc, thirdVecc, coarseningType,
        inputFilename);
    if (coarseningType == 2) {
      writeCoords(Mc, firstVecc, secondVecc, thirdVecc,
        coarseningType, doHDE, refineType, epsc, inputFilename);
    }
  }

  // Load to matrix
  startTimerPart = std::chrono::high_resolution_clock::now();
  SparseMatrix<double,RowMajor> M(n,n);
  VectorXd degrees(n);
  // load the full graph/matrix now
  loadToMatrix(M, degrees, &g, 0);
  endTimerPart = std::chrono::high_resolution_clock::now();
  elt = endTimerPart - startTimerPart;
  std::cout << "Matrix load time: " << elt.count() << " s." << std::endl;

  // Compute second and third eigenvectors using
  // Koren's power iteration algorithm
  VectorXd firstVec(n);
  firstVec.setOnes();
  firstVec.normalize();

  VectorXd secondVec(n);
  VectorXd thirdVec(n);

  // Initialize with previously-found coarse vectors
  if (coarseningType == 1) {
    for (long i=0; i<g.n; i++) {
      secondVec(i) = secondVecc(g.coarseID[i]);
      thirdVec(i)  = thirdVecc(g.coarseID[i]);
    }
    secondVec.normalize();
    thirdVec.normalize();

  // initialize with HDE vectors
  } else if (doHDE == 1) {
    HDE(M, &g, degrees, secondVec, thirdVec);

  // Random vectors
  } else if (doHDE == 0) {
     secondVec.setRandom();
     if (secondVec(0) < 0) {
       secondVec = -secondVec;
     }
     secondVec.normalize();
     thirdVec.setRandom();
     if (thirdVec(0) < 0) {
       thirdVec = -thirdVec;
     }
     thirdVec.normalize();
  }

  if (coarseningType != 2) {
    int numTutteSmoothing = 500;
    double eps = 1e-5;
    if (refineType == 0) {
      secondVec.normalize();
      thirdVec.normalize();
    } else if (refineType == 1) {
      powerIterationKoren(M, degrees, eps, firstVec, secondVec, thirdVec,
      0, inputFilename);
    } else if (refineType == 2) {
      RefineTutte(M, secondVec, thirdVec, numTutteSmoothing);
    } else if (refineType == 3) {
      RefineTutte(M, secondVec, thirdVec, numTutteSmoothing);
      powerIterationKoren(M, degrees, eps, firstVec, secondVec, thirdVec,
      0, inputFilename);
    }
    writeCoords(M, firstVec, secondVec, thirdVec,
          coarseningType, doHDE, refineType, 0, inputFilename);
  }

  free(g.rowOffsets);
  free(g.adj);

  if (coarseningType > 0) {
    free(g.rowOffsetsCoarse);
    free(g.adjCoarse);
    free(g.coarseID);
    free(g.eweights);
  }
  endTimerPart = std::chrono::high_resolution_clock::now();
  elt = endTimerPart - startTimer;
  std::cout << "Overall time: " << elt.count() << " s." << std::endl;

  return 0;
}


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

#define GRAPHBLAS_LOGGING_LEVEL 0

#include <iostream>
#include <graphblas/graphblas.hpp>

using namespace grb;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE transpose_test_suite

#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_two_argument_transpose_bad_dim)
{
    IndexArrayType i_mA    = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mA    = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double>       v_mA    = {1, 2, 3, 4, 5, 6, 7, 8, 9};
    Matrix<double, DirectedMatrixTag> mA(3, 3);
    mA.build(i_mA, j_mA, v_mA);

    Matrix<double, DirectedMatrixTag> mC(3, 4);

    BOOST_CHECK_THROW(
        (transpose(mC, NoMask(), NoAccumulate(), mA)),
        DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_two_argument_transpose_square)
{
    IndexArrayType i_mA    = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mA    = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double>       v_mA    = {1, 2, 3, 4, 5, 6, 7, 8, 9};
    Matrix<double, DirectedMatrixTag> mA(3, 3);
    mA.build(i_mA, j_mA, v_mA);

    Matrix<double, DirectedMatrixTag> answer(3, 3);
    answer.build(j_mA, i_mA, v_mA);

    Matrix<double, DirectedMatrixTag> mC(3, 3);

    transpose(mC, NoMask(), NoAccumulate(), mA);
    BOOST_CHECK_EQUAL(mC, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_two_argument_transpose_nonsquare)
{
    IndexArrayType i_mA    = {0, 0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mA    = {0, 1, 2, 3, 0, 1, 2, 0, 1, 2};
    std::vector<double>       v_mA    = {1, 2, 3,-2, 4, 5, 6, 7, 8, 9};
    Matrix<double, DirectedMatrixTag> mA(3, 4);
    mA.build(i_mA, j_mA, v_mA);

    Matrix<double, DirectedMatrixTag> answer(4, 3);
    answer.build(j_mA, i_mA, v_mA);

    Matrix<double, DirectedMatrixTag> mC(4, 3);

    transpose(mC, NoMask(), NoAccumulate(), mA);

    BOOST_CHECK_EQUAL(mC, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_transpose_noaccum)
{
    // Build input matrix
    // | 1 1 - - |T
    // | 1 2 2 - |
    // | - 2 3 3 |

    std::vector<std::vector<double>> Atmp = {{1, 1, 0},
                                             {1, 2, 2},
                                             {0, 2, 3},
                                             {0, 0, 3}};
    Matrix<double, DirectedMatrixTag> A(Atmp, 0.0);

    std::vector<std::vector<double>> Ctmp = {{9, 9, 9, 9},
                                             {9, 9, 9, 9},
                                             {9, 9, 9, 9}};

    std::vector<std::vector<uint8_t>> mask = {{1, 1, 1, 1},
                                              {0, 1, 1, 1},
                                              {0, 0, 0, 1}};
    Matrix<uint8_t> M(mask, 0);
    {
        std::vector<std::vector<double>> ans = {{1, 1, 0, 0},
                                                {1, 2, 2, 0},
                                                {0, 2, 3, 3}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  NoMask(),
                  NoAccumulate(),
                  A);
        BOOST_CHECK_EQUAL(C, answer);
    }

    // Mask, replace
    {
        std::vector<std::vector<double>> ans = {{1, 1, 0, 0},
                                                {0, 2, 2, 0},
                                                {0, 0, 0, 3}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  M,
                  NoAccumulate(),
                  A,
                  REPLACE);
        BOOST_CHECK_EQUAL(C, answer);
    }

    // Mask, merge
    {
        std::vector<std::vector<double>> ans = {{1, 1, 0, 0},
                                                {9, 2, 2, 0},
                                                {9, 9, 9, 3}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  M,
                  NoAccumulate(),
                  A,
                  MERGE);
        BOOST_CHECK_EQUAL(C, answer);
    }

    // scmp(Mask), replace
    {
        std::vector<std::vector<double>> ans = {{0, 0, 0, 0},
                                                {1, 0, 0, 0},
                                                {0, 2, 3, 0}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  complement(M),
                  NoAccumulate(),
                  A,
                  REPLACE);
        BOOST_CHECK_EQUAL(C, answer);
    }

    // scmp(Mask), merge
    {
        std::vector<std::vector<double>> ans = {{9, 9, 9, 9},
                                                {1, 9, 9, 9},
                                                {0, 2, 3, 9}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  complement(M),
                  NoAccumulate(),
                  A,
                  MERGE);
        BOOST_CHECK_EQUAL(C, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_transpose_plus_accum)
{
    // Build input matrix
    // | 1 1 - - |T
    // | 1 2 2 - |
    // | - 2 3 3 |

    std::vector<std::vector<double>> Atmp = {{1, 1, 0},
                                             {1, 2, 2},
                                             {0, 2, 3},
                                             {0, 0, 3}};
    Matrix<double, DirectedMatrixTag> A(Atmp, 0.0);

    std::vector<std::vector<double>> Ctmp = {{9, 9, 9, 9},
                                             {9, 9, 9, 9},
                                             {9, 9, 9, 9}};

    std::vector<std::vector<uint8_t>> mask = {{1, 1, 1, 1},
                                              {0, 1, 1, 1},
                                              {0, 0, 0, 1}};
    Matrix<uint8_t> M(mask, 0);
    {
        std::vector<std::vector<double>> ans = {{10, 10,  9,  9},
                                                {10, 11, 11,  9},
                                                { 9, 11, 12, 12}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  NoMask(),
                  Plus<double>(),
                  A);
        BOOST_CHECK_EQUAL(C, answer);
    }

    // Mask, replace
    {
        std::vector<std::vector<double>> ans = {{10, 10,  9,  9},
                                                {0,  11, 11,  9},
                                                {0,   0,  0, 12}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  M,
                  Plus<double>(),
                  A,
                  REPLACE);
        BOOST_CHECK_EQUAL(C, answer);
    }

    // Mask, merge
    {
        std::vector<std::vector<double>> ans = {{10, 10, 9,  9},
                                                { 9, 11, 11, 9},
                                                { 9, 9,  9, 12}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  M,
                  Plus<double>(),
                  A,
                  MERGE);
        BOOST_CHECK_EQUAL(C, answer);
    }

    // scmp(Mask), replace
    {
        std::vector<std::vector<double>> ans = {{ 0,  0,  0, 0},
                                                {10,  0,  0, 0},
                                                { 9, 11, 12, 0}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  complement(M),
                  Plus<double>(),
                  A,
                  REPLACE);
        BOOST_CHECK_EQUAL(C, answer);
    }

    // scmp(Mask), merge
    {
        std::vector<std::vector<double>> ans = {{ 9,  9,  9, 9},
                                                {10,  9,  9, 9},
                                                { 9, 11, 12, 9}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        transpose(C,
                  complement(M),
                  Plus<double>(),
                  A,
                  MERGE);
        BOOST_CHECK_EQUAL(C, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_transpose_nonsquare_trans)
{
    // Build input matrix
    // | 1 1 - - |T
    // | 1 2 2 - |
    // | - 2 3 3 |

    std::vector<std::vector<double>> Atmp = {{1, 1, 0, 0},
                                             {1, 2, 2, 0},
                                             {0, 2, 3, 3}};
    Matrix<double, DirectedMatrixTag> A(Atmp, 0.0);

    {
        std::vector<std::vector<double>> ans = {{1, 1, 0, 0},
                                                {1, 2, 2, 0},
                                                {0, 2, 3, 3}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(3, 4);
        transpose(C,
                  NoMask(),
                  NoAccumulate(),
                  transpose(A));
        BOOST_CHECK_EQUAL(C, answer);
    }
}

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_MAJORITY_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MAJORITY_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing majority capabilities

    Returns @ref True if at least two inputs are not @ref Zero else @ref False.

    @par Semantic:

    @code
    as_logical_t<T> r = majority(x,y,z);
    @endcode

    is similar to:

    @code
    as_logical_t<T> r = (x!= 0)+(y!= 0)+(z!= 0) >= 2;
    @endcode

  **/
  const boost::dispatch::functor<tag::majority_> majority = {};
} }
#endif

#include <boost/simd/function/scalar/majority.hpp>
#include <boost/simd/function/simd/majority.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_ELLIPTIC_FUNCTIONS_GENERIC_ELLIPKE_HPP_INCLUDED
#define NT2_ELLIPTIC_FUNCTIONS_GENERIC_ELLIPKE_HPP_INCLUDED

#include <nt2/elliptic/functions/ellipke.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  (ellipke_, tag::cpu_,
                             (A0),
                             (generic_<floating_<A0> >)

                            )
  {
    typedef std::pair<A0, A0> result_type;

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename nt2::meta::scalar_of<A0>::type sA0;
      A0 first, second;
      nt2::ellipke(a0, nt2::Eps<sA0>(), first, second);
      return result_type(first, second);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (ellipke_, tag::cpu_,
                             (A0)(A1),
                             (generic_<floating_<A0> >)
                             (scalar_<floating_<A1> >)

                            )
  {
    typedef std::pair<A0, A0> result_type;

    NT2_FUNCTOR_CALL(2)
    {
      A0 first, second;
      nt2::ellipke(a0, a1, first, second);
      return result_type(first, second);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (  ellipke_, tag::cpu_,
                               (A0)(A1),
                               (generic_<floating_<A0> >)
                               (scalar_<floating_<A1> >)
                               (generic_<floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A1 const & a1,A0 & a3) const
    {
      A0 a2;
      nt2::ellipke(a0,a1,a2,a3);
      return a2;
    }
  };

} }
#endif


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Dec 2, 2014
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file kinematics/Transformation.hpp
 * @brief Header file for the Transformation class.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_OKVIS_TRANSFORMATION_HPP_
#define INCLUDE_OKVIS_TRANSFORMATION_HPP_

#include <stdint.h>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "okvis/kinematics/operators.hpp"

/// \brief okvis Main namespace of this package.
namespace okvis {

/// \brief kinematics Namespace for kinematics functionality, i.e. transformations and stuff.
namespace kinematics {

/// \brief Implements sin(x)/x for all x in R.
/// @param[in] x The argument of the sinc function.
/// \return The result.
double sinc(double x);

/// \brief Implements the exponential map for quaternions.
/// @param[in] dAlpha a axis*angle (minimal) input in tangent space.
/// \return The corresponding Quaternion.
Eigen::Quaterniond deltaQ(const Eigen::Vector3d& dAlpha);

/// \brief Right Jacobian, see Forster et al. RSS 2015 eqn. (8)
Eigen::Matrix3d rightJacobian(const Eigen::Vector3d & PhiVec);

/// \brief A class that does homogeneous transformations.
/// This relates a frame A and B: T_AB; it consists of
///   translation r_AB (represented in frame A) and
///   Quaternion q_AB (as an Eigen Quaternion).
/// see also the RSS'13 / IJRR'14 paper or the Thesis.
/// Follows some of the functionality of the SchweizerMesser library by Paul Furgale,
/// but uses Eigen quaternions underneath.
/// \warning This means the convention is different to SchweizerMesser
///          and the RSS'13 / IJRR'14 paper / the Thesis
class Transformation
{
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /// \brief Default constructor: initialises a unit transformation.
  Transformation();

  /// \brief Copy constructor: nothing fancy but takes care
  ///        of not doing bad stuff with internal caching.
  Transformation(const Transformation & other);

  /// \brief Move constructor: nothing fancy but takes care
  ///        of not doing bad stuff with internal caching.
  Transformation(Transformation && other);

  /// \brief Construct from a translation and quaternion.
  /// @param[in] r_AB The translation r_AB (represented in frame A).
  /// @param[in] q_AB The Quaternion q_AB (as an Eigen Quaternion).
  Transformation(const Eigen::Vector3d & r_AB, const Eigen::Quaterniond& q_AB);

  /// \brief Construct from a homogeneous transformation matrix.
  /// @param[in] T_AB The homogeneous transformation matrix.
  explicit Transformation(const Eigen::Matrix4d & T_AB);

  /// \brief Trivial destructor.
  ~Transformation();

  /// \brief Parameter setting, all 7.
  /// \tparam Derived_coeffs Deducible matrix type.
  /// @param[in] coeffs The parameters as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
  template<typename Derived_coeffs>
  bool setCoeffs(const Eigen::MatrixBase<Derived_coeffs> & coeffs);

  /// \brief Parameter setting, all 7.
  /// \tparam Derived_coeffs Deducible matrix type.
  /// @param[in] parameters The parameters as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
  template<typename Derived_coeffs>
  bool setParameters(const Eigen::MatrixBase<Derived_coeffs> & parameters)
  {
    return setCoeffs(parameters);
  }

  /// \brief The underlying homogeneous transformation matrix.
  Eigen::Matrix4d T() const;

  /// \brief Returns the rotation matrix (cached).
  const Eigen::Matrix3d & C() const;

  /// \brief Returns the translation vector r_AB (represented in frame A).
  const Eigen::Map<Eigen::Vector3d> & r() const;

  /// \brief Returns the Quaternion q_AB (as an Eigen Quaternion).
  const Eigen::Map<Eigen::Quaterniond> & q() const;

  const Eigen::Quaterniond  hamilton_quaternion() const {
    return Eigen::Quaterniond(C_);
  }

  /// \brief Get the upper 3x4 part of the homogeneous transformation matrix T_AB.
  Eigen::Matrix<double, 3, 4> T3x4() const;

  /// \brief The coefficients (parameters) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
  const Eigen::Matrix<double, 7, 1> & coeffs() const
  {
    return parameters_;
  }

  /// \brief The parameters (coefficients) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
  const Eigen::Matrix<double, 7, 1> & parameters() const
  {
    return parameters_;
  }

  /// \brief Get the parameters --- support for ceres.
  /// \warning USE WITH CARE!
  const double* parameterPtr() const
  {
    return &parameters_[0];
  }

  /// \brief Set this to a random transformation.
  void setRandom();
  /// \brief Set this to a random transformation with bounded rotation and translation.
  /// @param[in] translationMaxMeters Maximum translation [m].
  /// @param[in] rotationMaxRadians Maximum rotation [rad].
  void setRandom(double translationMaxMeters, double rotationMaxRadians);

  /// \brief Set from a homogeneous transformation matrix.
  /// @param[in] T_AB The homogeneous transformation matrix.
  void set(const Eigen::Matrix4d & T_AB);

  /// \brief Set from a translation and quaternion.
  /// @param[in] r_AB The translation r_AB (represented in frame A).
  /// @param[in] q_AB The Quaternion q_AB (as an Eigen Quaternion).
  void set(const Eigen::Vector3d & r_AB, const Eigen::Quaternion<double>& q_AB);

  /// \brief Set this transformation to identity
  void setIdentity();

  /// \brief Get an identity transformation
  static Transformation Identity();

  /// \brief Returns a copy of the transformation inverted.
  Transformation inverse() const;

  // operator* (group operator)
  /// \brief Multiplication with another transformation object.
  /// @param[in] rhs The right-hand side transformation for this to be multiplied with.
  Transformation operator*(const Transformation & rhs) const;

  /// \brief Transform a direction as v_A = C_AB*v_B (with rhs = hp_B)..
  /// \warning This only applies the rotation!
  /// @param[in] rhs The right-hand side direction for this to be multiplied with.
  Eigen::Vector3d operator*(const Eigen::Vector3d & rhs) const;

  /// \brief Transform a homogenous point as hp_B = T_AB*hp_B (with rhs = hp_B).
  /// @param[in] rhs The right-hand side direction for this to be multiplied with.
  Eigen::Vector4d operator*(const Eigen::Vector4d & rhs) const;

  /// \brief Assignment -- copy. Takes care of proper caching.
  /// @param[in] rhs The rhs for this to be assigned to.
  Transformation& operator=(const Transformation & rhs);

  /// \brief Apply a small update with delta being 6x1.
  /// \tparam Derived_delta Deducible matrix type.
  /// @param[in] delta The 6x1 minimal update.
  /// \return True on success.
  template<typename Derived_delta>
  bool oplus(const Eigen::MatrixBase<Derived_delta> & delta);

  /// \brief Apply a small update with delta being 6x1 --
  ///        the Jacobian is a 7 by 6 matrix.
  /// @param[in] delta The 6x1 minimal update.
  /// @param[out] jacobian The output Jacobian.
  /// \return True on success.
  template<typename Derived_delta, typename Derived_jacobian>
  bool oplus(const Eigen::MatrixBase<Derived_delta> & delta,
             const Eigen::MatrixBase<Derived_jacobian> & jacobian);

  /// \brief Get the Jacobian of the oplus operation (a 7 by 6 matrix).
  /// @param[out] jacobian The output Jacobian.
  /// \return True on success.
  template<typename Derived_jacobian>
  bool oplusJacobian(
      const Eigen::MatrixBase<Derived_jacobian> & jacobian) const;

  /// \brief Gets the jacobian dx/dChi,
  ///        i.e. lift the minimal Jacobian to a full one (as needed by ceres).
  // @param[out] jacobian The output lift Jacobian (6 by 7 matrix).
  /// \return True on success.
  template<typename Derived_jacobian>
  bool liftJacobian(const Eigen::MatrixBase<Derived_jacobian> & jacobian) const;

 protected:
  /// \brief Update the caching of the rotation matrix.
  void updateC();
  Eigen::Matrix<double, 7, 1> parameters_;  ///< Concatenated parameters [r;q].
  Eigen::Map<Eigen::Vector3d> r_;  ///< Translation {_A}r_{B}.
  Eigen::Map<Eigen::Quaterniond> q_;  ///< Quaternion q_{AB}.
  Eigen::Matrix3d C_; ///< The cached DCM C_{AB}.
};

}  // namespace kinematics
}  // namespace okvis

#include "implementation/Transformation.hpp"

#endif /* INCLUDE_OKVIS_TRANSFORMATION_HPP_ */


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the tangent of the input in degrees:
    \f$\sin(\pi x/180)/\cos(\pi x/180) \f$.

    @par Header <boost/simd/function/tand.hpp>

    @par Note

      As most other trigonometric function tand can be called with a
      second optional parameter  which is a tag on speed and accuracy
      (see @ref cos for further details)

    @see tan, tanpi

    @par Example:

      @snippet tand.cpp tand

    @par Possible output:

      @snippet tand.txt tand

  **/
  IEEEValue tand(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/tand.hpp>
#include <boost/simd/function/simd/tand.hpp>

#endif


#include <vocabulary-tree/vocabulary-tree-maker.h>

#include <algorithm>
#include <bitset>
#include <cstdio>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <loopclosure-common/types.h>
#include <vi-map/vertex.h>

#include "vocabulary-tree/tree-builder.h"
#include "vocabulary-tree/types.h"

DEFINE_int32(
    lc_kmeans_splits, 10, "Number of splits in the kmeans step per level.");

DEFINE_int32(lc_kmeans_levels, 4, "Number of levels in the vocabulary tree.");

DEFINE_int32(lc_num_kmeans_restarts, 5, "Number of restarts for the kmeans.");

namespace loop_closure {
void trainVocabularyTree(
    const std::vector<std::shared_ptr<vi_map::Vertex> >& vertices,
    typename VocabularyTreeBuilder::Tree* tree) {
  CHECK_NOTNULL(tree);
  tree->Clear();

  using loop_closure::DescriptorType;

  // Make a list of references into the binary feature store for the
  // clusterer to use.
  std::vector<DescriptorType> descriptor_refs;
  constexpr int kExpectedDescriptors = 10000;
  descriptor_refs.reserve(kExpectedDescriptors);

  int descriptor_size = -1;
  for (const std::shared_ptr<vi_map::Vertex>& vertex_ptr : vertices) {
    CHECK(vertex_ptr != nullptr);
    const vi_map::Vertex& vertex = *vertex_ptr;
    const unsigned int num_frames = vertex.numFrames();
    for (unsigned int frame_idx = 0; frame_idx < num_frames; ++frame_idx) {
      const aslam::VisualFrame& visual_frame = vertex.getVisualFrame(frame_idx);
      const loop_closure::DescriptorContainer& descriptors =
          visual_frame.getDescriptors();
      if (descriptors.cols() != 0) {
        descriptor_size = descriptors.rows();
        for (int i = 0; i < descriptors.cols(); ++i) {
          DescriptorType descriptor(
              const_cast<unsigned char*>(&descriptors(0, i)),
              descriptors.rows(), false);
          descriptor_refs.push_back(descriptor);
        }
      }
    }
  }

  VLOG(3) << "Creating tree with " << FLAGS_lc_kmeans_splits << " splits on "
          << FLAGS_lc_kmeans_levels << " levels.";

  // Create tree.

  CHECK(!descriptor_refs.empty());
  DescriptorType descriptor_zero(descriptor_size);
  descriptor_zero.SetZero();
  VocabularyTreeBuilder builder(descriptor_zero);
  builder.kmeans().SetRestarts(FLAGS_lc_num_kmeans_restarts);

  VLOG(3) << "Got " << descriptor_refs.size() << " descriptors to train the "
                                                 "vocabulary tree from.";

  builder.Build(
      descriptor_refs, FLAGS_lc_kmeans_splits, FLAGS_lc_kmeans_levels);
  printf("%lu centers\n", builder.tree().centers().size());
  *tree = builder.tree();
}
}  // namespace loop_closure