#include "ter.h"

#include <cstdio>
#include <cassert>
#include <iostream>
#include <limits>
#include <sstream>
#include <tr1/unordered_map>
#include <set>
#include <valarray>
#include <boost/functional/hash.hpp>
#include <stdexcept>
#include "tdict.h"

const bool ter_use_average_ref_len = true;
const int ter_short_circuit_long_sentences = -1;

using namespace std;
using namespace std::tr1;

struct COSTS {
  static const float substitution;
  static const float deletion;
  static const float insertion;
  static const float shift;
};
const float COSTS::substitution = 1.0f;
const float COSTS::deletion = 1.0f;
const float COSTS::insertion = 1.0f;
const float COSTS::shift = 1.0f;

static const int MAX_SHIFT_SIZE = 10;
static const int MAX_SHIFT_DIST = 50;

struct Shift {
  unsigned int d_;
  Shift() : d_() {}
  Shift(int b, int e, int m) : d_() {
    begin(b);
    end(e);
    moveto(m);
  }
  inline int begin() const {
    return d_ & 0x3ff;
  }
  inline int end() const {
    return (d_ >> 10) & 0x3ff;
  }
  inline int moveto() const {
    int m = (d_ >> 20) & 0x7ff;
    if (m > 1024) { m -= 1024; m *= -1; }
    return m;
  }
  inline void begin(int b) {
    d_ &= 0xfffffc00u;
    d_ |= (b & 0x3ff);
  }
  inline void end(int e) {
    d_ &= 0xfff003ffu;
    d_ |= (e & 0x3ff) << 10;
  }
  inline void moveto(int m) {
    bool neg = (m < 0);
    if (neg) { m *= -1; m += 1024; }
    d_ &= 0xfffff;
    d_ |= (m & 0x7ff) << 20;
  }
};

class TERScorerImpl {

 public:
  enum TransType { MATCH, SUBSTITUTION, INSERTION, DELETION };

  explicit TERScorerImpl(const vector<WordID>& ref) : ref_(ref) {
    for (int i = 0; i < ref.size(); ++i)
      rwexists_.insert(ref[i]);
  }

  float Calculate(const vector<WordID>& hyp, int* subs, int* ins, int* dels, int* shifts) const {
    return CalculateAllShifts(hyp, subs, ins, dels, shifts);
  }

  inline int GetRefLength() const {
    return ref_.size();
  }

 private:
  vector<WordID> ref_;
  set<WordID> rwexists_;

  typedef unordered_map<vector<WordID>, set<int>, boost::hash<vector<WordID> > > NgramToIntsMap;
  mutable NgramToIntsMap nmap_;

  static float MinimumEditDistance(
      const vector<WordID>& hyp,
      const vector<WordID>& ref,
      vector<TransType>* path) {
    vector<vector<TransType> > bmat(hyp.size() + 1, vector<TransType>(ref.size() + 1, MATCH));
    vector<vector<float> > cmat(hyp.size() + 1, vector<float>(ref.size() + 1, 0));
    for (int i = 0; i <= hyp.size(); ++i)
      cmat[i][0] = i;
    for (int j = 0; j <= ref.size(); ++j)
      cmat[0][j] = j;
    for (int i = 1; i <= hyp.size(); ++i) {
      const WordID& hw = hyp[i-1];
      for (int j = 1; j <= ref.size(); ++j) {
        const WordID& rw = ref[j-1];
	float& cur_c = cmat[i][j];
	TransType& cur_b = bmat[i][j];

        if (rw == hw) {
          cur_c = cmat[i-1][j-1];
          cur_b = MATCH;
        } else {
          cur_c = cmat[i-1][j-1] + COSTS::substitution;
          cur_b = SUBSTITUTION;
        }
	float cwoi = cmat[i-1][j];
        if (cur_c > cwoi + COSTS::insertion) {
          cur_c = cwoi + COSTS::insertion;
          cur_b = INSERTION;
        }
        float cwod = cmat[i][j-1];
        if (cur_c > cwod + COSTS::deletion) {
          cur_c = cwod + COSTS::deletion;
          cur_b = DELETION;
        }
      }
    }

    // trace back along the best path and record the transition types
    path->clear();
    int i = hyp.size();
    int j = ref.size();
    while (i > 0 || j > 0) {
      if (j == 0) {
        --i;
        path->push_back(INSERTION);
      } else if (i == 0) {
        --j;
        path->push_back(DELETION);
      } else {
        TransType t = bmat[i][j];
        path->push_back(t);
        switch (t) {
          case SUBSTITUTION:
          case MATCH:
            --i; --j; break;
          case INSERTION:
            --i; break;
          case DELETION:
            --j; break;
        }
      }
    }
    reverse(path->begin(), path->end());
    return cmat[hyp.size()][ref.size()];
  }

  void BuildWordMatches(const vector<WordID>& hyp, NgramToIntsMap* nmap) const {
    nmap->clear();
    set<WordID> exists_both;
    for (int i = 0; i < hyp.size(); ++i)
      if (rwexists_.find(hyp[i]) != rwexists_.end())
        exists_both.insert(hyp[i]);
    for (int start=0; start<ref_.size(); ++start) {
      if (exists_both.find(ref_[start]) == exists_both.end()) continue;
      vector<WordID> cp;
      int mlen = min(MAX_SHIFT_SIZE, static_cast<int>(ref_.size() - start));
      for (int len=0; len<mlen; ++len) {
        if (len && exists_both.find(ref_[start + len]) == exists_both.end()) break;
        cp.push_back(ref_[start + len]);
	(*nmap)[cp].insert(start);
      }
    }
  }

  static void PerformShift(const vector<WordID>& in,
    int start, int end, int moveto, vector<WordID>* out) {
    // cerr << "ps: " << start << " " << end << " " << moveto << endl;
    out->clear();
    if (moveto == -1) {
      for (int i = start; i <= end; ++i)
       out->push_back(in[i]);
      for (int i = 0; i < start; ++i)
       out->push_back(in[i]);
      for (int i = end+1; i < in.size(); ++i)
       out->push_back(in[i]);
    } else if (moveto < start) {
      for (int i = 0; i <= moveto; ++i)
       out->push_back(in[i]);
      for (int i = start; i <= end; ++i)
       out->push_back(in[i]);
      for (int i = moveto+1; i < start; ++i)
       out->push_back(in[i]);
      for (int i = end+1; i < in.size(); ++i)
       out->push_back(in[i]);
    } else if (moveto > end) {
      for (int i = 0; i < start; ++i)
       out->push_back(in[i]);
      for (int i = end+1; i <= moveto; ++i)
       out->push_back(in[i]);
      for (int i = start; i <= end; ++i)
       out->push_back(in[i]);
      for (int i = moveto+1; i < in.size(); ++i)
       out->push_back(in[i]);
    } else {
      for (int i = 0; i < start; ++i)
       out->push_back(in[i]);
      for (int i = end+1; (i < in.size()) && (i <= end + (moveto - start)); ++i)
       out->push_back(in[i]);
      for (int i = start; i <= end; ++i)
       out->push_back(in[i]);
      for (int i = (end + (moveto - start))+1; i < in.size(); ++i)
       out->push_back(in[i]);
    }
    if (out->size() != in.size()) {
      cerr << "ps: " << start << " " << end << " " << moveto << endl;
      cerr << "in=" << TD::GetString(in) << endl;
      cerr << "out=" << TD::GetString(*out) << endl;
    }
    assert(out->size() == in.size());
    // cerr << "ps: " << TD::GetString(*out) << endl;
  }

  void GetAllPossibleShifts(const vector<WordID>& hyp,
      const vector<int>& ralign,
      const vector<bool>& herr,
      const vector<bool>& rerr,
      const int min_size,
      vector<vector<Shift> >* shifts) const {
    for (int start = 0; start < hyp.size(); ++start) {
      vector<WordID> cp(1, hyp[start]);
      NgramToIntsMap::iterator niter = nmap_.find(cp);
      if (niter == nmap_.end()) continue;
      bool ok = false;
      int moveto;
      for (set<int>::iterator i = niter->second.begin(); i != niter->second.end(); ++i) {
        moveto = *i;
        int rm = ralign[moveto];
        ok = (start != rm &&
              (rm - start) < MAX_SHIFT_DIST &&
              (start - rm - 1) < MAX_SHIFT_DIST);
        if (ok) break;
      }
      if (!ok) continue;
      cp.clear();
      for (int end = start + min_size - 1;
           ok && end < hyp.size() && end < (start + MAX_SHIFT_SIZE); ++end) {
        cp.push_back(hyp[end]);
	vector<Shift>& sshifts = (*shifts)[end - start];
        ok = false;
        NgramToIntsMap::iterator niter = nmap_.find(cp);
        if (niter == nmap_.end()) break;
        bool any_herr = false;
        for (int i = start; i <= end && !any_herr; ++i)
          any_herr = herr[i];
        if (!any_herr) {
          ok = true;
          continue;
        }
        for (set<int>::iterator mi = niter->second.begin();
             mi != niter->second.end(); ++mi) {
          int moveto = *mi;
	  int rm = ralign[moveto];
	  if (! ((rm != start) &&
	        ((rm < start) || (rm > end)) &&
		(rm - start <= MAX_SHIFT_DIST) &&
		((start - rm - 1) <= MAX_SHIFT_DIST))) continue;
          ok = true;
	  bool any_rerr = false;
	  for (int i = 0; (i <= end - start) && (!any_rerr); ++i)
            any_rerr = rerr[moveto+i];
	  if (!any_rerr) continue;
	  for (int roff = 0; roff <= (end - start); ++roff) {
	    int rmr = ralign[moveto+roff];
	    if ((start != rmr) && ((roff == 0) || (rmr != ralign[moveto])))
	      sshifts.push_back(Shift(start, end, moveto + roff));
	  }
        }
      }
    }
  }

  bool CalculateBestShift(const vector<WordID>& cur,
                          const vector<WordID>& hyp,
                          float curerr,
                          const vector<TransType>& path,
                          vector<WordID>* new_hyp,
                          float* newerr,
                          vector<TransType>* new_path) const {
    vector<bool> herr, rerr;
    vector<int> ralign;
    int hpos = -1;
    for (int i = 0; i < path.size(); ++i) {
      switch (path[i]) {
        case MATCH:
	  ++hpos;
	  herr.push_back(false);
	  rerr.push_back(false);
	  ralign.push_back(hpos);
          break;
        case SUBSTITUTION:
	  ++hpos;
	  herr.push_back(true);
	  rerr.push_back(true);
	  ralign.push_back(hpos);
          break;
        case INSERTION:
	  ++hpos;
	  herr.push_back(true);
          break;
	case DELETION:
	  rerr.push_back(true);
	  ralign.push_back(hpos);
          break;
      }
    }
#if 0
    cerr << "RALIGN: ";
    for (int i = 0; i < rerr.size(); ++i)
      cerr << ralign[i] << " ";
    cerr << endl;
    cerr << "RERR: ";
    for (int i = 0; i < rerr.size(); ++i)
      cerr << (bool)rerr[i] << " ";
    cerr << endl;
    cerr << "HERR: ";
    for (int i = 0; i < herr.size(); ++i)
      cerr << (bool)herr[i] << " ";
    cerr << endl;
#endif

    vector<vector<Shift> > shifts(MAX_SHIFT_SIZE + 1);
    GetAllPossibleShifts(cur, ralign, herr, rerr, 1, &shifts);
    float cur_best_shift_cost = 0;
    *newerr = curerr;
    vector<TransType> cur_best_path;
    vector<WordID> cur_best_hyp;

    bool res = false;
    for (int i = shifts.size() - 1; i >=0; --i) {
      float curfix = curerr - (cur_best_shift_cost + *newerr);
      float maxfix = 2.0f * (1 + i) - COSTS::shift;
      if ((curfix > maxfix) || ((cur_best_shift_cost == 0) && (curfix == maxfix))) break;
      for (int j = 0; j < shifts[i].size(); ++j) {
        const Shift& s = shifts[i][j];
	curfix = curerr - (cur_best_shift_cost + *newerr);
	maxfix = 2.0f * (1 + i) - COSTS::shift;  // TODO remove?
        if ((curfix > maxfix) || ((cur_best_shift_cost == 0) && (curfix == maxfix))) continue;
	vector<WordID> shifted(cur.size());
	PerformShift(cur, s.begin(), s.end(), ralign[s.moveto()], &shifted);
	vector<TransType> try_path;
	float try_cost = MinimumEditDistance(shifted, ref_, &try_path);
	float gain = (*newerr + cur_best_shift_cost) - (try_cost + COSTS::shift);
	if (gain > 0.0f || ((cur_best_shift_cost == 0.0f) && (gain == 0.0f))) {
	  *newerr = try_cost;
	  cur_best_shift_cost = COSTS::shift;
	  new_path->swap(try_path);
	  new_hyp->swap(shifted);
	  res = true;
	  // cerr << "Found better shift " << s.begin() << "..." << s.end() << " moveto " << s.moveto() << endl;
	}
      }
    }

    return res;
  }

  static void GetPathStats(const vector<TransType>& path, int* subs, int* ins, int* dels) {
    *subs = *ins = *dels = 0;
    for (int i = 0; i < path.size(); ++i) {
      switch (path[i]) {
        case SUBSTITUTION:
	  ++(*subs);
        case MATCH:
          break;
        case INSERTION:
          ++(*ins); break;
	case DELETION:
          ++(*dels); break;
      }
    }
  }

  float CalculateAllShifts(const vector<WordID>& hyp,
      int* subs, int* ins, int* dels, int* shifts) const {
    BuildWordMatches(hyp, &nmap_);
    vector<TransType> path;
    float med_cost = MinimumEditDistance(hyp, ref_, &path);
    float edits = 0;
    vector<WordID> cur = hyp;
    *shifts = 0;
    if (ter_short_circuit_long_sentences < 0 ||
        ref_.size() < ter_short_circuit_long_sentences) {
      while (true) {
        vector<WordID> new_hyp;
        vector<TransType> new_path;
        float new_med_cost;
        if (!CalculateBestShift(cur, hyp, med_cost, path, &new_hyp, &new_med_cost, &new_path))
          break;
        edits += COSTS::shift;
        ++(*shifts);
        med_cost = new_med_cost;
        path.swap(new_path);
        cur.swap(new_hyp);
      }
    }
    GetPathStats(path, subs, ins, dels);
    return med_cost + edits;
  }
};

class TERScore : public ScoreBase<TERScore> {
  friend class TERScorer;

 public:
  static const unsigned kINSERTIONS = 0;
  static const unsigned kDELETIONS = 1;
  static const unsigned kSUBSTITUTIONS = 2;
  static const unsigned kSHIFTS = 3;
  static const unsigned kREF_WORDCOUNT = 4;
  static const unsigned kDUMMY_LAST_ENTRY = 5;

 TERScore() : stats(0,kDUMMY_LAST_ENTRY) {}
  float ComputePartialScore() const { return 0.0;}
  float ComputeScore() const {
    float edits = static_cast<float>(stats[kINSERTIONS] + stats[kDELETIONS] + stats[kSUBSTITUTIONS] + stats[kSHIFTS]);
    return edits / static_cast<float>(stats[kREF_WORDCOUNT]);
  }
  void ScoreDetails(string* details) const;
  void PlusPartialEquals(const Score& rhs, int oracle_e_cover, int oracle_f_cover, int src_len){}
  void PlusEquals(const Score& delta, const float scale) {
    if (scale==1)
      stats += static_cast<const TERScore&>(delta).stats;
    if (scale==-1)
      stats -= static_cast<const TERScore&>(delta).stats;
    throw std::runtime_error("TERScore::PlusEquals with scale != +-1");
 }
  void PlusEquals(const Score& delta) {
    stats += static_cast<const TERScore&>(delta).stats;
  }

  ScoreP GetZero() const {
    return ScoreP(new TERScore);
  }
  ScoreP GetOne() const {
    return ScoreP(new TERScore);
  }
  void Subtract(const Score& rhs, Score* res) const {
    static_cast<TERScore*>(res)->stats = stats - static_cast<const TERScore&>(rhs).stats;
  }
  void Encode(std::string* out) const {
    ostringstream os;
    os << stats[kINSERTIONS] << ' '
       << stats[kDELETIONS] << ' '
       << stats[kSUBSTITUTIONS] << ' '
       << stats[kSHIFTS] << ' '
       << stats[kREF_WORDCOUNT];
    *out = os.str();
  }
  bool IsAdditiveIdentity() const {
    for (int i = 0; i < kDUMMY_LAST_ENTRY; ++i)
      if (stats[i] != 0) return false;
    return true;
  }
 private:
  valarray<int> stats;
};

ScoreP TERScorer::ScoreFromString(const std::string& data) {
  istringstream is(data);
  TERScore* r = new TERScore;
  is >> r->stats[TERScore::kINSERTIONS]
     >> r->stats[TERScore::kDELETIONS]
     >> r->stats[TERScore::kSUBSTITUTIONS]
     >> r->stats[TERScore::kSHIFTS]
     >> r->stats[TERScore::kREF_WORDCOUNT];
  return ScoreP(r);
}

void TERScore::ScoreDetails(std::string* details) const {
  char buf[200];
  sprintf(buf, "TER = %.2f, %3d|%3d|%3d|%3d (len=%d)",
     ComputeScore() * 100.0f,
     stats[kINSERTIONS],
     stats[kDELETIONS],
     stats[kSUBSTITUTIONS],
     stats[kSHIFTS],
     stats[kREF_WORDCOUNT]);
  *details = buf;
}

TERScorer::~TERScorer() {
  for (vector<TERScorerImpl*>::iterator i = impl_.begin(); i != impl_.end(); ++i)
    delete *i;
}

TERScorer::TERScorer(const vector<vector<WordID> >& refs) : impl_(refs.size()) {
  for (int i = 0; i < refs.size(); ++i)
    impl_[i] = new TERScorerImpl(refs[i]);
}

ScoreP TERScorer::ScoreCCandidate(const vector<WordID>& hyp) const {
  return ScoreP();
}

ScoreP TERScorer::ScoreCandidate(const std::vector<WordID>& hyp) const {
  float best_score = numeric_limits<float>::max();
  TERScore* res = new TERScore;
  int avg_len = 0;
  for (int i = 0; i < impl_.size(); ++i)
    avg_len += impl_[i]->GetRefLength();
  avg_len /= impl_.size();
  for (int i = 0; i < impl_.size(); ++i) {
    int subs, ins, dels, shifts;
    float score = impl_[i]->Calculate(hyp, &subs, &ins, &dels, &shifts);
    // cerr << "Component TER cost: " << score << endl;
    if (score < best_score) {
      res->stats[TERScore::kINSERTIONS] = ins;
      res->stats[TERScore::kDELETIONS] = dels;
      res->stats[TERScore::kSUBSTITUTIONS] = subs;
      res->stats[TERScore::kSHIFTS] = shifts;
      if (ter_use_average_ref_len) {
        res->stats[TERScore::kREF_WORDCOUNT] = avg_len;
      } else {
        res->stats[TERScore::kREF_WORDCOUNT] = impl_[i]->GetRefLength();
      }

      best_score = score;
    }
  }
  return ScoreP(res);
}


//
// Copyright 2018-2020 Mateusz Loskot <mateusz at loskot dot net>
//
// 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
//
#include <boost/gil/algorithm.hpp>
#include <boost/gil/image.hpp>

#include <boost/core/lightweight_test.hpp>

namespace gil = boost::gil;

void test_lambda_expression()
{
    gil::gray8_pixel_t const gray128(128);
    gil::gray8_image_t image(2, 2, gray128);

    int sum{0};
    gil::for_each_pixel(gil::view(image), [&sum](gil::gray8_pixel_t& p) {
        sum += gil::at_c<0>(p);
    });
    BOOST_TEST(sum == 2 * 2 * 128);
}

int main()
{
    test_lambda_expression();

    return ::boost::report_errors();
}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/integral_constant.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/comparable.hpp>
#include <laws/foldable.hpp>
#include <laws/iterable.hpp>
#include <laws/searchable.hpp>

#include <type_traits>
namespace hana = boost::hana;

int main() {
    auto ranges = hana::make_tuple(
          hana::make_range(hana::int_c<0>, hana::int_c<0>)
        , hana::make_range(hana::int_c<0>, hana::int_c<1>)
        , hana::make_range(hana::int_c<0>, hana::int_c<2>)
        , hana::make_range(hana::int_c<1>, hana::int_c<1>)
        , hana::make_range(hana::int_c<1>, hana::int_c<2>)
        , hana::make_range(hana::int_c<1>, hana::int_c<3>)
        , hana::make_range(hana::int_c<50>, hana::int_c<60>)

        , hana::make_range(hana::int_c<50>, hana::long_c<60>)
        , hana::make_range(hana::long_c<50>, hana::int_c<60>)
    );

    auto integers = hana::tuple_c<int, 0, 1, 900>;

    hana::test::TestComparable<hana::range_tag>{ranges};
    hana::test::TestFoldable<hana::range_tag>{ranges};
    hana::test::TestIterable<hana::range_tag>{ranges};
    hana::test::TestSearchable<hana::range_tag>{ranges, integers};
}


#include <lib/lib.hpp>
#include <lib/lib_test_support.hpp>

#include <rapidcheck.h>

#include <boost/test/unit_test.hpp>
// needs to be included after any boost test headers
#include <rapidcheck/boost_test.h>

#include <ostream>
#include <string>

using ::xzr::lib::add;
using ::xzr::lib::from_str;
using ::xzr::lib::to_str;
using ::xzr::lib::ymd;
using ::xzr::lib::ymd_str;

namespace
{
BOOST_AUTO_TEST_SUITE(lib_tests)

RC_BOOST_PROP(exchanging_parameter_yields_same_result, (int a, int b))
{
    const int res_a{add(a, b)};
    const int res_b{add(b, a)};
    RC_ASSERT(res_a == res_b);
}

RC_BOOST_PROP(adding_one_twice_is_same_as_adding_two, (int a))
{
    const int res_a{add(add(a, 1), 1)};
    const int res_b{add(a, 2)};
    RC_ASSERT(res_a == res_b);
}

RC_BOOST_PROP(adding_zero_does_nothing, (int a))
{
    const int res_a{add(a, 0)};
    RC_ASSERT(res_a == a);
}

RC_BOOST_PROP(ymd_from_string, (ymd rand_ymd))
{
    //! [ymd_str_conv]
    RC_ASSERT(rand_ymd == from_str(to_str(rand_ymd)));
    //! [ymd_str_conv]
}

RC_BOOST_PROP(ymd_to_string, (ymd_str rand_ymd_str))
{
    RC_ASSERT(rand_ymd_str == to_str(from_str(rand_ymd_str)));
}

BOOST_AUTO_TEST_SUITE_END()
}


// Copyright (C) 2009-2012 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0
// (see accompanying file LICENSE_1_0.txt or a copy at
// http://www.boost.org/LICENSE_1_0.txt)
// Home at http://www.boost.org/libs/local_function

#include <boost/local_function.hpp>
#include <boost/detail/lightweight_test.hpp>
#include <algorithm>

//[add_seq
int main(void) {
    int sum = 0, factor = 10;

    void BOOST_LOCAL_FUNCTION( (const bind factor) (bind& sum) (int num) ) {
        sum += factor * num;
    } BOOST_LOCAL_FUNCTION_NAME(add)

    add(1);
    int nums[] = {2, 3};
    std::for_each(nums, nums + 2, add);

    BOOST_TEST(sum == 60);
    return boost::report_errors();
}
//]


#pragma once

#include "util.hpp"
#include "habitat.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>

#ifndef GRAPH_H
#define GRAPH_H

struct node {
    vector<int> neighbors;
    int label;
};

/*
 Triangular graph, possibly irregularly shaped. (The shape is determined by the habitat outline.)
 Or alternatively, the function Graph::generate_grid() can be modified to load a pre-constructed
 population grid, with samples already assigned to vertices.
 */

class Graph {
public:

    Graph( );
    ~Graph( );

    void generate_grid(const string &datapath, const string &gridpath, const Habitat &habitat,
                       const int nDemeDensity, const int nIndiv);
    bool dlmwrite_grid(const string &mcmcpath) const;

    bool is_connected() const;
    int get_num_edges() const;
    int get_num_obsrv_demes() const;
    int get_num_total_demes() const;
    int get_deme_of_indiv(const int i) const;
    void get_edge(int edge, int &alpha, int &beta) const;
    void index_closest_to_deme(const MatrixXd &X, VectorXi &Closest) const;
    MatrixXd get_the_obsrv_demes() const;
    node get_node(int deme) const;
    int get_label(node n) const;
    VectorXi get_indiv2deme() const;

private:

    // DemeCoord: list of vertices (one vertex per row, with two coordinates)
    // DemePairs: pairs of connected vertices (one edge per row)
    MatrixXd DemeCoord;
    VectorXi DemeSizes, indiv2deme;
    MatrixXi DemePairs;
    vector<node> nodes;

    int neighbors_in_grid(const int r1, const int c1, int &r2, int &c2, const int pos,
                          const int nx, const int ny) const;
    void make_triangular_grid(const Habitat &habitat, const int nDemeDensity);
    void map_indiv_to_deme(const string &datapath, const int nIndiv);
    void reindex_demes();
    void populate_nodes();

    // The grid to read does not need to be triangular as long as it is connected
    bool read_input_grid(const string &datapath, MatrixXd &DemeCoord, MatrixXi &DemePairs);
    bool read_indiv_to_deme(const string &datapath, const int nDemes, VectorXi &indiv2deme);

};

#endif


/* boost random/uniform_int.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_int.hpp,v 1.21.2.1 2003/03/17 01:39:15 beman_dawes Exp $
 *
 * Revision history
 *  2001-04-08  added min<max assertion (N. Becker)
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_INT_HPP
#define BOOST_RANDOM_UNIFORM_INT_HPP

#include <cassert>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/detail/signed_unsigned_compare.hpp>
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#include <boost/type_traits/is_float.hpp>
#endif

namespace boost {

// uniform integer distribution on [min, max]

namespace detail {

template<class UniformRandomNumberGenerator, class IntType>
class uniform_int_integer
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  uniform_int_integer(base_type & rng, IntType min, IntType max)
    : _rng(&rng), _bmin(_rng->min()), _brange(_rng->max() - _bmin)
  {
    assert(min < max);
    set(min, max);
  }

  void set(result_type min, result_type max)
  {
    _min = min;
    _max = max;
    _range = _max - _min;

    if(random::equal_signed_unsigned(_brange, _range))
      _range_comparison = 0;
    else if(random::lessthan_signed_unsigned(_brange, _range))
      _range_comparison = -1;
    else
      _range_comparison = 1;
  }

  result_type min() const { return _min; }
  result_type max() const { return _max; }
  base_type& base() const { return *_rng; }

  result_type operator()();

private:
  typedef typename base_type::result_type base_result;
  base_type * _rng;
  result_type _min, _max, _range;
  base_result _bmin, _brange;
  int _range_comparison;
};

template<class UniformRandomNumberGenerator, class IntType>
inline IntType uniform_int_integer<UniformRandomNumberGenerator, IntType>::operator()()
{
  if(_range_comparison == 0) {
    // this will probably never happen in real life
    // basically nothing to do; just take care we don't overflow / underflow
    return static_cast<result_type>((*_rng)() - _bmin) + _min;
  } else if(_range_comparison < 0) {
    // use rejection method to handle things like 0..3 --> 0..4
    for(;;) {
      // concatenate several invocations of the base RNG
      // take extra care to avoid overflows
      result_type limit;
      if(_range == std::numeric_limits<result_type>::max()) {
        limit = _range/(result_type(_brange)+1);
        if(_range % result_type(_brange)+1 == result_type(_brange))
          ++limit;
      } else {
        limit = (_range+1)/(result_type(_brange)+1);
      }
      // we consider "result" as expressed to base (_brange+1)
      // for every power of (_brange+1), we determine a random factor
      result_type result = result_type(0);
      result_type mult = result_type(1);
      while(mult <= limit) {
        result += ((*_rng)() - _bmin) * mult;
        mult *= result_type(_brange)+result_type(1);
      }
      if(mult == limit)
        // _range+1 is an integer power of _brange+1: no rejections required
        return result;
      // _range/mult < _brange+1  -> no endless loop
      result += uniform_int_integer<base_type,result_type>(*_rng, 0, _range/mult)() * mult;
      if(result <= _range)
        return result + _min;
    }
  } else {                   // brange > range
    if(_brange / _range > 4 /* quantization_cutoff */ ) {
      // the new range is vastly smaller than the source range,
      // so quantization effects are not relevant
      return boost::uniform_smallint<base_type,result_type>(*_rng, _min, _max)();
    } else {
      // use rejection method to handle things like 0..5 -> 0..4
      for(;;) {
        base_result result = (*_rng)() - _bmin;
        // result and range are non-negative, and result is possibly larger
        // than range, so the cast is safe
        if(result <= static_cast<base_result>(_range))
          return result + _min;
      }
    }
  }
}

template<class UniformRandomNumberGenerator, class IntType>
class uniform_int_float
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  uniform_int_float(base_type & rng, IntType min, IntType max)
    : _rng(rng)
  {
    set(min,max);
  }

  void set(result_type min, result_type max)
  {
    _min = min;
    _max = max;
    _range = static_cast<base_result>(_max-_min)+1;
  }

  result_type min() const { return _min; }
  result_type max() const { return _max; }
  base_type& base() const { return _rng.base(); }

  result_type operator()()
  {
    return static_cast<IntType>(_rng() * _range) + _min;
  }

private:
  typedef typename base_type::result_type base_result;
  uniform_01<base_type> _rng;
  result_type _min, _max;
  base_result _range;
};

// simulate partial specialization
template<bool is_integer>
struct uniform_int;

template<>
struct uniform_int<true>
{
  template<class UniformRandomNumberGenerator, class IntType>
  struct impl
  {
    typedef uniform_int_integer<UniformRandomNumberGenerator, IntType> type
;
  };
};

template<>
struct uniform_int<false>
{
  template<class UniformRandomNumberGenerator, class IntType>
  struct impl
  {
    typedef uniform_int_float<UniformRandomNumberGenerator, IntType> type;
  };
};

} // namespace detail

template<class UniformRandomNumberGenerator, class IntType = int>
class uniform_int
{
private:
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
  typedef typename detail::uniform_int<std::numeric_limits<typename UniformRandomNumberGenerator::result_type>::is_integer>::BOOST_NESTED_TEMPLATE impl<UniformRandomNumberGenerator, IntType>::type impl_type;
#elif BOOST_WORKAROUND( __BORLANDC__, BOOST_TESTED_AT(0x0570) )
  typedef typename detail::uniform_int< boost::is_float<typename UniformRandomNumberGenerator::result_type>::value == false >::BOOST_NESTED_TEMPLATE impl<UniformRandomNumberGenerator, IntType>::type impl_type;
#else
  BOOST_STATIC_CONSTANT(bool, base_float = (boost::is_float<typename UniformRandomNumberGenerator::result_type>::value == false));
  typedef typename detail::uniform_int<base_float>::BOOST_NESTED_TEMPLATE impl<UniformRandomNumberGenerator, IntType>::type impl_type;
#endif

public:
  typedef uniform_int<UniformRandomNumberGenerator, IntType> adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

  explicit uniform_int(base_type & rng, IntType min = 0, IntType max = 9)
    : impl(rng, min, max)
  {
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    // MSVC fails BOOST_STATIC_ASSERT with std::numeric_limits at class scope
    BOOST_STATIC_ASSERT(std::numeric_limits<IntType>::is_integer);
#endif
  }

  result_type min() const { return impl.min(); }
  result_type max() const { return impl.max(); }
  adaptor_type& adaptor() { return *this; }
  base_type& base() const { return impl.base(); }
  void reset() { }

  result_type operator()() { return impl(); }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const uniform_int& x, const uniform_int& y)
  { return x.min() == y.min() && x.max() == y.max() && x.base() == y.base(); }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const uniform_int& ud)
  {
    os << ud.min() << " " << ud.max();
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, uniform_int& ud)
  {
# if BOOST_WORKAROUND(_MSC_FULL_VER, BOOST_TESTED_AT(13102292)) && BOOST_MSVC > 1300
      return detail::extract_uniform_int(is, ud, ud.impl);
# else
    IntType min, max;
    is >> std::ws >> min >> std::ws >> max;
    ud.impl.set(min, max);
    return is;
# endif
  }
#endif

#else
  // Use a member function
  bool operator==(const uniform_int& rhs) const
  { return min() == rhs.min() && max() == rhs.max() && base() == rhs.base();  }
#endif
private:
  impl_type impl;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UniformRandomNumberGenerator, class IntType>
const bool uniform_int<UniformRandomNumberGenerator, IntType>::has_fixed_range;
#endif

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_INT_HPP


//==============================================================================
//         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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_TOUINT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_TOUINT_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/touint.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::touint_, tag::cpu_ , (A0)
                            , (scalar_< integer_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned> ::type result_type;

    BOOST_FORCEINLINE  result_type
    operator()(A0 const& a0) const
    {
      return a0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::touint_, tag::cpu_, (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned> ::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      if (boost::simd::is_nan(a0))       return Zero<result_type>();
      if (a0 == boost::simd::Inf<A0>())  return boost::simd::Valmax<result_type>();
      if (a0 == boost::simd::Minf<A0>())  return boost::simd::Valmin<result_type>();
      return result_type(a0);
    }
  };
} } }

#endif


/**
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include <gtest/gtest.h>

#include <fstream>
#include <memory>
#include <typeinfo>

#include <logog/include/logog.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include "BaseLib/BuildInfo.h"
#include "NumLib/NumericsConfig.h"
#include "NumLib/ODESolver/ConvergenceCriterionDeltaX.h"
#include "MathLib/LinAlg/LinAlg.h"
#include "Tests/TestTools.h"
#include "ODEs.h"

#include "TimeLoopSingleODE.h"

#ifndef USE_PETSC
std::unique_ptr<GlobalLinearSolver>
createLinearSolver()
{
    return std::make_unique<GlobalLinearSolver>("", nullptr);
}
#else
std::unique_ptr<GlobalLinearSolver>
createLinearSolver()
{
    const char xml[] =
        "<petsc><parameters>"
        "-ksp_type bcgs -pc_type sor -ksp_rtol 1e-24 -ksp_max_it 100 -ksp_initial_guess_nonzero false"
        "</parameters></petsc>";
    auto const ptree = readXml(xml);
    BaseLib::ConfigTree config(ptree, "", BaseLib::ConfigTree::onerror,
                               BaseLib::ConfigTree::onwarning);
    return std::make_unique<GlobalLinearSolver>("", &config);
}
#endif

struct Solution
{
    std::vector<double> ts;
    std::vector<GlobalVector> solutions;
};

template<NumLib::NonlinearSolverTag NLTag>
class TestOutput
{
public:
    using TimeDisc = NumLib::TimeDiscretization;
    using NLSolver = NumLib::NonlinearSolver<NLTag>;

    template <class ODE>
    Solution run_test(ODE& ode, TimeDisc& timeDisc,
                              const unsigned num_timesteps)
    {
        using ODE_ = ODE;
        using ODET = ODETraits<ODE>;

        Solution sol;

        const int process_id = 0;
        NumLib::TimeDiscretizedODESystem<ODE_::ODETag, NLTag>
                ode_sys(process_id, ode, timeDisc);

        auto linear_solver = createLinearSolver();
        auto conv_crit = std::make_unique<NumLib::ConvergenceCriterionDeltaX>(
            _tol, boost::none, MathLib::VecNormType::NORM2);
        auto nonlinear_solver =
            std::make_unique<NLSolver>(*linear_solver, _maxiter);

        NumLib::TimeLoopSingleODE<NLTag> loop(ode_sys, std::move(linear_solver),
                                              std::move(nonlinear_solver),
                                              std::move(conv_crit));

        const double t0      = ODET::t0;
        const double t_end   = ODET::t_end;
        const double delta_t = (num_timesteps == 0) ? -1.0
                                                    : ((t_end-t0) / num_timesteps);

        DBUG("Running test with %u timesteps of size %g s.", num_timesteps,
             delta_t);

        // initial condition
        GlobalVector x0(ode.getMatrixSpecifications(process_id).nrows);
        ODET::setIC(x0);

        sol.ts.push_back(t0);
        sol.solutions.push_back(x0);

        auto cb = [&sol](const double t, GlobalVector const& x) {
            sol.ts.push_back(t);
            sol.solutions.push_back(x);
        };

        if (num_timesteps > 0)
        {
            EXPECT_TRUE(loop.loop(t0, x0, t_end, delta_t, cb).error_norms_met);
        }

        for (auto& x : sol.solutions)
        {
            MathLib::LinAlg::setLocalAccessibleVector(x);
        }

        return sol;
    }

private:
    const double _tol = 1e-9;
    const unsigned _maxiter = 20;
};

template <typename TimeDisc, typename ODE, NumLib::NonlinearSolverTag NLTag>
typename std::enable_if<std::is_same<TimeDisc, NumLib::BackwardEuler>::value,
                        Solution>::type
run_test_case(const unsigned num_timesteps)
{
    ODE ode;
    TimeDisc timeDisc;

    TestOutput<NLTag> test;
    return test.run_test(ode, timeDisc, num_timesteps);
}

template <typename TimeDisc, typename ODE, NumLib::NonlinearSolverTag NLTag>
typename std::enable_if<std::is_same<TimeDisc, NumLib::ForwardEuler>::value,
                        Solution>::type
run_test_case(const unsigned num_timesteps)
{
    ODE ode;
    TimeDisc timeDisc;

    TestOutput<NLTag> test;
    return test.run_test(ode, timeDisc, num_timesteps);
}

template <typename TimeDisc, typename ODE, NumLib::NonlinearSolverTag NLTag>
typename std::enable_if<std::is_same<TimeDisc, NumLib::CrankNicolson>::value,
                        Solution>::type
run_test_case(const unsigned num_timesteps)
{
    ODE ode;
    TimeDisc timeDisc(0.5);

    TestOutput<NLTag> test;
    return test.run_test(ode, timeDisc, num_timesteps);
}

template <typename TimeDisc, typename ODE, NumLib::NonlinearSolverTag NLTag>
typename std::enable_if<
    std::is_same<TimeDisc, NumLib::BackwardDifferentiationFormula>::value,
    Solution>::type
run_test_case(const unsigned num_timesteps)
{
    ODE ode;
    TimeDisc timeDisc(3);

    TestOutput<NLTag> test;
    return test.run_test(ode, timeDisc, num_timesteps);
}

// This class is only here s.t. I don't have to put the members into
// the definition of the macro TCLITEM below.
template <class ODE_, class TimeDisc_>
struct TestCaseBase
{
    using ODE = ODE_;
    using TimeDisc = TimeDisc_;
};

template <class ODE_, class TimeDisc_>
struct TestCase;

// /////////////////////////////////////
//
//  Put new test cases to that list
//
//  Arguments are: The ODE, the used time discretization
//                 the absolute infinity-norm tolerance when comparing Picard to
//                 Newton results, and the absolute infinity-norm tolerance when
//                 comparing the numerical to the analytical solution
//
// /////////////////////////////////////
#define TESTCASESLIST \
    TCLITEM(ODE1, BackwardEuler                 , 1e-14  , 0.2) TCLSEP \
    TCLITEM(ODE1, ForwardEuler                  , 1e-13  , 0.22) TCLSEP \
    TCLITEM(ODE1, CrankNicolson                 , 1e-14  , 2.1e-3) TCLSEP \
    TCLITEM(ODE1, BackwardDifferentiationFormula, 1e-14  , 3e-3) TCLSEP \
    \
    TCLITEM(ODE2, BackwardEuler                 , 1.5e-10, 2e-3) TCLSEP \
    TCLITEM(ODE2, ForwardEuler                  , 2e-3   , 2e-3) TCLSEP \
    TCLITEM(ODE2, CrankNicolson                 , 1.5e-10, 8e-6) TCLSEP \
    TCLITEM(ODE2, BackwardDifferentiationFormula, 1.5e-10, 2e-4) TCLSEP \
    \
    TCLITEM(ODE3, BackwardEuler                 , 1e-9   , 0.028) TCLSEP \
    TCLITEM(ODE3, ForwardEuler                  , 1e-13  , 0.03) TCLSEP \
    TCLITEM(ODE3, CrankNicolson                 , 2e-9   , 6e-4) TCLSEP \
    TCLITEM(ODE3, BackwardDifferentiationFormula, 2e-9   , 7e-4)

#define TCLITEM(ODE, TIMEDISC, TOL_PICARD_NEWTON, TOL_ANALYT)                \
    template <>                                                              \
    struct TestCase<ODE, NumLib::TIMEDISC>                                   \
        : TestCaseBase<ODE, NumLib::TIMEDISC> {                              \
        static const double tol_picard_newton;                               \
        static const double tol_analyt;                                      \
    };                                                                       \
    const double TestCase<ODE, NumLib::TIMEDISC>::tol_picard_newton =        \
        (TOL_PICARD_NEWTON);                                                 \
    const double TestCase<ODE, NumLib::TIMEDISC>::tol_analyt = (TOL_ANALYT);
#define TCLSEP

TESTCASESLIST

#undef TCLITEM
#undef TCLSEP

#define TCLITEM(ODE, TIMEDISC, TOL_PICARD_NEWTON, TOL_ANALYT) \
    TestCase<ODE, NumLib::TIMEDISC>
#define TCLSEP ,

using TestCases = ::testing::Types<TESTCASESLIST>;

#undef TESTCASESLIST
#undef TCLSEP
#undef TCLITEM

template<class TestParams>
class NumLibODEIntTyped : public ::testing::Test
{
public:
    using ODE      = typename TestParams::ODE;
    using TimeDisc = typename TestParams::TimeDisc;

    static void test()
    {
        const unsigned num_timesteps = 100;

        auto const sol_picard =
            run_test_case<TimeDisc, ODE, NumLib::NonlinearSolverTag::Picard>(
                num_timesteps);

        auto const sol_newton =
            run_test_case<TimeDisc, ODE, NumLib::NonlinearSolverTag::Newton>(
                num_timesteps);

        ASSERT_EQ(sol_picard.ts.size(), sol_newton.ts.size());
        for (std::size_t i = 0; i < sol_picard.ts.size(); ++i)
        {
            ASSERT_EQ(sol_picard.ts[i], sol_newton.ts[i]);
            for (int comp = 0;
                 comp < static_cast<int>(sol_picard.solutions[i].size());
                 ++comp)
            {
                EXPECT_NEAR(sol_picard.solutions[i][comp],
                            sol_newton.solutions[i][comp],
                            TestParams::tol_picard_newton);

                auto const t = sol_picard.ts[i];
                auto sol_analyt = ODETraits<ODE>::solution(t);
                MathLib::LinAlg::setLocalAccessibleVector(sol_analyt);

                EXPECT_NEAR(sol_picard.solutions[i][comp], sol_analyt[comp],
                            TestParams::tol_analyt);
                EXPECT_NEAR(sol_newton.solutions[i][comp], sol_analyt[comp],
                            TestParams::tol_analyt);
            }
        }
    }
};

TYPED_TEST_CASE(NumLibODEIntTyped, TestCases);

// Temporarily disabled for PETSc issue #1989
#ifndef USE_PETSC
TYPED_TEST(NumLibODEIntTyped, T1)
#else
TYPED_TEST(NumLibODEIntTyped, DISABLED_T1)
#endif
{
    TestFixture::test();
}

/* TODO Other possible test cases:
 *
 * * check that the order of time discretization scales correctly
 *   with the timestep size
 */


#include "mp/following_graph.hpp"

#include <sstream>
#include <unordered_map>
#include <type_traits>

#include <boost/graph/connected_components.hpp>
#include <pugixml/pugixml.hpp>

#include "mp/following_detection.hpp"
#include "mp/page_rank.hpp"

namespace mp {

following_graph following_graph_at(const following_data &f, i64 timestamp)
{
    if (timestamp < f.begin_timestamp || timestamp > f.end_timestamp) {
        throw std::logic_error("timestamp is not in range: " + std::to_string(timestamp));
    }

    using vertex = following_graph::vertex_descriptor;
    using vertex_map = std::unordered_map<i32, vertex>;

    following_graph g;
    vertex_map vertices; // A mapping from device-id to vertex-id.
    for (size_t i = 0; i < f.devices.size(); ++i) {
        vertex_data d{f.devices[i]};
        vertex v = add_vertex(std::move(d), g);
        vertices[i] = v;
    }
    auto get_vertex = [&](i32 device_id) -> vertex {
        auto iter = vertices.find(device_id);
        if (iter != vertices.end()) {
            return iter->second;
        }
        throw std::logic_error("vertex for id does not exist: " + device_id);
    };

    // The data at "timestamp" can be thought of as
    // the list of edges for the result graph.
    auto &data = f.data_at(timestamp);
    for (auto &pair : data.co_moving) {
        auto left = get_vertex(pair.left);
        auto right = get_vertex(pair.right);

        edge_data d;
        d.weight = std::abs(pair.lag);

        switch (pair.type) {
        case following_type::following:
            add_edge(left, right, d, g);
            break;
        case following_type::leading:
            add_edge(right, left, d, g);
            break;
        case following_type::co_leading:
            add_edge(left, right, d, g);
            add_edge(right, left, d, g);
            break;
        }
    }

    return g;
}

// Takes a following graph as input
// and computes the set of connected components.
// The edges and vertices of the following_graph must not be modified
// during the lifetime of an instance of graph_components.
class graph_components
{
private:
    using undirected_graph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS>;

    using follow_vertex = boost::graph_traits<following_graph>::vertex_descriptor;
    using undir_vertex  = boost::graph_traits<following_graph>::vertex_descriptor;
    using vertex_map    = std::unordered_map<follow_vertex, undir_vertex>;

    // Takes a following_graph and constructs an undirected graph
    // with the same vertices and edges.
    // A mapping from following_graph vertex -> undirected_graph vertex will be stored in "mapping".
    static void make_undirected(const following_graph &g, undirected_graph &ug, vertex_map &mapping)
    {
        ug = undirected_graph();
        mapping.clear();
        mapping.reserve(num_vertices(g));

        for (auto vertex : make_iter_range(vertices(g))) {
            auto un_vertex = add_vertex(ug);
            mapping[vertex] = un_vertex;
        }
        for (auto edge : make_iter_range(edges(g))) {
            auto un_source = mapping[source(edge, g)];
            auto un_target = mapping[target(edge, g)];
            add_edge(un_source, un_target, ug);
        }
    }

public:
    graph_components(const following_graph &g)
        : ug()
        , num_components(0)
        , mapping()
        , vertex_components(num_vertices(g))
    {
        make_undirected(g, ug, mapping);
        num_components = boost::connected_components(
                    ug, boost::make_iterator_property_map(
                        vertex_components.begin(), get(boost::vertex_index, ug)));
    }

    int components() const
    {
        return num_components;
    }

    int component_of(following_graph::vertex_descriptor v) const
    {
        auto it = mapping.find(v);
        assert(it != mapping.end());

        auto uv = it->second;
        auto ui = get(boost::vertex_index, ug, uv);
        return vertex_components[ui];
    }

private:
    undirected_graph ug;
    int num_components;
    vertex_map mapping;
    vector<int> vertex_components;
};

// Stop condition for the PageRank algorithm.
// The algorithm will stop running if
// a) the absolute error (as measured by euclidean distance) is smaller
//    than "error" or
// b) the maximum number of iterations has been reached (as specified by "limit").
template<typename LastResultMap>
struct page_rank_stop
{
    page_rank_stop(double error, int limit, LastResultMap last)
        : counter(0)
        , limit(limit)
        , error(error)
        , last(last)
    {}

    int counter;
    int limit;
    double error;
    LastResultMap last;

    template<typename RankMap, typename Graph>
    bool operator()(const RankMap &result, const Graph &g)
    {
        if (++counter >= limit) {
            return true;
        }

        double err = 0.0;
        for (auto vertex : make_iter_range(vertices(g))) {
            double rank = get(result, vertex);
            double last_rank = get(last, vertex);
            double diff = rank - last_rank;

            err += diff * diff;
            put(last, vertex, rank);
        }
        return std::sqrt(err) <= error;
    }
};

vector<string> detect_leaders(const following_graph &g, bool use_weights)
{
    static constexpr double error = 0.000001;
    static constexpr double damping_factor = 0.85;
    static constexpr int    max_iterations = 500;

    // Run PageRank
    vector<double> ranks(num_vertices(g));
    auto rank_map = boost::make_iterator_property_map(
                ranks.begin(), get(boost::vertex_index, g));
    {
        vector<double> last_ranks(ranks);
        auto last_rank_map = boost::make_iterator_property_map(
                    last_ranks.begin(), get(boost::vertex_index, g));
        // All edges have the same weight for now.
        auto stop = page_rank_stop<decltype(last_rank_map)>(
                    error, max_iterations, last_rank_map);

        if (use_weights) {
            auto weight_map = get(&edge_data::weight, g);
            page_rank(g, rank_map, damping_factor, weight_map, stop);
        } else {
            auto weight_map = boost::make_static_property_map<
                    following_graph::edge_descriptor, double>(1.0);
            page_rank(g, rank_map, damping_factor, weight_map, stop);
        }
    }

    // Find leaders by taking the node with the hightest
    // PageRank value in each connected component.
    vector<string> leaders;
    graph_components c(g);
    for (int i = 0; i < c.components(); ++i) {
        double max_rank = 0;
        following_graph::vertex_descriptor max_vertex{};

        for (auto vertex : make_iter_range(vertices(g))) {
            if (c.component_of(vertex) == i) {
                double rank = get(rank_map, vertex);
                if (rank >= max_rank) {
                    max_rank = rank;
                    max_vertex = vertex;
                }
            }
        }
        // There is at least one vertex in each component,
        // thus max_vertex is valid.
        leaders.push_back(g[max_vertex].name);
    }
    return leaders;
}

vector<vector<string>> detect_groups(const following_graph &g)
{
    graph_components c(g);
    vector<vector<string>> result(c.components());

    for (auto vertex : make_iter_range(vertices(g))) {
        int group = c.component_of(vertex);
        const string &name = g[vertex].name;
        result[group].push_back(name);
    }
    return result;
}

leader_data detect_leaders(const following_data &fd, bool use_weights)
{
    leader_data ld;
    ld.begin_timestamp = fd.begin_timestamp;
    ld.end_timestamp = fd.end_timestamp;
    ld.duration = fd.duration;
    ld.devices = fd.devices;
    ld.timestamps.resize(static_cast<size_t>(ld.duration));

    for (i64 ts = ld.begin_timestamp; ts <= ld.end_timestamp; ++ts) {
        auto &data = ld.data_at(ts);
        data.timestamp = ts;
        data.leaders = detect_leaders(following_graph_at(fd, ts), use_weights);
    }
    return ld;
}

void to_graphml(const following_graph &g, std::ostream &o)
{
    using namespace pugi;

    xml_document doc;
    xml_node root = doc.append_child("graphml");
    root.append_attribute("xmlns") = "http://graphml.graphdrawing.org/xmlns";

    xml_node attr_weight = root.append_child("key");
    attr_weight.append_attribute("id") = "weight";
    attr_weight.append_attribute("for") = "edge";
    attr_weight.append_attribute("attr.name") = "weight";
    attr_weight.append_attribute("attr.type") = "double";

    xml_node graph = root.append_child("graph");
    graph.append_attribute("id") = "G";
    graph.append_attribute("edgedefault") = "directed";

    for (auto vertex : make_iter_range(vertices(g))) {
        const vertex_data &d = g[vertex];

        xml_node node = graph.append_child("node");
        node.append_attribute("id") = d.name.c_str();
    }

    for (auto edge : make_iter_range(edges(g))) {
        const vertex_data &sd = g[source(edge, g)];
        const vertex_data &td = g[target(edge, g)];

        xml_node node = graph.append_child("edge");
        node.append_attribute("source") = sd.name.c_str();
        node.append_attribute("target") = td.name.c_str();

        xml_node weight_node = node.append_child("data");
        weight_node.append_attribute("key") = "weight";
        weight_node.text() = g[edge].weight;
    }

    doc.save(o);
}

} // namespace mp


//==============================================================================
//          Copyright 2015 J.T.  Lapreste
//
//          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 BOOST_SIMD_BITWISE_FUNCTIONS_BITSET_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_BITSET_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  bitset generic tag

      Represents the bitset function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct bitset_ : ext::elementwise_<bitset_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<bitset_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_bitset_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::bitset_, Site> dispatching_bitset_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::bitset_, Site>();
    }
    template<class... Args>
    struct impl_bitset_;
  }
  /*!
    Returns x with the ith bit set

    @par semantic:
    For any given value @c x of type @c T, i  of type @c I:

    @code
    as_integer<T> r = bitset(x, i);
    @endcode

    @see  @funcref{bitset}
    @param  x
    @param  i

    @return      a value of the type of the first input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::bitset_, bitset, 2)
} }

#endif


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/constant/log10_2lo.hpp>
#include <boost/simd/constant/log10_2hi.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check log10_2lo behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_2lo;
  using boost::simd::Log10_2lo;
  T ref = T(0);
  STF_TYPE_IS(decltype(Log10_2lo<T>()), T);
  STF_EQUAL(Log10_2lo<T>(), ref);
  STF_EQUAL(log10_2lo( as(T{}) ),ref);
}

STF_CASE_TPL( "Check log10_2lo behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_2lo;
  using boost::simd::Log10_2lo;
  T ref = T(4.6050389811952140e-6);

  STF_TYPE_IS(decltype(Log10_2lo<T>()), T);
  STF_IEEE_EQUAL(Log10_2lo<T>(), ref);
  STF_IEEE_EQUAL(log10_2lo( as(T{}) ), ref);
}

STF_CASE_TPL( "Check log10_2lo behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_2lo;
  using boost::simd::Log10_2lo;
  using boost::simd::Log10_2hi;
  using boost::simd::Eps;
  using boost::simd::abs;
  double Log10_2 = 0.301029995663981195213738894724;
  STF_LESS(abs(float(double(Log10_2hi<float>())+double(Log10_2lo<float>())- Log10_2)), Eps<float>());
  T ref = T(2.4874566588550806e-4);
  STF_TYPE_IS(decltype(Log10_2lo<T>()), T);
  STF_IEEE_EQUAL(Log10_2lo<T>(), ref);
  STF_IEEE_EQUAL(log10_2lo( as(T{}) ), ref);
}


/* Copyright (c) 2012, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See LICENSE.txt or
 * http://www.opensource.org/licenses/mit-license.php */

#include <baseMeasure.hpp>
#include <hdp_gibbs.hpp>

#include <armadillo>

#include <boost/python.hpp>
#include <boost/python/wrapper.hpp>
#include <numpy/ndarrayobject.h> // for PyArrayObject

#ifdef PYTHON_2_6
  #include <python2.6/object.h> // for PyArray_FROM_O
#endif
#ifdef PYTHON_2_7
  #include <python2.7/object.h> // for PyArray_FROM_O
#endif

using namespace boost::python;

template <class U>
class HDP_gibbs_py : public HDP_gibbs<U>
{
public:
  HDP_gibbs_py(const BaseMeasure<U>& base, double alpha, double gamma)
  : HDP_gibbs<U>(base,alpha,gamma)
  {
    //cout<<"Creating "<<typeid(this).name()<<endl;
  };

  bool densityEst(uint32_t Nw, uint32_t K0, uint32_t T0, uint32_t It)
  {
//    cout<<"mX.size()="<<HDP<U>::mX.size()<<endl;
//    for (uint32_t i=0; i<HDP<U>::mX.size(); ++i)
//      cout<<"  x_"<<i<<": "<<HDP<U>::mX[i].n_cols<<": "<<HDP<U>::mX[i]<<endl;

    return HDP_gibbs<U>::densityEst(Nw, K0, T0, It);
  }

  // makes no copy of the external data x_i
  uint32_t addDoc(const numeric::array& x_i)
  {
    return HDP_gibbs<U>::addDoc(np2mat<U>(x_i));
  };

  uint32_t addHeldOut(const numeric::array& x_i)
  {
    return HDP_gibbs<U>::addHeldOut(np2mat<U>(x_i));
  };

  void getPerplexity(numeric::array& perp)
  {
    Row<double> perp_wrap=np2row<double>(perp);
    perp_wrap = HDP_gibbs<U>::perplexity();
  };

  /*
   * works on the data in z_i -> size has to be correct in order for this to work!
   * makes a copy of the internal labels vector
   */
  bool getClassLabels(numeric::array& z_i, uint32_t i)
  {
    Col<uint32_t> z_i_col;
    if(!HDP_gibbs<U>::getClassLabels(z_i_col, i)){return false;} // works on the data in z_i_mat
    Col<uint32_t> z_i_wrap=np2col<uint32_t>(z_i); // can do this since x_i_mat gets copied inside
    if(z_i_col.n_rows != z_i_wrap.n_rows)
      return false;
    else{
      for (uint32_t i=0; i<z_i_wrap.n_rows; ++i)
        z_i_wrap.at(i)=z_i_col.at(i);
      return true;
    }
  };
};

typedef HDP_gibbs_py<uint32_t> HDP_gibbs_Dir;
typedef HDP_gibbs_py<double> HDP_gibbs_NIW;


//  Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.
//  Copyright Toon Knapen

#ifndef BOOST_NUMERIC_BINDINGS_AMOS_AMOS_OVERLOADS_HPP
#define BOOST_NUMERIC_BINDINGS_AMOS_AMOS_OVERLOADS_HPP

#include <boost/numeric/bindings/amos/amos.h>
#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>

namespace boost { namespace numeric { namespace bindings { namespace amos { namespace detail {

  using namespace ::boost::numeric::bindings::traits ;

  //
  // BESI
  //

  //inline
  //void besi(const fcomplex * z, const fcomplex * fnu, const int * kode, const int * n, fcomplex* cy, int * nz, int * error) ;

  inline
  void besi(const double& z, const double& fnu, const int& kode, const int& n, double* cy, int& nz, int& error)
  { AMOS_DBESI( &z, &fnu, &kode, &n, cy, &nz ) ; }

  inline
  void besi(const complex_f& z, const float&  fnu, const int& kode, const int& n, complex_f* cy, int & nz, int & error)
  { AMOS_CBESI( complex_ptr( &z ), &fnu, &kode, &n, complex_ptr( cy ), &nz, &error ) ; }

  // inline
  // void besi(const complex_d* z, const double* fnu, const int * kode, const int * n, complex_d* cy, int * nz, int * error)  ;

  //
  // BESJ
  //

  inline
  void besj(const double& z, const double& fnu, const int& kode, const int& n, double* cy, int& nz, int& error)
  { AMOS_DBESJ( &z, &fnu, &n, cy, &nz ) ; }

  inline
  void besj(const complex_f& z, const float&  fnu, const int& kode, const int& n, complex_f* cy, int & nz, int & error)
  { AMOS_CBESJ( complex_ptr( &z ), &fnu, &kode, &n, complex_ptr( cy ), &nz, &error ) ; }

  //
  // BESY
  //

  inline
  void besy(const double& z, const double& fnu, const int& kode, const int& n, double* cy, int& nz, double* wrk, int& error)
  { AMOS_DBESY( &z, &fnu, &n, cy ) ; }

}}}}}

#endif // BOOST_NUMERIC_BINDINGS_AMOS_AMOS_OVERLOADS_HPP


#define BOOST_FILESYSTEM_VERSION 3

#include <iostream>
#include <fstream>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>
#include "huffman_tree.hpp"
#include "constants.hpp"

long COUNTS_SIZE;

std::vector<uint64_t> count(std::ifstream &file_in, std::ofstream &file_out){

    file_in.clear();
    file_in.seekg(0);

    file_out.clear();
    file_out.seekp(0);

    std::vector<uint64_t> counts;
    counts.reserve(BYTE_SIZE);
    for (int i = 0; i < BYTE_SIZE; ++i) {
        counts.emplace_back(0);
    }

    char c;
    while (file_in.get(c)) {
        counts[static_cast<unsigned char>(c)]++;
    }

    file_in.clear();
    unsigned long bytecount_ending = file_in.tellg() & 0xFF;

    c = static_cast<char>(bytecount_ending);
    file_out.put(c);

    file_in.clear();
    file_in.seekg(0);

    unsigned int VAL_SIZE = 0; // largest nonzero sub-byte in counts.

    uint64_t val = *std::max_element(counts.begin(), counts.end()); // biggest value in counts

    for(unsigned int j = 0; j < 8; j++) {
        if ((val & (0xFF << (8 * j))) != 0) VAL_SIZE = j + 1; // find largest non-zero byte
    }
    file_out.put(static_cast<char>(VAL_SIZE));

    for(uint64_t i : counts){
        for(uint64_t j = 0; j < VAL_SIZE; j++){
            file_out.put(static_cast<char>((i & (0xFF << 8*j) ) >> 8*j));
        }
    }

    COUNTS_SIZE = 2 + VAL_SIZE*BYTE_SIZE; // 1 for ending + 1 for size + 256 frequencies * VAL_SIZE bytes per frequency

    return counts;
}

void huffman_encode(const huffman_tree &huffman, std::ifstream &file_in, std::ofstream &file_out){
    char buffer_read[BLOCK_SIZE];
    char buffer_write[BLOCK_SIZE];
    int write_index = 0;

    char buffer_bool = 0;
    char bit_index = 0;

    file_out.clear();
    file_out.seekp(COUNTS_SIZE);

    do{
        file_in.read(buffer_read, BLOCK_SIZE);
        if (verbose) std::cout << "read " << file_in.gcount() << " bytes" << std::endl;
        for(int i = 0; i < file_in.gcount(); i++){
            auto c  = static_cast<unsigned char>(buffer_read[i]);
            auto decisions = huffman.paths[c];
            for (bool d : decisions){
                //std::cout << d << std::endl;
                buffer_bool |= d << bit_index++;

                if (bit_index == 8){
                    buffer_write[write_index++] = buffer_bool;
                    buffer_bool = bit_index = 0;

                    if (write_index == BLOCK_SIZE){
                        file_out.write(buffer_write, BLOCK_SIZE);
                        if (verbose) std::cout << "wrote " << BLOCK_SIZE << " bytes" << std::endl;
                        write_index = 0;
                    }
                }
            }
        }
    } while (file_in.gcount() > 0);

    buffer_write[write_index++] = buffer_bool;
    file_out.write(buffer_write, write_index);

    if (verbose) std::cout << "wrote " << write_index+1 << " bytes" << std::endl; // +1 because put
}

int main(int argc, char *argv[]) {
    auto filename_in = test_in / boost::filesystem::path("compress.cpp");
    if(argc > 1) filename_in = argv[1];

    auto file_part = filename_in.stem();
    auto extension = filename_in.extension();

    auto filename_out = test_huff;
    if(argc > 2) filename_out = argv[2];
    if (boost::filesystem::is_directory(filename_out)){
        filename_out /= file_part;
        filename_out += extension;
        filename_out += dot;
        filename_out += huff;
    }

    std::ifstream file_in;
    std::ofstream file_out;

    file_in.open(filename_in.string()  , std::ios::in  | std::ios::binary);
    file_out.open(filename_out.string(), std::ios::out | std::ios::binary);

    std::vector<uint64_t> counts = count(file_in, file_out);

    auto huff = huffman_tree(counts);

    huffman_encode(huff, file_in, file_out);

    file_in.close();
    file_out.close();
    return EXIT_SUCCESS;
}


///////////////////////////////////////////////////////////////////////////////
// ars::detail::data.hpp                                                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_ARS_DETAIL_DATA_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_DETAIL_DATA_HPP_ER_2009
#include <boost/function.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/ars/constant.hpp>
#include <boost/ars/point.hpp>
#include <boost/ars/detail/tangent_intersection.hpp>
#include <boost/ars/error.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

template<typename T>
struct data : point<T>, tangent_intersection<T>{
    typedef constant<T> const_;
    typedef point<T> super1;
    typedef tangent_intersection<T> super2;
    data():super1(),super2(){}
    data(const super1& p):super1(p),super2(){}
    data(const super1& p,const super2& ti):super1(p),super2(ti){}

};

// Warning:
// Since both point<T> and tangent<T> have operator <,
// use a const reference to the desired type for a and b

template <typename T>
std::ostream&
operator<<(std::ostream& out, const data<T> &d){
    typedef point<T> point_t;
    typedef tangent_intersection<T> tang_t;
    const point_t& p = static_cast<const point_t&>(d);
    const tang_t& ti = static_cast<const tang_t&>(d);
    out << '(' << p << ',' << ti << ')';

    return out;
}

// Usage:
//
// prior(end(datas))->z_ = tangent(prior(end(datas))),
// adjacent_difference(
//  make_reverse_iterator(end(datas)),
//  make_reverse_iterator(begin(datas)),
//  update_z<T>()
// );

template<typename T>
struct update_tangent_intersection{
    typedef point<T>            point_;
    typedef const point_&       first_argument_type;
    typedef const point_&       second_argument_type;
    typedef data<T>             result_type;

    result_type
    operator()(first_argument_type a,second_argument_type b)const{
        result_type result(a,tangent_intersection<T>(a,b));
        return result;
    }
};

struct pred_isinf{
    typedef bool result_type;
    // Used by find_if(begin(datas_),end(datas_),pred_isinf)
    // because had problem with bind etc. (reason: mutable data?)
    template<typename D>
    result_type operator()(D& d)const{
        return math::isinf(d.cum_sum_);
    }
};

}// ars
}// detail
}// statistics
}// boost

#endif


#include <utility>
#include <vector>

#include <Eigen/Core>
#include <aslam/common/memory.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include <inverted-multi-index/inverted-multi-index-common.h>
#include <inverted-multi-index/inverted-multi-product-quantization-index.h>

namespace loop_closure {
namespace inverted_multi_index {
namespace {
class TestableInvertedMultiPQIndex
    : public InvertedMultiProductQuantizationIndex<int, 4, 1, 2> {
 public:
  TestableInvertedMultiPQIndex(
      const Eigen::MatrixXf& words1, const Eigen::MatrixXf& words2,
      const Eigen::MatrixXf& quantizer_centers_1,
      const Eigen::MatrixXf& quantizer_centers_2,
      int num_closest_words_for_nn_search)
      : InvertedMultiProductQuantizationIndex<int, 4, 1, 2>(
            words1, words2, quantizer_centers_1, quantizer_centers_2,
            num_closest_words_for_nn_search) {}

  using InvertedMultiProductQuantizationIndex<int, 4, 1, 2>::words_1_index_;
  using InvertedMultiProductQuantizationIndex<int, 4, 1, 2>::words_2_index_;
  using InvertedMultiProductQuantizationIndex<int, 4, 1, 2>::word_index_map_;
  using InvertedMultiProductQuantizationIndex<int, 4, 1, 2>::inverted_files_;
  using InvertedMultiProductQuantizationIndex<int, 4, 1,
                                              2>::max_db_descriptor_index_;
};

class InvertedMultiProductQuantizationIndexTest : public ::testing::Test {
 public:
  void SetUp() {
    descriptors_.resize(4, 5);
    // The descriptors are stored column-wise and the corresponding indices of
    // the closest word from the product vocabulary are 0, 0, 14, 5, 11.
    descriptors_ << 2.5, 1.8, -2.1, 0.5, -1.0, 2.5, 2.2, 1.8, -0.5, -1.0, 0.1,
        0.3, 0.05, 1.0, -2.5, 1.9, 2.6, -2.4, 0.1, 0.5;

    words1_.resize(2, 4);
    words1_ << 2.0, 2.0, -2.0, -2.0, 2.0, -2.0, -2.0, 2.0;
    words2_.resize(2, 4);
    words2_ << 0.0, 2.0, 0.0, -2.0, 2.0, 0.0, -2.0, 0.0;

    // For simplicity, use the same cluster centers for each product quantizer.
    quantizer_centers_1_.resize(1, 16);
    quantizer_centers_1_ << -1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0,
        1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0;
    quantizer_centers_2_.resize(1, 16);
    quantizer_centers_2_ << -0.5, 0.5, -0.5, 0.5, -0.5, 0.5, -0.5, 0.5, -0.5,
        0.5, -0.5, 0.5, -0.5, 0.5, -0.5, 0.5;

    // FLAG used to control the amount of backtracking done during kd-tree-based
    // nearest neighbor search. In order to work, this test needs a certain
    // amount of backtracking.
    FLAGS_lc_knn_epsilon = 0.2;
  }

  Eigen::MatrixXf descriptors_;
  Eigen::MatrixXf words1_;
  Eigen::MatrixXf words2_;
  Eigen::MatrixXf quantizer_centers_1_;
  Eigen::MatrixXf quantizer_centers_2_;
};

using ::testing::ContainerEq;

TEST_F(InvertedMultiProductQuantizationIndexTest, AddDescriptorsWorks) {
  TestableInvertedMultiPQIndex index(
      words1_, words2_, quantizer_centers_1_, quantizer_centers_2_, 16);
  index.AddDescriptors(descriptors_);

  // Tests whether the map used to index the inverted files is correct.
  std::vector<int> activated_product_words = {0, 5, 11, 14};
  std::vector<int> expected_map_entries = {0, 2, 3, 1};
  for (size_t i = 0; i < activated_product_words.size(); ++i) {
    ASSERT_NE(
        index.word_index_map_.find(activated_product_words[i]),
        index.word_index_map_.end());
    EXPECT_EQ(
        expected_map_entries[i],
        index.word_index_map_.find(activated_product_words[i])->second);
  }

  // Tests whether the descriptors are quantized and stored correctly.
  Aligned<std::vector, Eigen::Matrix<int, 4, 1> >
      expected_quantized_descriptors(5);
  expected_quantized_descriptors[0] << 1, 1, 1, 0;
  expected_quantized_descriptors[1] << 0, 1, 1, 1;
  expected_quantized_descriptors[2] << 0, 0, 1, 0;
  expected_quantized_descriptors[3] << 0, 1, 0, 1;
  expected_quantized_descriptors[4] << 1, 1, 0, 1;

  std::vector<int> expected_num_entries_per_inverted_file = {2, 1, 1, 1};

  ASSERT_EQ(4, index.inverted_files_.size());
  int counter = 0;
  for (int i = 0; i < 4; ++i) {
    EXPECT_EQ(
        index.inverted_files_[i].descriptors_.size(),
        index.inverted_files_[i].indices_.size());
    ASSERT_EQ(
        expected_num_entries_per_inverted_file[i],
        index.inverted_files_[i].descriptors_.size());
    for (int j = 0; j < expected_num_entries_per_inverted_file[i]; ++j) {
      EXPECT_EQ(counter, index.inverted_files_[i].indices_[j]);
      EXPECT_TRUE(
          ::common::MatricesEqual(
              expected_quantized_descriptors[counter],
              index.inverted_files_[i].descriptors_[j]))
          << "The quantized representation for descriptor " << counter << " ( "
          << index.inverted_files_[i].descriptors_[j].transpose()
          << " ) does not match the expected quantized representation ( "
          << expected_quantized_descriptors[counter].transpose() << " ).";
      ++counter;
    }
  }

  // After filling the index, tests whether the index gets cleared correctly.
  index.Clear();
  EXPECT_EQ(0, index.max_db_descriptor_index_);
  EXPECT_TRUE(index.word_index_map_.empty());
  EXPECT_TRUE(index.inverted_files_.empty());
}

TEST_F(InvertedMultiProductQuantizationIndexTest, GetNNearestNeighborsWorks) {
  TestableInvertedMultiPQIndex index(
      words1_, words2_, quantizer_centers_1_, quantizer_centers_2_, 16);
  index.AddDescriptors(descriptors_);

  Eigen::Matrix<float, 4, 1> query_descriptor;
  query_descriptor << 1.0, 0.5, 0.5, 1.0;

  static constexpr int kNumNeighbors = 8;
  Eigen::VectorXi indices(kNumNeighbors, 1);
  Eigen::VectorXf distances(kNumNeighbors, 1);
  index.GetNNearestNeighbors(
      query_descriptor, kNumNeighbors, indices, distances);

  constexpr float infinity = std::numeric_limits<float>::infinity();
  std::vector<int> expected_indices = {3, 1, 0, 4, 2, -1, -1, -1};
  std::vector<float> expected_distances = {3.5,  8.5,      10.5,     15.5,
                                           28.5, infinity, infinity, infinity};

  std::vector<int> actual_indices;
  std::vector<float> actual_distances;

  for (int i = 0; i < kNumNeighbors; ++i) {
    actual_distances.push_back(distances[i]);
    actual_indices.push_back(indices[i]);
  }

  EXPECT_THAT(actual_distances, ContainerEq(expected_distances));
  EXPECT_THAT(actual_indices, ContainerEq(expected_indices));
}
}  // namespace
}  // namespace inverted_multi_index
}  // namespace loop_closure

MAPLAB_MOCKTEST_ENTRYPOINT


// Copyright Aleksey Gurtovoy 2000-2004
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/advance.hpp>
#include <boost/mpl/iterator_tags.hpp>
#include <boost/mpl/aux_/test.hpp>

template< int pos > struct iter
{
    typedef mpl::bidirectional_iterator_tag category;
    typedef iter<(pos + 1)> next;
    typedef iter<(pos - 1)> prior;
    typedef int_<pos> type;
};

#if BOOST_WORKAROUND(__MWERKS__, BOOST_TESTED_AT(0x3003))
namespace boost { namespace mpl {
template< int pos, typename Default > struct tag< iter<pos>,Default > : void_ {};
}}
#endif

typedef iter<0> first;
typedef iter<10> last;

MPL_TEST_CASE()
{
    typedef mpl::advance<first,int_<10> >::type iter1;
    typedef advance_c<first,10>::type           iter2;

    MPL_ASSERT(( is_same<iter1, last> ));
    MPL_ASSERT(( is_same<iter2, last> ));
}

MPL_TEST_CASE()
{
    typedef mpl::advance<last,int_<-10> >::type iter1;
    typedef advance_c<last,-10>::type           iter2;

    MPL_ASSERT(( is_same<iter1, first> ));
    MPL_ASSERT(( is_same<iter2, first> ));
}


#include "ros/ros.h"
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <math.h>
#include <sstream>
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <cmath>
#include <stdio.h>
#include <stdlib.h>
#include <tbb/atomic.h>

#include "serial/serial.h"
#include "std_msgs/Int32.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"

#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;
bool App_exit = false;
int Compass = 0;

ros::Publisher chatter_pub[20];

float IEEE_754_to_float(const uint8_t raw[4]) {
        int sign = (raw[0] >> 7) ? -1 : 1;

        int8_t exponent = (raw[0] << 1) + (raw[1] >> 7) - 126;

        uint32_t fraction_bits = ((raw[1] & 0x7F) << 16) + (raw[2] << 8) + raw[3];

        float fraction = 0.5f;
        for (uint8_t ii = 0; ii < 24; ++ii)
                fraction += ldexpf((fraction_bits >> (23 - ii)) & 1, -(ii + 1));

        float significand = sign * fraction;

        return ldexpf(significand, exponent);
}

void serial_logic()
{
    cout << "serial reader Xsense started... 17 july" << endl;
    while (App_exit == false)
    {

        try
        {

            try
            {

                serial::Serial my_serial("/dev/serial/by-id/usb-Xsens_USB-serial_Converter_XSUWTK6Q-if00-port0", 115200 , serial::Timeout::simpleTimeout(1000));

                cout << "Is the serial port open? ";
                if (my_serial.isOpen())
                {
                    cout << " Yes." << endl;
                }
                else
                    cout << " No." << endl;

                    boost::this_thread::sleep(boost::posix_time::milliseconds(1000));

                    uint8_t read = 0;
                    uint8_t result_read[30] = {0};
                    uint8_t raw[4] = {0};

                        while (App_exit == false)
                        {
                            my_serial.read(&read, 1);

                            if ( read == 250 )
                            {
                                my_serial.read(&read, 1);

                                if ( read == 255 )
                                {
                                    my_serial.read(&read, 1);

                                    if ( read == 50 )
                                    {
                                        my_serial.read(&read, 1);

                                        if ( read == 12 )
                                        {
                                            ROS_INFO("Header ok...");

                                            my_serial.read(result_read,12);

                                            raw[0] = result_read[8];
                                            raw[1] = result_read[9];
                                            raw[2] = result_read[10];
                                            raw[3] = result_read[11];

                                           float x = IEEE_754_to_float(raw);
                                           Compass = (int)x;

                                           if ( Compass >= 360 ) Compass = Compass % 360;
                                           if ( Compass < 0 ) Compass += 360;

                                           cout<<x<<endl;
                                           //================================
                                           my_serial.read(&read, 1);
                                        }
                                    }
                                }

                            }

                            boost::this_thread::sleep(boost::posix_time::milliseconds(20));

                       }//while

            }//try
            catch (serial::SerialException e)
            {
                cout << "Read Error [Xsense]" << endl;
            }

        }//try
        catch (serial::IOException e)
        {
            cout << "Port Not Opened [Xsense]" << endl;
        }

        boost::this_thread::sleep(boost::posix_time::milliseconds(1000));

    }//while

}

void update_data()
{
    std_msgs::Int32 c_msg;
    c_msg.data = Compass;
    chatter_pub[10].publish(c_msg); //Compass
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "xsense");
    ros::Time::init();

    boost::thread _thread_logic(&serial_logic);

    ros::Rate ros_rate(20);

    ros::NodeHandle node_handles[15];
    ros::Subscriber sub_handles[15];

    chatter_pub[10] = node_handles[0].advertise<std_msgs::Int32>("AUTROBOTOUT_compass", 10);

    while (ros::ok())
    {
        ros::spinOnce();
        ros_rate.sleep();
        update_data();
    }

    App_exit = true;

    _thread_logic.interrupt();
    _thread_logic.join();

    return 0;
}


#include "CoordinateMatrixAnalysis.h"

#include "CoordinateMatrix.h"
#include <macgyver/Exception.h>

#include <boost/optional.hpp>

#include <cmath>
#include <iomanip>
#include <iostream>

namespace Fmi
{
namespace
{
// We discard any grid cell whose bbox exceeds 1000 kilometers in size

const double cell_size_limit = 1000 * 1000;

enum class Handedness
{
  ClockwiseConvex,
  CounterClockwiseConvex,
  Invalid,
  NotConvex,
  Huge,
  Oblong
};

// ----------------------------------------------------------------------
/*!
 * A polygon is convex if all cross products of adjacent edges are of the same sign,
 * and the sign itself says whether the polygon is clockwise or not.
 * Ref: http://www.easywms.com/node/3602
 * We must disallow non-convex cells such as V-shaped ones, since the intersections
 * formulas may then produce values outside the cell.
 *
 * Note that we permit colinear adjacent edges, since for example in latlon grids
 * the poles are represented by multiple grid points. This test thus passes the
 * redundant case of a rectangle with no area, but the rest of the code can handle it.
 * empty boolean for such grid cells.
 */
// ----------------------------------------------------------------------

Handedness analyze_cell(
    double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
{
  try
  {
    // Disallow cells with any invalid coordinates
    if (std::isnan(x1) || std::isnan(y1) || std::isnan(x2) || std::isnan(y2) || std::isnan(x3) ||
        std::isnan(y3) || std::isnan(x4) || std::isnan(y4))
      return Handedness::Invalid;

    // Check for oblong cells which typically occur at projection discontinuities. One could
    // use the Polsby-Popper test here, but calculating the edge lengths in addition to the
    // areas below would be slower tahn simply testing the cell bbox

    const auto xmin = std::min(std::min(x1, x2), std::min(x3, x4));
    const auto xmax = std::max(std::max(x1, x2), std::max(x3, x4));
    const auto ymin = std::min(std::min(y1, y2), std::min(y3, y4));
    const auto ymax = std::max(std::max(y1, y2), std::max(y3, y4));

    const auto dx = xmax - xmin;
    const auto dy = ymax - ymin;

    // Empty cell?
    if (dx == 0 || dy == 0)
      return Handedness::Invalid;

    // Huge cell? (most likely due to projection instabilities)
    if (dx >= cell_size_limit || dy >= cell_size_limit)
      return Handedness::Huge;

    const auto ratio = dy / dx;

    if (ratio < 0.01 || ratio > 100)
      return Handedness::Oblong;

    // Check for convexness and orientation

    const auto area1 = (x2 - x1) * (y3 - y2) - (y2 - y1) * (x3 - x2);
    const auto area2 = (x3 - x2) * (y4 - y3) - (y3 - y2) * (x4 - x3);
    const auto area3 = (x4 - x3) * (y1 - y4) - (y4 - y3) * (x1 - x4);
    const auto area4 = (x1 - x4) * (y2 - y1) - (y1 - y4) * (x2 - x1);

    if (area1 <= 0 && area2 <= 0 && area3 <= 0 && area4 <= 0)
      return Handedness::ClockwiseConvex;

    if (area1 >= 0 && area2 >= 0 && area3 >= 0 && area4 >= 0)
      return Handedness::CounterClockwiseConvex;

    return Handedness::NotConvex;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace

CoordinateAnalysis analysis(const CoordinateMatrix& coords)
{
  try
  {
    const auto nx = coords.width();
    const auto ny = coords.height();

    BoolMatrix valid(nx - 1, ny - 1, true);
    BoolMatrix clockwise(nx - 1, ny - 1, false);

    std::size_t cw = 0;
    std::size_t ccw = 0;
    std::size_t bad = 0;

#if 0
    std::size_t huge = 0;
    std::size_t oblong = 0;
    std::size_t notconvex = 0;
#endif

    // Go through the coordinates once

    for (std::size_t j = 0; j < ny - 1; j++)
      for (std::size_t i = 0; i < nx - 1; i++)
      {
        auto hand = analyze_cell(coords.x(i, j),
                                 coords.y(i, j),
                                 coords.x(i, j + 1),
                                 coords.y(i, j + 1),
                                 coords.x(i + 1, j + 1),
                                 coords.y(i + 1, j + 1),
                                 coords.x(i + 1, j),
                                 coords.y(i + 1, j));

        if (hand == Handedness::ClockwiseConvex)
        {
          clockwise.set(i, j, true);
          ++cw;
        }
        else if (hand == Handedness::CounterClockwiseConvex)
        {
          // clockwise.set(i, j, false);
          ++ccw;
        }
        else
        {
          valid.set(i, j, false);
          ++bad;
#if 0
          if (hand == Handedness::Huge)
            ++huge;
          else if (hand == Handedness::Oblong)
            ++oblong;
          else if (hand == Handedness::NotConvex)
            ++notconvex;
#endif
        }
      }

    // The coordinates are likely to be upside done if there are many more CCW cells than CW cells

    bool needs_flipping = (ccw > 2 * cw);

    return CoordinateAnalysis{valid, clockwise, needs_flipping};
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace Fmi


#include <boost/graph/random.hpp>


/*
Copyright 2013 Henrik Mühe and Florian Funke

This file is part of CampersCoreBurner.

CampersCoreBurner is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

CampersCoreBurner is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with CampersCoreBurner.  If not, see <http://www.gnu.org/licenses/>.
*/

/*
COMMENT SUMMARY FOR THIS FILE:

Tokenizer is used to hold the metadata for a document. We first parse the document using ParserMixin and then
compute all filters for each word that we've found. All meta data is ordered by the second coefficient inside
the Haar-Wavelet transformation of the Frequency filter such that we can easily exclude filters during matching.
*/

#pragma once

#include "core.h"
#include "frequency.hpp"
#include "hashtable.hpp"
#include "parser.hpp"
#include "utils.hpp"
#include <array>
#include <boost/utility/string_ref.hpp>
#include <cassert>
#include <cstring>
#include <list>
#include <iostream>
#include <vector>

namespace campers {

/// The tokenizer and meta data store
struct Tokenizer : public ParserMixin<Tokenizer> {
    /// All unique words inside the document by length
    std::array<FixedLengthWordVector,MAX_WORD_LENGTH+1> words;
    /// Frequencies matching the words, also by length
    std::array<std::vector<Frequency>,MAX_WORD_LENGTH+1> freq;
    /// FastFrequencies matching the words, also by length
    std::array<std::vector<FrequencyFast>,MAX_WORD_LENGTH+1> freqFast;
    /// Permutation which is applied inside parse, matching the words, also by length
    std::array<std::vector<uint64_t>,MAX_WORD_LENGTH+1> perm;
    /// The Haar-Wavelet coefficients (see summary and README), also by length
    std::array<std::vector<uint8_t>,MAX_WORD_LENGTH+1> coefficient;
    /// The offsets for each Haar-Wavelet coefficient value such that we can iterate over all filters which might actually match, also by length.
    std::array<std::array<unsigned,66>,MAX_WORD_LENGTH+1> offsets;
    /// Hash set used to deduplicate the document words
    HashSet<boost::string_ref> h;
    /// The number of unique words the document contains in total
    uint64_t size_;

public:
    /// Constructor
    Tokenizer() : size_(0) {
        for (uint64_t length=MIN_WORD_LENGTH;length<=MAX_WORD_LENGTH;++length)
            words[length].setLength(length);
    }
    /// Called from the parser to insert a new word
    void insert(boost::string_ref word);
    /// Wrapper for ParserMixing<>::parse
    uint64_t parse(const char* str,uint64_t len);
    /// Allows finding a match inside the deduplication hash (used for exact matching)
    bool find(boost::string_ref word) const { return h.find(word); }
    /// Allows finding a match inside the deduplication hash (used for exact matching), precomputed hash
    bool find(boost::string_ref word, uint32_t hashValue) const { return h.find(word, hashValue); }
    /// Allows access to the underlying deduplication hashset
    const HashSet<boost::string_ref>& getHash() const { return h; }
    /// Returns number of unique words inside the document
    uint64_t size() const { return size_; }
    /// Allows retrieving the words vector
    const FixedLengthWordVector& getWords(uint64_t length) const { return words[length]; }
};

}


/*
* LEGAL NOTICE
* This computer software was prepared by Battelle Memorial Institute,
* hereinafter the Contractor, under Contract No. DE-AC05-76RL0 1830
* with the Department of Energy (DOE). NEITHER THE GOVERNMENT NOR THE
* CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY
* LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this
* sentence must appear on any copies of this computer software.
*
* EXPORT CONTROL
* User agrees that the Software will not be shipped, transferred or
* exported into any country or used in any manner prohibited by the
* United States Export Administration Act or any other applicable
* export laws, restrictions or regulations (collectively the "Export Laws").
* Export of the Software may require some form of license or other
* authority from the U.S. Government, and failure to obtain such
* export control license may result in criminal liability under
* U.S. laws. In addition, if the Software is identified as export controlled
* items under the Export Laws, User represents and warrants that User
* is not a citizen, or otherwise located within, an embargoed nation
* (including without limitation Iran, Syria, Sudan, Cuba, and North Korea)
*     and that User is not otherwise prohibited
* under the Export Laws from receiving the Software.
*
* Copyright 2011 Battelle Memorial Institute.  All Rights Reserved.
* Distributed as open-source under the terms of the Educational Community
* License version 2.0 (ECL 2.0). http://www.opensource.org/licenses/ecl2.php
*
* For further details, see: http://www.globalchange.umd.edu/models/gcam/
*
*/

#if USE_LAPACK
/* This whole file is used only if we are using lapack */
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>

#include "solution/util/include/ublas-helpers.hpp"

#include "solution/util/include/svd_invert_solve.hpp"
#include "util/base/include/util.h"

namespace ublas = boost::numeric::ublas;

// Solve U*S*VT * x = b,
// Where U,S,VT are the SVD of a matrix.  x will replace b on output
int svdInvertSolve(const ublas::matrix<double,ublas::column_major> &U,
                     const ublas::vector<double> &S,
                     const ublas::matrix<double,ublas::column_major> &VT,
                     ublas::vector<double> &b,
                     std::ostream &logf)
{
  const double small = 1.0e-8;
  int nsing = 0;
  ublas::matrix<double,ublas::column_major> Utmp(U.size2(),U.size1()), Vtmp(VT.size2(),VT.size1());
  ublas::matrix<double,ublas::column_major> Ainv(Vtmp.size1(),Utmp.size2());
  ublas::vector<double> tmpvec(Vtmp.size2()); // able to store a row of VT (== a column of V)

  Utmp = ublas::trans(U);
  Vtmp = ublas::trans(VT);

  int nrow = Utmp.size1();
  int ncol = Utmp.size2();

  logf << "Largest singular value = " << S[0] << "\n";

  // multiply Utmp from the left by the inverse of the diagonal matrix S
  double singthresh = S[0]*small;
  for(int i=0;i<nrow;++i) {
    double sinv = S[i];
    if(sinv < singthresh) {
      // Note that the elements of S are all >=0.  For "sufficiently
      // small" singular values we set 1/s = 1/singthresh and log the singular
      // component
      double s= sinv;           // store for output without moving the meat of this branch down below all the output
      nsing++;
      sinv = 1.0/singthresh;    // keep the multiplier on this singular component "reasonable".

      // log the singular component (everything between here and the
      // marker below is strictly diagnostic and not necessary for the
      // algorithm.)

      // the columns of V (== trans(VT)) give the basis vectors for the nullspace
      int kmax = 0;
      double vtmax = fabs(Vtmp(0,i));
      for(int k=0; k<Vtmp.size1(); ++k) {
        tmpvec[k] = Vtmp(k,i);  // column i, kth entry
        double vtabs = fabs(tmpvec[k]);
        if(vtabs > vtmax) {
          vtmax = vtabs;
          kmax = k;
        }
      }
      // suppress printing of small values.
      double prnthresh = 1.0e-4*vtmax;
      for(int k=0; k<tmpvec.size(); ++k)
        if(fabs(tmpvec[k]) < prnthresh)
          tmpvec[k] = 0.0;

      logf << "Singular component:  s= " << s
           << "  kmax= " << kmax << "  vtval= " << vtmax
           << "  sinv set to: " << sinv
           << "\n";
      logf << tmpvec << "\n";
      // end of singular component logging
    }
    else {
      sinv = 1.0/sinv;
    }
    for(int j=0;j<ncol;++j)
      Utmp(i,j) *= sinv;
  }

  Ainv = prod(Vtmp,Utmp);

  ublas::vector<double> bb(b);
  axpy_prod(Ainv,bb,b);

  return nsing;
}

#endif