#include <cstdlib>
#include <algorithm>
#include <sstream>
#include <iostream>
#include <vector>
#include <limits>
#include <cmath>
#include <chrono>
#include <ctime>
#include <time.h>

#include <unordered_map>
#include <unordered_set>

#include <execinfo.h>
#include <unistd.h>
#include <signal.h>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/program_options.hpp>

#include "cnn/training.h"
#include "cnn/cnn.h"
#include "cnn/expr.h"
#include "cnn/nodes.h"
#include "cnn/lstm.h"
#include "cnn/rnn.h"
#include "c2.h"

cpyp::Corpus corpus;
volatile bool requested_stop = false;
unsigned LAYERS = 2;
unsigned INPUT_DIM = 40;
unsigned HIDDEN_DIM = 60;
unsigned ACTION_DIM = 36;
unsigned PRETRAINED_DIM = 50;
unsigned LSTM_INPUT_DIM = 60;
unsigned POS_DIM = 10;
unsigned REL_DIM = 8;

bool USE_POS = false;

constexpr const char* ROOT_SYMBOL = "ROOT";
unsigned kROOT_SYMBOL = 0;
unsigned ACTION_SIZE = 0;
unsigned VOCAB_SIZE = 0;
unsigned POS_SIZE = 0;

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

vector<unsigned> possible_actions;
unordered_map<unsigned, vector<float>> pretrained;

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("training_data,T", po::value<string>(), "List of Transitions - Training corpus")
        ("dev_data,d", po::value<string>(), "Development corpus")
        ("test_data,p", po::value<string>(), "Test corpus")
        ("unk_strategy,o", po::value<unsigned>()->default_value(1), "Unknown word strategy: 1 = singletons become UNK with probability unk_prob")
        ("unk_prob,u", po::value<double>()->default_value(0.2), "Probably with which to replace singletons with UNK in training data")
        ("model,m", po::value<string>(), "Load saved model from this file")
        ("use_pos_tags,P", "make POS tags visible to parser")
        ("layers", po::value<unsigned>()->default_value(2), "number of LSTM layers")
        ("action_dim", po::value<unsigned>()->default_value(16), "action embedding size")
        ("input_dim", po::value<unsigned>()->default_value(32), "input embedding size")
        ("hidden_dim", po::value<unsigned>()->default_value(64), "hidden dimension")
        ("pretrained_dim", po::value<unsigned>()->default_value(50), "pretrained input dimension")
        ("pos_dim", po::value<unsigned>()->default_value(12), "POS dimension")
        ("rel_dim", po::value<unsigned>()->default_value(10), "relation dimension")
        ("lstm_input_dim", po::value<unsigned>()->default_value(60), "LSTM input dimension")
        ("train,t", "Should training be run?")
        ("words,w", po::value<string>(), "Pretrained word embeddings")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
  if (conf->count("training_data") == 0) {
    cerr << "Please specify --traing_data (-T): this is required to determine the vocabulary mapping, even if the parser is used in prediction mode.\n";
    exit(1);
  }
}

struct ParserBuilder {

  LSTMBuilder stack_lstm; // (layers, input, hidden, trainer)
  LSTMBuilder buffer_lstm;
  LSTMBuilder action_lstm;
  LookupParameters* p_w; // word embeddings
  LookupParameters* p_t; // pretrained word embeddings (not updated)
  LookupParameters* p_a; // input action embeddings
  LookupParameters* p_r; // relation embeddings
  LookupParameters* p_p; // pos tag embeddings
  Parameters* p_pbias; // parser state bias
  Parameters* p_A; // action lstm to parser state
  Parameters* p_B; // buffer lstm to parser state
  Parameters* p_S; // stack lstm to parser state
  Parameters* p_H; // head matrix for composition function
  Parameters* p_D; // dependency matrix for composition function
  Parameters* p_R; // relation matrix for composition function
  Parameters* p_w2l; // word to LSTM input
  Parameters* p_p2l; // POS to LSTM input
  Parameters* p_t2l; // pretrained word embeddings to LSTM input
  Parameters* p_ib; // LSTM input bias
  Parameters* p_cbias; // composition function bias
  Parameters* p_p2a;   // parser state to action
  Parameters* p_action_start;  // action bias
  Parameters* p_abias;  // action bias
  Parameters* p_buffer_guard;  // end of buffer
  Parameters* p_stack_guard;  // end of stack

  explicit ParserBuilder(Model* model, const unordered_map<unsigned, vector<float>>& pretrained) :
      stack_lstm(LAYERS, LSTM_INPUT_DIM, HIDDEN_DIM, model),
      buffer_lstm(LAYERS, LSTM_INPUT_DIM, HIDDEN_DIM, model),
      action_lstm(LAYERS, ACTION_DIM, HIDDEN_DIM, model),
      p_w(model->add_lookup_parameters(VOCAB_SIZE, {INPUT_DIM})),
      p_a(model->add_lookup_parameters(ACTION_SIZE, {ACTION_DIM})),
      p_r(model->add_lookup_parameters(ACTION_SIZE, {REL_DIM})),
      p_pbias(model->add_parameters({HIDDEN_DIM})),
      p_A(model->add_parameters({HIDDEN_DIM, HIDDEN_DIM})),
      p_B(model->add_parameters({HIDDEN_DIM, HIDDEN_DIM})),
      p_S(model->add_parameters({HIDDEN_DIM, HIDDEN_DIM})),
      p_H(model->add_parameters({LSTM_INPUT_DIM, LSTM_INPUT_DIM})),
      p_D(model->add_parameters({LSTM_INPUT_DIM, LSTM_INPUT_DIM})),
      p_R(model->add_parameters({LSTM_INPUT_DIM, REL_DIM})),
      p_w2l(model->add_parameters({LSTM_INPUT_DIM, INPUT_DIM})),
      p_ib(model->add_parameters({LSTM_INPUT_DIM})),
      p_cbias(model->add_parameters({LSTM_INPUT_DIM})),
      p_p2a(model->add_parameters({ACTION_SIZE, HIDDEN_DIM})),
      p_action_start(model->add_parameters({ACTION_DIM})),
      p_abias(model->add_parameters({ACTION_SIZE})),
      p_buffer_guard(model->add_parameters({LSTM_INPUT_DIM})),
      p_stack_guard(model->add_parameters({LSTM_INPUT_DIM})) {
    if (USE_POS) {
      p_p = model->add_lookup_parameters(POS_SIZE, {POS_DIM});
      p_p2l = model->add_parameters({LSTM_INPUT_DIM, POS_DIM});
    }
    if (pretrained.size() > 0) {
      p_t = model->add_lookup_parameters(VOCAB_SIZE, {PRETRAINED_DIM});
      for (auto it : pretrained)
        p_t->Initialize(it.first, it.second);
      p_t2l = model->add_parameters({LSTM_INPUT_DIM, PRETRAINED_DIM});
    } else {
      p_t = nullptr;
      p_t2l = nullptr;
    }
  }

static bool IsActionForbidden(const string& a, unsigned bsize, unsigned ssize, const vector<int>& stacki) {
  if (a[1]=='W' && ssize<3) return true;
  if (a[1]=='W') {
        int top=stacki[stacki.size()-1];
        int sec=stacki[stacki.size()-2];
        if (sec>top) return true;
  }

  bool is_shift = (a[0] == 'S' && a[1]=='H');
  bool is_reduce = !is_shift;
  if (is_shift && bsize == 1) return true;
  if (is_reduce && ssize < 3) return true;
  if (bsize == 2 && // ROOT is the only thing remaining on buffer
      ssize > 2 && // there is more than a single element on the stack
      is_shift) return true;
  // only attach left to ROOT
  if (bsize == 1 && ssize == 3 && a[0] == 'R') return true;
  return false;
}

// take a vector of actions and return a parse tree (labeling of every
// word position with its head's position)
static map<int,int> compute_heads(unsigned sent_len, const vector<unsigned>& actions, const vector<string>& setOfActions, map<int,string>* pr = nullptr) {
  map<int,int> heads;
  map<int,string> r;
  map<int,string>& rels = (pr ? *pr : r);
  for(unsigned i=0;i<sent_len;i++) { heads[i]=-1; rels[i]="ERROR"; }
  vector<int> bufferi(sent_len + 1, 0), stacki(1, -999);
  for (unsigned i = 0; i < sent_len; ++i)
    bufferi[sent_len - i] = i;
  bufferi[0] = -999;
  for (auto action: actions) { // loop over transitions for sentence
    const string& actionString=setOfActions[action];
    const char ac = actionString[0];
    const char ac2 = actionString[1];
    if (ac =='S' && ac2=='H') {  // SHIFT
      assert(bufferi.size() > 1); // dummy symbol means > 1 (not >= 1)
      stacki.push_back(bufferi.back());
      bufferi.pop_back();
    } else if (ac=='S' && ac2=='W') { // SWAP
      assert(stacki.size() > 2);
      unsigned ii = 0, jj = 0;
      jj = stacki.back();
      stacki.pop_back();
      ii = stacki.back();
      stacki.pop_back();
      bufferi.push_back(ii);
      stacki.push_back(jj);
    } else { // LEFT or RIGHT
      assert(stacki.size() > 2); // dummy symbol means > 2 (not >= 2)
      assert(ac == 'L' || ac == 'R');
      unsigned depi = 0, headi = 0;
      (ac == 'R' ? depi : headi) = stacki.back();
      stacki.pop_back();
      (ac == 'R' ? headi : depi) = stacki.back();
      stacki.pop_back();
      stacki.push_back(headi);
      heads[depi] = headi;
      rels[depi] = actionString;
    }
  }
  assert(bufferi.size() == 1);
  //assert(stacki.size() == 2);
  return heads;
}

// *** if correct_actions is empty, this runs greedy decoding ***
// returns parse actions for input sentence (in training just returns the reference)
// OOV handling: raw_sent will have the actual words
//               sent will have words replaced by appropriate UNK tokens
// this lets us use pretrained embeddings, when available, for words that were OOV in the
// parser training data
vector<unsigned> log_prob_parser(ComputationGraph* hg,
                     const vector<unsigned>& raw_sent,  // raw sentence
                     const vector<unsigned>& sent,  // sent with oovs replaced
                     const vector<unsigned>& sentPos,
                     const vector<unsigned>& correct_actions,
                     const vector<string>& setOfActions,
                     const map<unsigned, std::string>& intToWords,
                     double *right) {
    vector<unsigned> results;
    const bool build_training_graph = correct_actions.size() > 0;

    stack_lstm.new_graph(*hg);
    buffer_lstm.new_graph(*hg);
    action_lstm.new_graph(*hg);
    stack_lstm.start_new_sequence();
    buffer_lstm.start_new_sequence();
    action_lstm.start_new_sequence();
    // variables in the computation graph representing the parameters
    Expression pbias = parameter(*hg, p_pbias);
    Expression H = parameter(*hg, p_H);
    Expression D = parameter(*hg, p_D);
    Expression R = parameter(*hg, p_R);
    Expression cbias = parameter(*hg, p_cbias);
    Expression S = parameter(*hg, p_S);
    Expression B = parameter(*hg, p_B);
    Expression A = parameter(*hg, p_A);
    Expression ib = parameter(*hg, p_ib);
    Expression w2l = parameter(*hg, p_w2l);
    Expression p2l;
    if (USE_POS)
      p2l = parameter(*hg, p_p2l);
    Expression t2l;
    if (p_t2l)
      t2l = parameter(*hg, p_t2l);
    Expression p2a = parameter(*hg, p_p2a);
    Expression abias = parameter(*hg, p_abias);
    Expression action_start = parameter(*hg, p_action_start);

    action_lstm.add_input(action_start);

    vector<Expression> buffer(sent.size() + 1);  // variables representing word embeddings (possibly including POS info)
    vector<int> bufferi(sent.size() + 1);  // position of the words in the sentence
    // precompute buffer representation from left to right

    for (unsigned i = 0; i < sent.size(); ++i) {
      assert(sent[i] < VOCAB_SIZE);
      Expression w =lookup(*hg, p_w, sent[i]);

      vector<Expression> args = {ib, w2l, w}; // learn embeddings
      if (USE_POS) { // learn POS tag?
        Expression p = lookup(*hg, p_p, sentPos[i]);
        args.push_back(p2l);
        args.push_back(p);
      }
      if (p_t && pretrained.count(raw_sent[i])) {  // include fixed pretrained vectors?
        Expression t = const_lookup(*hg, p_t, raw_sent[i]);
        args.push_back(t2l);
        args.push_back(t);
      }
      buffer[sent.size() - i] = rectify(affine_transform(args));
      bufferi[sent.size() - i] = i;
    }
    // dummy symbol to represent the empty buffer
    buffer[0] = parameter(*hg, p_buffer_guard);
    bufferi[0] = -999;
    for (auto& b : buffer)
      buffer_lstm.add_input(b);

    vector<Expression> stack;  // variables representing subtree embeddings
    vector<int> stacki; // position of words in the sentence of head of subtree
    stack.push_back(parameter(*hg, p_stack_guard));
    stacki.push_back(-999); // not used for anything
    // drive dummy symbol on stack through LSTM
    stack_lstm.add_input(stack.back());
    vector<Expression> log_probs;
    string rootword;
    unsigned action_count = 0;  // incremented at each prediction
    while(stack.size() > 2 || buffer.size() > 1) {
      // get list of possible actions for the current parser state
      vector<unsigned> current_valid_actions;
      for (auto a: possible_actions) {
        if (IsActionForbidden(setOfActions[a], buffer.size(), stack.size(), stacki))
          continue;
        current_valid_actions.push_back(a);
      }

      // p_t = pbias + S * slstm + B * blstm + A * almst
      Expression p_t = affine_transform({pbias, S, stack_lstm.back(), B, buffer_lstm.back(), A, action_lstm.back()});
      Expression nlp_t = rectify(p_t);
      // r_t = abias + p2a * nlp
      Expression r_t = affine_transform({abias, p2a, nlp_t});

      // adist = log_softmax(r_t, current_valid_actions)
      Expression adiste = log_softmax(r_t, current_valid_actions);
      vector<float> adist = as_vector(hg->incremental_forward());
      double best_score = adist[current_valid_actions[0]];
      unsigned best_a = current_valid_actions[0];
      for (unsigned i = 1; i < current_valid_actions.size(); ++i) {
        if (adist[current_valid_actions[i]] > best_score) {
          best_score = adist[current_valid_actions[i]];
          best_a = current_valid_actions[i];
        }
      }
      unsigned action = best_a;
      if (build_training_graph) {  // if we have reference actions (for training) use the reference action
        action = correct_actions[action_count];
        if (best_a == action) { (*right)++; }
      }
      ++action_count;
      log_probs.push_back(pick(adiste, action));
      results.push_back(action);

      // add current action to action LSTM
      Expression actione = lookup(*hg, p_a, action);
      action_lstm.add_input(actione);

      // get relation embedding from action (TODO: convert to relation from action?)
      Expression relation = lookup(*hg, p_r, action);

      // do action
      const string& actionString=setOfActions[action];
      const char ac = actionString[0];
      const char ac2 = actionString[1];

      if (ac =='S' && ac2=='H') {  // SHIFT
        assert(buffer.size() > 1); // dummy symbol means > 1 (not >= 1)
        stack.push_back(buffer.back());
        stack_lstm.add_input(buffer.back());
        buffer.pop_back();
        buffer_lstm.rewind_one_step();
        stacki.push_back(bufferi.back());
        bufferi.pop_back();
      } else if (ac=='S' && ac2=='W'){ //SWAP --- Miguel
        assert(stack.size() > 2); // dummy symbol means > 2 (not >= 2)

        Expression toki, tokj;
        unsigned ii = 0, jj = 0;
        tokj=stack.back();
        jj=stacki.back();
        stack.pop_back();
        stacki.pop_back();

        toki=stack.back();
        ii=stacki.back();
        stack.pop_back();
        stacki.pop_back();

        buffer.push_back(toki);
        bufferi.push_back(ii);

        stack_lstm.rewind_one_step();
        stack_lstm.rewind_one_step();

        buffer_lstm.add_input(buffer.back());

        stack.push_back(tokj);
        stacki.push_back(jj);

        stack_lstm.add_input(stack.back());
      } else { // LEFT or RIGHT
        assert(stack.size() > 2); // dummy symbol means > 2 (not >= 2)
        assert(ac == 'L' || ac == 'R');
        Expression dep, head;
        unsigned depi = 0, headi = 0;
        (ac == 'R' ? dep : head) = stack.back();
        (ac == 'R' ? depi : headi) = stacki.back();
        stack.pop_back();
        stacki.pop_back();
        (ac == 'R' ? head : dep) = stack.back();
        (ac == 'R' ? headi : depi) = stacki.back();
        stack.pop_back();
        stacki.pop_back();
        if (headi == sent.size() - 1) rootword = intToWords.find(sent[depi])->second;
        // composed = cbias + H * head + D * dep + R * relation
        Expression composed = affine_transform({cbias, H, head, D, dep, R, relation});
        Expression nlcomposed = tanh(composed);
        stack_lstm.rewind_one_step();
        stack_lstm.rewind_one_step();
        stack_lstm.add_input(nlcomposed);
        stack.push_back(nlcomposed);
        stacki.push_back(headi);
      }
    }
    assert(stack.size() == 2); // guard symbol, root
    assert(stacki.size() == 2);
    assert(buffer.size() == 1); // guard symbol
    assert(bufferi.size() == 1);
    Expression tot_neglogprob = -sum(log_probs);
    assert(tot_neglogprob.pg != nullptr);
    return results;
  }
};

void signal_callback_handler(int /* signum */) {
  if (requested_stop) {
    cerr << "\nReceived SIGINT again, quitting.\n";
    _exit(1);
  }
  cerr << "\nReceived SIGINT terminating optimization early...\n";
  requested_stop = true;
}

unsigned compute_correct(const map<int,int>& ref, const map<int,int>& hyp, unsigned len) {
  unsigned res = 0;
  for (unsigned i = 0; i < len; ++i) {
    auto ri = ref.find(i);
    auto hi = hyp.find(i);
    assert(ri != ref.end());
    assert(hi != hyp.end());
    if (ri->second == hi->second) ++res;
  }
  return res;
}

void output_conll(const vector<unsigned>& sentence, const vector<unsigned>& pos,
                  const vector<string>& sentenceUnkStrings,
                  const map<unsigned, string>& intToWords,
                  const map<unsigned, string>& intToPos,
                  const map<int,int>& hyp, const map<int,string>& rel_hyp) {
  for (unsigned i = 0; i < (sentence.size()-1); ++i) {
    auto index = i + 1;
    assert(i < sentenceUnkStrings.size() &&
           ((sentence[i] == corpus.get_or_add_word(cpyp::Corpus::UNK) &&
             sentenceUnkStrings[i].size() > 0) ||
            (sentence[i] != corpus.get_or_add_word(cpyp::Corpus::UNK) &&
             sentenceUnkStrings[i].size() == 0 &&
             intToWords.find(sentence[i]) != intToWords.end())));
    string wit = (sentenceUnkStrings[i].size() > 0)?
      sentenceUnkStrings[i] : intToWords.find(sentence[i])->second;
    auto pit = intToPos.find(pos[i]);
    assert(hyp.find(i) != hyp.end());
    auto hyp_head = hyp.find(i)->second + 1;
    if (hyp_head == (int)sentence.size()) hyp_head = 0;
    auto hyp_rel_it = rel_hyp.find(i);
    assert(hyp_rel_it != rel_hyp.end());
    auto hyp_rel = hyp_rel_it->second;
    size_t first_char_in_rel = hyp_rel.find('(') + 1;
    size_t last_char_in_rel = hyp_rel.rfind(')') - 1;
    hyp_rel = hyp_rel.substr(first_char_in_rel, last_char_in_rel - first_char_in_rel + 1);
    cout << index << '\t'       // 1. ID
         << wit << '\t'         // 2. FORM
         << "_" << '\t'         // 3. LEMMA
         << "_" << '\t'         // 4. CPOSTAG
         << pit->second << '\t' // 5. POSTAG
         << "_" << '\t'         // 6. FEATS
         << hyp_head << '\t'    // 7. HEAD
         << hyp_rel << '\t'     // 8. DEPREL
         << "_" << '\t'         // 9. PHEAD
         << "_" << endl;        // 10. PDEPREL
  }
  cout << endl;
}

int main(int argc, char** argv) {
  cnn::Initialize(argc, argv);

  cerr << "COMMAND:";
  for (unsigned i = 0; i < static_cast<unsigned>(argc); ++i) cerr << ' ' << argv[i];
  cerr << endl;
  unsigned status_every_i_iterations = 100;

  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  USE_POS = conf.count("use_pos_tags");

  LAYERS = conf["layers"].as<unsigned>();
  INPUT_DIM = conf["input_dim"].as<unsigned>();
  PRETRAINED_DIM = conf["pretrained_dim"].as<unsigned>();
  HIDDEN_DIM = conf["hidden_dim"].as<unsigned>();
  ACTION_DIM = conf["action_dim"].as<unsigned>();
  LSTM_INPUT_DIM = conf["lstm_input_dim"].as<unsigned>();
  POS_DIM = conf["pos_dim"].as<unsigned>();
  REL_DIM = conf["rel_dim"].as<unsigned>();
  const unsigned unk_strategy = conf["unk_strategy"].as<unsigned>();
  cerr << "Unknown word strategy: ";
  if (unk_strategy == 1) {
    cerr << "STOCHASTIC REPLACEMENT\n";
  } else {
    abort();
  }
  const double unk_prob = conf["unk_prob"].as<double>();
  assert(unk_prob >= 0.); assert(unk_prob <= 1.);
  ostringstream os;
  os << "parser_" << (USE_POS ? "pos" : "nopos")
     << '_' << LAYERS
     << '_' << INPUT_DIM
     << '_' << HIDDEN_DIM
     << '_' << ACTION_DIM
     << '_' << LSTM_INPUT_DIM
     << '_' << POS_DIM
     << '_' << REL_DIM
     << "-pid" << getpid() << ".params";
  int best_correct_heads = 0;
  const string fname = os.str();
  cerr << "Writing parameters to file: " << fname << endl;
  bool softlinkCreated = false;
  corpus.load_correct_actions(conf["training_data"].as<string>());
  const unsigned kUNK = corpus.get_or_add_word(cpyp::Corpus::UNK);
  kROOT_SYMBOL = corpus.get_or_add_word(ROOT_SYMBOL);

  if (conf.count("words")) {
    pretrained[kUNK] = vector<float>(PRETRAINED_DIM, 0);
    cerr << "Loading from " << conf["words"].as<string>() << " with" << PRETRAINED_DIM << " dimensions\n";
    ifstream in(conf["words"].as<string>().c_str());
    string line;
    getline(in, line);
    vector<float> v(PRETRAINED_DIM, 0);
    string word;
    while (getline(in, line)) {
      istringstream lin(line);
      lin >> word;
      for (unsigned i = 0; i < PRETRAINED_DIM; ++i) lin >> v[i];
      unsigned id = corpus.get_or_add_word(word);
      pretrained[id] = v;
    }
  }

  set<unsigned> training_vocab; // words available in the training corpus
  set<unsigned> singletons;
  {  // compute the singletons in the parser's training data
    map<unsigned, unsigned> counts;
    for (auto sent : corpus.sentences)
      for (auto word : sent.second) { training_vocab.insert(word); counts[word]++; }
    for (auto wc : counts)
      if (wc.second == 1) singletons.insert(wc.first);
  }

  cerr << "Number of words: " << corpus.nwords << endl;
  VOCAB_SIZE = corpus.nwords + 1;
  ACTION_SIZE = corpus.nactions + 1;
  POS_SIZE = corpus.npos + 10;  // bad way of dealing with the fact that we may see new POS tags in the test set
  possible_actions.resize(corpus.nactions);
  for (unsigned i = 0; i < corpus.nactions; ++i)
    possible_actions[i] = i;

  Model model;
  ParserBuilder parser(&model, pretrained);
  if (conf.count("model")) {
    ifstream in(conf["model"].as<string>().c_str());
    boost::archive::text_iarchive ia(in);
    ia >> model;
  }

  // OOV words will be replaced by UNK tokens
  corpus.load_correct_actionsDev(conf["dev_data"].as<string>());
  //TRAINING
  if (conf.count("train")) {
    signal(SIGINT, signal_callback_handler);
    SimpleSGDTrainer sgd(&model);
    //MomentumSGDTrainer sgd(&model);
    sgd.eta_decay = 0.08;
    //sgd.eta_decay = 0.05;
    vector<unsigned> order(corpus.nsentences);
    for (unsigned i = 0; i < corpus.nsentences; ++i)
      order[i] = i;
    double tot_seen = 0;
    status_every_i_iterations = min(status_every_i_iterations, corpus.nsentences);
    unsigned si = corpus.nsentences;
    cerr << "NUMBER OF TRAINING SENTENCES: " << corpus.nsentences << endl;
    unsigned trs = 0;
    double right = 0;
    double llh = 0;
    bool first = true;
    int iter = -1;
    time_t time_start = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
    //cerr << "TRAINING STARTED AT: " << put_time(localtime(&time_start), "%c %Z") << endl;
    while(!requested_stop) {
      ++iter;
      for (unsigned sii = 0; sii < status_every_i_iterations; ++sii) {
           if (si == corpus.nsentences) {
             si = 0;
             if (first) { first = false; } else { sgd.update_epoch(); }
             cerr << "**SHUFFLE\n";
             random_shuffle(order.begin(), order.end());
           }
           tot_seen += 1;
           const vector<unsigned>& sentence=corpus.sentences[order[si]];
           vector<unsigned> tsentence=sentence;
           if (unk_strategy == 1) {
             for (auto& w : tsentence)
               if (singletons.count(w) && cnn::rand01() < unk_prob) w = kUNK;
           }
	   const vector<unsigned>& sentencePos=corpus.sentencesPos[order[si]];
	   const vector<unsigned>& actions=corpus.correct_act_sent[order[si]];
           ComputationGraph hg;
           parser.log_prob_parser(&hg,sentence,tsentence,sentencePos,actions,corpus.actions,corpus.intToWords,&right);
           double lp = as_scalar(hg.incremental_forward());
           if (lp < 0) {
             cerr << "Log prob < 0 on sentence " << order[si] << ": lp=" << lp << endl;
             assert(lp >= 0.0);
           }
           hg.backward();
           sgd.update(1.0);
           llh += lp;
           ++si;
           trs += actions.size();
      }
      sgd.status();
      //time_t time_now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
      time_t time_now = time(NULL);
      std::string t_n(asctime(localtime(&time_now)));
      cerr << "update #" << iter << " (epoch " << (tot_seen / corpus.nsentences) << " |time=" << t_n.substr(0, t_n.size() - 1) << ")\tllh: "<< llh<<" ppl: " << exp(llh / trs) << " err: " << (trs - right) / trs << endl;
      llh = trs = right = 0;

      static int logc = 0;
      ++logc;
      if (logc % 25 == 1) { // report on dev set
        unsigned dev_size = corpus.nsentencesDev;
        // dev_size = 100;
        double llh = 0;
        double trs = 0;
        double right = 0;
        double correct_heads = 0;
        double total_heads = 0;
        auto t_start = std::chrono::high_resolution_clock::now();
        for (unsigned sii = 0; sii < dev_size; ++sii) {
           const vector<unsigned>& sentence=corpus.sentencesDev[sii];
	   const vector<unsigned>& sentencePos=corpus.sentencesPosDev[sii];
	   const vector<unsigned>& actions=corpus.correct_act_sentDev[sii];
           vector<unsigned> tsentence=sentence;
           for (auto& w : tsentence)
             if (training_vocab.count(w) == 0) w = kUNK;

           ComputationGraph hg;
	   vector<unsigned> pred = parser.log_prob_parser(&hg,sentence,tsentence,sentencePos,vector<unsigned>(),corpus.actions,corpus.intToWords,&right);
	   double lp = 0;
           llh -= lp;
           trs += actions.size();
           map<int,int> ref = parser.compute_heads(sentence.size(), actions, corpus.actions);
           map<int,int> hyp = parser.compute_heads(sentence.size(), pred, corpus.actions);
           //output_conll(sentence, corpus.intToWords, ref, hyp);
           correct_heads += compute_correct(ref, hyp, sentence.size() - 1);
           total_heads += sentence.size() - 1;
        }
        auto t_end = std::chrono::high_resolution_clock::now();
        cerr << "  **dev (iter=" << iter << " epoch=" << (tot_seen / corpus.nsentences) << ")\tllh=" << llh << " ppl: " << exp(llh / trs) << " err: " << (trs - right) / trs << " uas: " << (correct_heads / total_heads) << "\t[" << dev_size << " sents in " << std::chrono::duration<double, std::milli>(t_end-t_start).count() << " ms]" << endl;
        if (correct_heads > best_correct_heads) {
	  cerr << "saving current best model" << endl;
          best_correct_heads = correct_heads;
          ofstream out(fname);
          boost::archive::text_oarchive oa(out);
          oa << model;
          // Create a soft link to the most recent model in order to make it
          // easier to refer to it in a shell script.
          if (!softlinkCreated) {
            string softlink = " latest_model";
            if (system((string("rm -f ") + softlink).c_str()) == 0 &&
                system((string("ln -s ") + fname + softlink).c_str()) == 0) {
              cerr << "Created " << softlink << " as a soft link to " << fname
                   << " for convenience." << endl;
            }
            softlinkCreated = true;
          }
        }
      }
    }
  } // should do training?
  if (true) { // do test evaluation
    double llh = 0;
    double trs = 0;
    double right = 0;
    double correct_heads = 0;
    double total_heads = 0;
    auto t_start = std::chrono::high_resolution_clock::now();
    unsigned corpus_size = corpus.nsentencesDev;
    for (unsigned sii = 0; sii < corpus_size; ++sii) {
      const vector<unsigned>& sentence=corpus.sentencesDev[sii];
      const vector<unsigned>& sentencePos=corpus.sentencesPosDev[sii];
      const vector<string>& sentenceUnkStr=corpus.sentencesStrDev[sii];
      const vector<unsigned>& actions=corpus.correct_act_sentDev[sii];
      vector<unsigned> tsentence=sentence;
      for (auto& w : tsentence)
        if (training_vocab.count(w) == 0) w = kUNK;
      ComputationGraph cg;
      double lp = 0;
      vector<unsigned> pred;
      pred = parser.log_prob_parser(&cg,sentence,tsentence,sentencePos,vector<unsigned>(),corpus.actions,corpus.intToWords,&right);
      llh -= lp;
      trs += actions.size();
      map<int, string> rel_ref, rel_hyp;
      map<int,int> ref = parser.compute_heads(sentence.size(), actions, corpus.actions, &rel_ref);
      map<int,int> hyp = parser.compute_heads(sentence.size(), pred, corpus.actions, &rel_hyp);
      output_conll(sentence, sentencePos, sentenceUnkStr, corpus.intToWords, corpus.intToPos, hyp, rel_hyp);
      correct_heads += compute_correct(ref, hyp, sentence.size() - 1);
      total_heads += sentence.size() - 1;
    }
    auto t_end = std::chrono::high_resolution_clock::now();
    cerr << "TEST llh=" << llh << " ppl: " << exp(llh / trs) << " err: " << (trs - right) / trs << " uas: " << (correct_heads / total_heads) << "\t[" << corpus_size << " sents in " << std::chrono::duration<double, std::milli>(t_end-t_start).count() << " ms]" << endl;
  }
  for (unsigned i = 0; i < corpus.actions.size(); ++i) {
    //cerr << corpus.actions[i] << '\t' << parser.p_r->values[i].transpose() << endl;
    //cerr << corpus.actions[i] << '\t' << parser.p_p2a->values.col(i).transpose() << endl;
  }
}


// #pragma GCC optimize ("O0")

#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <iostream>

#include <boost/log/trivial.hpp>
#include <boost/algorithm/string.hpp>

#include "navigation.hpp"
#include "constants.hpp"
#include "antenna.hpp"
#include "common.hpp"
#include "enums.h"

#include "eigenIncluder.hpp"

/* decode antenna field */
int decodef(char *p, int n, double *v)
{
	int i;
	for (i = 0; i < n; i++)
		v[i] = 0;

	for (i = 0, p = strtok(p," "); p && i < n; p = strtok(nullptr, " "))
	{
		v[i] = atof(p) * 1E-3;
		i++;
	}
	return i;
}

bool findAntenna(
	string				code,
	GTime				time,
	nav_t&				nav,
	E_FType				ft,
	PhaseCenterData**	pcd_ptr_ptr)
{
// 	BOOST_LOG_TRIVIAL(debug)
// 	<< "Searching for " << type << ", " << code;

	auto it1 = nav.pcvMap.find(code);
	if (it1 == nav.pcvMap.end())
	{
		return false;
	}

	auto& [dummyCode, pcvFreqMap] = *it1;

	auto it2 = pcvFreqMap.find(ft);
	if (it2 == pcvFreqMap.end())
	{
		return false;
	}

	auto& [dummy2, pcvTimeMap] = *it2;

	auto it3 = pcvTimeMap.lower_bound(time);
	if (it3 == pcvTimeMap.end())
	{
		//just use the first chronologically, (last when sorted as they are) instead
		auto it4 = pcvTimeMap.rbegin();

		auto& [dummyTime, pcd] = *it4;

		if (pcd_ptr_ptr)
			*pcd_ptr_ptr = &pcd;

		return true;
	}

	auto& [dummyTime, pcd] = *it3;

	if (pcd_ptr_ptr)
		*pcd_ptr_ptr = &pcd;

	return true;
}

/* linear interpolate pcv ------------------------------------------------------
*
* args     :       double x1              I       x1 lower bound (degree)
*                  double x2              I       x2 upper bound (degree)
*                  double y1              I       y1 lower bound (m)
*                  double y2              I       y2 upper bound (m)
*                  double x               O       x current point (degree)
*
* return   :       interpolated pcv (m)
*----------------------------------------------------------------------------*/
double interp(double x1, double x2, double y1, double y2, double x)
{
#if (0)
	return (y2-y1)*(x-x1)/(x2-x1)+y1;
#endif
	return y2-(y2-y1)*(x2-x)/(x2-x1);
}

Vector3d makeAntPco(
	string		id,
	E_FType		ft,
	GTime		time)
{
	if (ft == F1)		return Vector3d::Zero();
	if (ft == F2)		return Vector3d::Zero();

	Vector3d pco1 = antPco(id, F1, time);
	Vector3d pco2 = antPco(id, F2, time);

	if (pco1.isZero())	return Vector3d::Zero();
	if (pco2.isZero())	return Vector3d::Zero();

	double lam1 = lam_carr[F1];
	double lam2 = lam_carr[F2];
	double lamX = lam_carr[ft];

	if (lamX == 0)		return Vector3d::Zero();

	double k32 = (lamX-lam2)/(lam1-lam2);
	double k31 = (lamX-lam1)/(lam1-lam2);

	Vector3d pco	= k32 * pco1
					- k31 * pco2;

	return pco;
}

/** fetch pco
 */
Vector3d antPco(
	string		id,
	E_FType		ft,
	GTime		time,
	bool		interp)
{
	auto it1 = nav.pcoMap.find(id);
	if (it1 == nav.pcoMap.end())
	{
		return Vector3d::Zero();
	}

	auto& [dummy1, pcoFreqMap] = *it1;

	auto it2 = pcoFreqMap.find(ft);
	if (it2 == pcoFreqMap.end())
	{
		if (interp)		return makeAntPco(id, ft, time);
		else			return Vector3d::Zero();
	}

	auto& [dummy2, pcoTimeMap] = *it2;

	auto it3 = pcoTimeMap.lower_bound(time);
	if (it3 == pcoTimeMap.end())
	{
		return Vector3d::Zero();
	}

	auto& [dummy3, pco] = *it3;

	return pco;
}

/** find and interpolate antenna pcv
*/
double antPcv(
	string		id,		///< antenna id
	E_FType		ft,		///< frequency
	GTime		time,	///< time
	double		aCos,	///< angle between target and antenna axis (radians)
	double		azi)	///< azimuth angle (radians)
{
	auto it1 = nav.pcvMap.find(id);
	if (it1 == nav.pcvMap.end())
	{
		return 0;
	}

	auto& [dummy1, pcvFreqMap] = *it1;

	auto it2 = pcvFreqMap.find(ft);
	if (it2 == pcvFreqMap.end())
	{
		return 0;
	}

	auto& [dummy2, pcvTimeMap] = *it2;

	auto it3 = pcvTimeMap.lower_bound(time);
	if (it3 == pcvTimeMap.end())
	{
		return 0;
	}

	auto& [dummy3, pcd] = *it3;

	auto& pcvMap1D = pcd.PCVMap1D;
	auto& pcvMap2D = pcd.PCVMap2D;

	int		nz		= pcd.nz;
	int		naz		= pcd.naz;
	double	zen1	= pcd.zenStart;
	double	dzen	= pcd.zenDelta;
	double	dazi	= pcd.aziDelta;
	double	zen		= aCos * R2D;
	azi *= R2D;

	double	pcv;

	/* select zenith angle range */
	int zen_n;
	for (zen_n = 1; zen_n < nz; zen_n++)
	{
		if ((zen1 + dzen * zen_n) >= zen)
		{
			break;
		}
	}

	double xz1 = zen1 + dzen * (zen_n - 1);
	double xz2 = zen1 + dzen * (zen_n);

	if	( naz == 0
		||azi == 0)
	{
		/* linear interpolate receiver pcv - non azimuth-dependent */
		/* interpolate */

		double	yz1 = pcvMap1D[zen_n - 1];		// lower bound
		double	yz2 = pcvMap1D[zen_n];			// upper bound
		pcv = interp(xz1, xz2, yz1, yz2, zen);
	}
	else
	{
		/* bilinear interpolate receiver pcv - azimuth-dependent */
		/* select azimuth angle range */
		int az_n;
		for (az_n = 1; az_n < naz; az_n++)
		{
			if ((dazi * az_n) >= azi)
			{
				break;
			}
		}

		double xa1 = dazi * (az_n -1);
		double xa2 = dazi * (az_n);

		double yz3 = pcvMap2D[az_n-1]	[zen_n-1];		double yz1 = pcvMap2D[az_n-1]	[zen_n];
		double yz4 = pcvMap2D[az_n]		[zen_n-1];		double yz2 = pcvMap2D[az_n]		[zen_n];

		/* linear interpolation along zenith angle */
		double ya1	= interp(xz1, xz2, yz3, yz1, zen);
		double ya2 	= interp(xz1, xz2, yz4, yz2, zen);

		/* linear interpolation along azimuth angle */
		pcv	= interp(xa1, xa2, ya1, ya2, azi);
	}

	return pcv;
}

//=============================================================================
// radomeNoneAntennaType = radome2none(antennaType)
//
//       e,g, "AOAD/M_T        JPLA" => "AOAD/M_T        NONE"
//
// Change the last four characters of antenna type to NONE
// This function is useful for when searching for an antenna model in ANTEX
//
// The IGS convention is to default to NONE for the radome if the calibration
// value is not available
//=============================================================================
//void radome2none(char *restrict antenna_type)
void radome2none(string& antenna_type)
{
	size_t length = antenna_type.size();
	if (length != 20)
	{
		printf("\n*** ERROR radome2none(): string length is less then 20 characters received %ld characters\n",length);
		return;
	}
	antenna_type.replace(length - 4, 4, "NONE");
}

map<string, E_FType> antexCodes =
{
	{"G01", L1    },
	{"G02", L2    },
	{"G05", L5    },
	{"R01", G1    },
	{"R02", G2    },
	{"E01", E1    },
	{"E05", E5A   },
	{"E07", E5B   },
	{"E08", E5AB  },
	{"E06", E6    },
	{"C01", E1    },
	{"C02", E2    },
	{"C07", E5B   },
	{"C06", E6    },
	{"J01", L1    },
	{"J02", L2    },
	{"J05", L5    },
	{"J06", LEX   },
	{"S01", L1    },
	{"S05", L5    }
};

/** Read antex file */
int readantexf(
	string	file,
	nav_t&	nav)
{
	int offset;
	int noazi_flag		= 0;
	int num_azi_rd		= 0;
	int new_antenna		= 0;
	int irms			= 0;

	char tmp[10];
	char *p;

	FILE* fp = fopen(file.c_str(),"r");
	if (fp == nullptr)
	{
		BOOST_LOG_TRIVIAL(warning)
		<< "Warning: ANTEX file opening error";

		return 0;
	}

	const PhaseCenterData pcv0 = {};
	PhaseCenterData recPcv;
	PhaseCenterData freqPcv;
	Vector3d	pco = Vector3d::Zero();
	string		id;
	GTime		time;

	E_FType	ft = FTYPE_NONE;

	char buff[512];
	while (fgets(buff, sizeof(buff), fp))
	{
		char* comment = buff + 60;

		if (irms)
			continue;

		/* Read in the ANTEX header information */

		if (strlen(buff) < 60 )								{	continue;	}
		if (strstr(comment, "ANTEX VERSION / SYST"))		{	continue;	}
		if (strstr(comment, "PCV TYPE / REFANT")) 			{	continue;	}
		if (strstr(comment, "COMMENT")) 					{	continue;	}
		if (strstr(comment, "END OF HEADER"))				{	continue;	}

		/* Read in specific Antenna information now */

		if (strstr(comment, "START OF ANTENNA"))
		{
			recPcv	= pcv0;
			freqPcv	= pcv0;
			pco		= Vector3d::Zero();
			id		= "";

			continue;
		}
// 		if (strstr(comment, "END OF ANTENNA"))
// 		{
// 			GTime time = epoch2time(recPcv.tf);
//
// 			continue;
// 		}

		if (strstr(comment, "METH / BY / # / DATE"))
		{
// 			int num_calibrated;
// 			char cal_method[20];
// 			char cal_agency[20];
// 			char cal_date[10];
// 			strncpy(cal_method,	buff,		20);/* Should be CHAMBER or FIELD or ROBOT or COPIED ot CONVERTED */
// 			cal_method[19] = '\0';
// 			strncpy(cal_agency, buff + 20,	20);
// 			cal_agency[19] = '\0';
// 			strncpy(tmp,		buff + 40,	10);
// 			num_calibrated = atoi(tmp);
// 			strncpy(cal_date,	buff + 50,	10);
// 			cal_date[9] = '\0';

			continue;
		}

		if (strstr(comment, "DAZI"))
		{
			strncpy(tmp,buff   ,8);		tmp[8] = '\0';
			recPcv.aziDelta = atof(tmp);

			if (recPcv.aziDelta < 0.0001)	recPcv.naz = 0;
			else							recPcv.naz = (360 / recPcv.aziDelta) + 1;

			continue;
		}

		if (strstr(comment, "SINEX CODE"))
		{
			recPcv.calibModel	.assign(buff,		10);
			continue;
		}

		if (strstr(comment, "TYPE / SERIAL NO"))
		{
			recPcv.type			.assign(buff,		20);
			recPcv.code			.assign(buff+20,	20);
			recPcv.svn			.assign(buff+40,	4);
			recPcv.cospar		.assign(buff+50,	10);

			/* stack antenna pco and pcv */
			string satId = recPcv.code;
			if (satId.find_first_not_of(' ') == satId.npos)		{ id = recPcv.type;	}
			else												{ id = recPcv.code;	}

			boost::trim_right(id);

			continue;
		}

		if (strstr(comment, "ZEN1 / ZEN2 / DZEN"))
		{
			strncpy(tmp, buff,		8);	tmp[8] = '\0'; 	recPcv.zenStart	= atof(tmp);
			strncpy(tmp, buff+8,	7);	tmp[8] = '\0'; 	recPcv.zenStop	= atof(tmp);
			strncpy(tmp, buff+16,	7);	tmp[8] = '\0'; 	recPcv.zenDelta	= atof(tmp);

			recPcv.nz = (recPcv.zenStop - recPcv.zenStart) / recPcv.zenDelta + 1 ;

			continue;
		}

		if (strstr(comment, "# OF FREQUENCIES"))
		{
//			strncpy(tmp, buff,		8);
// 			tmp[8] = '\0';
// 			pcv.nf		= atoi(tmp);

			continue;
		}

		if (strstr(comment, "VALID FROM"))
		{
			char valid_from[44];
			/* if (!str2time(buff,0,43,pcv.ts)) continue;*/
			strncpy(valid_from, buff, 43);
			valid_from[43] = '\0';
			p = strtok(valid_from, " ");
			int j = 0;
			while (p != nullptr)
			{
				recPcv.tf[j] = (double) atoi(p);
				p = strtok(nullptr, " ");
				j++;
			}

			time = epoch2time(recPcv.tf);

			continue;
		}

		if (strstr(comment, "VALID UNTIL"))
		{
			char valid_until[44];
			/* if (!str2time(buff,0,43,pcv.te)) continue;*/
			strncpy(valid_until, buff   ,43);
			valid_until[43] = '\0';
			p = strtok(valid_until, " ");
			int j = 0;
			while (p != nullptr)
			{
				recPcv.tu[j] = (double) atoi(p);
				p = strtok(nullptr, " ");
				j++;
			}

			continue;
		}

		if (strstr(comment, "NORTH / EAST / UP"))
		{
			double neu[3];
			if (decodef(buff, 3, neu) < 3)
			{
				continue;
			}

			/* assign pco value in ENU */
			Vector3d enu;
			enu[0] = neu[1];
			enu[1] = neu[0];
			enu[2] = neu[2];

			pco = enu;

			continue;
		}

		if (strstr(comment, "START OF FREQUENCY"))
		{
			num_azi_rd = 0;
			noazi_flag = 0;

			string antexFCode;
			antexFCode.assign(&buff[3], 3);

			ft = antexCodes[antexFCode];

			freqPcv = recPcv;

			continue;
		}

		if (strstr(comment, "END OF FREQUENCY"))
		{
			noazi_flag	= 0;

			nav.pcvMap[id][ft][time] = freqPcv;
			nav.pcoMap[id][ft][time] = pco;

			continue;
		}

		if (strstr(comment, "START OF FREQ RMS"))	{	irms		= 1;	continue;	}
		if (strstr(comment, "END OF FREQ RMS"))		{	irms		= 0;	continue;	}

		if	(  irms == 0
			&& strstr(buff, "NOAZI"))
		{
			for (int i = 0; i < recPcv.nz; i++)
			{
				offset = i * 8 + 8;
				strncpy(tmp, buff + offset, 8);
				tmp[8]='\0';
				double pcv_val = atof(tmp);
				freqPcv.PCVMap1D			.push_back(pcv_val * 1e-3);
			}
			noazi_flag = 1;

			continue;
		}

		if	(  irms == 0
			&& noazi_flag == 1)
		{
			strncpy(tmp, buff, 8);
			tmp[8]='\0';

			for (int i = 0; i < recPcv.nz; i++)
			{
				offset = i * 8 + 8;
				strncpy(tmp, buff + offset, 8);
				tmp[8]='\0';
				double pcv_val = atof(tmp);
				freqPcv.PCVMap2D[num_azi_rd].push_back(pcv_val * 1e-3);
			}
			num_azi_rd++;

			continue;
		}
	}

	fclose(fp);

	return 1;
}


/*===========================================================================
 NetEvo Library
 Copyright (C) 2011 Thomas E. Gorochowski <tgorochowski@me.com>
 Bristol Centre for Complexity Sciences, University of Bristol, Bristol, UK
 ----------------------------------------------------------------------------
 NetEvo is a computing framework designed to allow researchers to investigate
 evolutionary aspects of dynamical complex networks. By providing tools to
 easily integrate each of these factors in a coherent way, it is hoped a
 greater understanding can be gained of key attributes and features displayed
 by complex systems.

 NetEvo is open-source software released under the Open Source Initiative
 (OSI) approved Non-Profit Open Software License ("Non-Profit OSL") 3.0.
 Detailed information about this licence can be found in the COPYING file
 included as part of the source distribution.

 This library 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.
 ============================================================================*/

#include "simulate.h"
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/adams_bashforth_moulton.hpp>

using namespace boost::numeric::odeint;

namespace netevo {

   void SimulateMap::simulate (System &sys, double tMax, State &initial, SimObserver &obs, ChangeLog &logger) {

      // We are in discrete time so use integers for time
      int t = 0, tEnd = (int)tMax;
      int states = (countNodes(sys)*sys.nodeStates()) + (countArcs(sys)*sys.arcStates());

      // Check to ensure that initial conditions are correct size
      if (initial.size() < states || initial.size() > states) {
         cerr << "Incorrect number of states for initial conditions (SimulateMap::simulate)" << endl;
         return;
      }

      // Vector to hold the output. Copy the initial conditions and add to the output
      State y1 = State(initial);
      State y2 = State(initial);

      // Check that the state IDs are correct, if not refresh
      if (!sys.validStateIDs()) { sys.refreshStateIDs(); }

      // Send the observer the initial conditions
      logger.newState (sys, y1);
      logger.endStep(SIM_STEP);
      logger.commit();
      obs(y1, 0.0);

      // Loop through all time steps and calculate new states
      for (t = 1; t <= tEnd; t++) {
         if (t%2 == 0) {
            // Use y1 as old and y2 as new
            // Simulate the dynamics
            sys(y1, y2, (double)t);
            // Log the state change
            logger.newState (sys, y2);
            logger.endStep(SIM_STEP);
            logger.commit();
            // Send result to the observer
            obs(y2, (double)t);
         }
         else {
            // Use y2 as old and y1 as new
            // Simulate the dynamics
            sys(y2, y1, (double)t);
            // Log the state change
            logger.newState (sys, y1);
            logger.endStep(SIM_STEP);
            logger.commit();
            // Send result to the observer
            obs(y1, (double)t);
         }
      }

      // Ensure the initial vector is updated to the final result
      initial = ( t%2 == 0 )  ? y1 : y2;
   }

   void SimulateOdeFixed::simulate (System &sys, double tMax, State &initial, SimObserver &obs, ChangeLog &logger) {

      // Check to ensure that initial conditions are correct size
      int states = (countNodes(sys)*sys.nodeStates()) + (countArcs(sys)*sys.arcStates());
      if (initial.size() < states || initial.size() > states) {
         cerr << "Incorrect number of states for initial conditions (SimulateOdeFixed::simulate)" << endl;
         return;
      }

      // Check that the state IDs are correct, if not refresh
      if (!sys.validStateIDs()) { sys.refreshStateIDs(); }

      // Create the required steppers
      typedef runge_kutta4<State> rk4_stepper_type;
      typedef adams_bashforth_moulton<5,State> adams_bash_moul_stepper_type;

      // Solve the system
      switch (mStepper) {
         case RK_4:
            integrate_const(rk4_stepper_type(),
                                    Simulator(&sys), initial, 0.0, tMax, mStepSize, ObserverPassThrough(sys, obs, logger));
            break;
         case ADAM_BASH_MOUL:
            integrate_const(adams_bash_moul_stepper_type(),
                                    Simulator(&sys), initial, 0.0, tMax, mStepSize, ObserverPassThrough(sys, obs, logger));
            break;
         default:
            // Do nothing
            break;
      }
   }

   void SimulateOdeConst::simulate (System &sys, double tMax, State &initial, SimObserver &obs, ChangeLog &logger) {

      // Check to ensure that initial conditions are correct size
      int states = (countNodes(sys)*sys.nodeStates()) + (countArcs(sys)*sys.arcStates());
      if (initial.size() < states || initial.size() > states) {
         cerr << "Incorrect number of states for initial conditions (SimulateOdeConst::simulate)" << endl;
         return;
      }

      // Check that the state IDs are correct, if not refresh
      if (!sys.validStateIDs()) { sys.refreshStateIDs(); }

      // Create the required steppers
      typedef runge_kutta_cash_karp54<State> rkck54_error_stepper_type;
      typedef runge_kutta_dopri5<State> dopri5_error_stepper_type;

      // Solve the system
      switch (mStepper) {
         case RK_CASH_KARP_54:
            integrate_const(make_controlled( mEpsAbs , mEpsRel , rkck54_error_stepper_type() ),
                                    Simulator(&sys), initial, 0.0, tMax, mOutputStep, ObserverPassThrough(sys, obs, logger));
            break;
         case RK_DOPRI_5:
            integrate_const(make_controlled( mEpsAbs , mEpsRel , dopri5_error_stepper_type() ),
                                    Simulator(&sys), initial, 0.0, tMax, mOutputStep, ObserverPassThrough(sys, obs, logger));
            break;
         case RK_DOPRI_5_DENSE:
            integrate_const(make_dense_output( mEpsAbs , mEpsRel , dopri5_error_stepper_type() ),
                                    Simulator(&sys), initial, 0.0, tMax, mOutputStep, ObserverPassThrough(sys, obs, logger));
            break;
         default:
            // Do nothing
            break;
      }
   }

   void SimulateOdeAdaptive::simulate (System &sys, double tMax, State &initial, SimObserver &obs, ChangeLog &logger) {

      // Check to ensure that initial conditions are correct size
      int states = (countNodes(sys)*sys.nodeStates()) + (countArcs(sys)*sys.arcStates());
      if (initial.size() < states || initial.size() > states) {
         cerr << "Incorrect number of states for initial conditions (SimulateOdeAdaptive::simulate)" << endl;
         return;
      }

      // Check that the state IDs are correct, if not refresh
      if (!sys.validStateIDs()) { sys.refreshStateIDs(); }

      // Create the required steppers
      typedef runge_kutta_cash_karp54<State> rkck54_error_stepper_type;
      typedef runge_kutta_dopri5<State> dopri5_error_stepper_type;

      // Solve the system
      switch (mStepper) {
         case RK_CASH_KARP_54:
            integrate_adaptive(make_controlled( mEpsAbs , mEpsRel , rkck54_error_stepper_type() ),
                                       Simulator(&sys), initial, 0.0, tMax, mInitialStep, ObserverPassThrough(sys, obs, logger));
            break;
         case RK_DOPRI_5:
            integrate_adaptive(make_controlled( mEpsAbs , mEpsRel , dopri5_error_stepper_type() ),
                                       Simulator(&sys), initial, 0.0, tMax, mInitialStep, ObserverPassThrough(sys, obs, logger));
            break;
         case RK_DOPRI_5_DENSE:
            integrate_adaptive(make_dense_output( mEpsAbs , mEpsRel , dopri5_error_stepper_type() ),
                                       Simulator(&sys), initial, 0.0, tMax, mInitialStep, ObserverPassThrough(sys, obs, logger));
            break;
         default:
            // Do nothing
            break;
      }
   }

} // netevo namespace


/* Copyright (c) 2021, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
 * platform" software is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <opencv2/xfeatures2d.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <interest_point.h>           // from the isaac repo
#include <camera_image.h>             // from the isaac repo
#include <interest_point/matching.h>  // from the astrobee repo

// Get rid of warnings beyond our control
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#pragma GCC diagnostic ignored "-Wunused-function"
#pragma GCC diagnostic push
#include <openMVG/multiview/projection.hpp>
#include <openMVG/multiview/rotation_averaging_l1.hpp>
#include <openMVG/multiview/triangulation_nview.hpp>
#include <openMVG/numeric/numeric.h>
#include <openMVG/tracks/tracks.hpp>
#pragma GCC diagnostic pop

#include <boost/filesystem.hpp>

#include <iostream>
#include <fstream>

// SIFT is doing so much better than SURF for haz cam images.
DEFINE_string(refiner_feature_detector, "SIFT", "The feature detector to use. SIFT or SURF.");
DEFINE_int32(sift_nFeatures, 10000, "Number of SIFT features");
DEFINE_int32(sift_nOctaveLayers, 3, "Number of SIFT octave layers");
DEFINE_double(sift_contrastThreshold, 0.02,
              "SIFT contrast threshold");  // decrease for more ip
DEFINE_double(sift_edgeThreshold, 10, "SIFT edge threshold");
DEFINE_double(sift_sigma, 1.6, "SIFT sigma");

namespace dense_map {

void detectFeatures(const cv::Mat& image, bool verbose,
                    // Outputs
                    cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints) {
  bool histogram_equalization = false;

  // If using histogram equalization, need an extra image to store it
  cv::Mat* image_ptr = const_cast<cv::Mat*>(&image);
  cv::Mat hist_image;
  if (histogram_equalization) {
    cv::equalizeHist(image, hist_image);
    image_ptr = &hist_image;
  }

  std::vector<cv::KeyPoint> storage;

  if (FLAGS_refiner_feature_detector == "SIFT") {
    cv::Ptr<cv::xfeatures2d::SIFT> sift =
      cv::xfeatures2d::SIFT::create(FLAGS_sift_nFeatures, FLAGS_sift_nOctaveLayers, FLAGS_sift_contrastThreshold,
                                    FLAGS_sift_edgeThreshold, FLAGS_sift_sigma);
    sift->detect(image, storage);
    sift->compute(image, storage, *descriptors);

  } else if (FLAGS_refiner_feature_detector == "SURF") {
    interest_point::FeatureDetector detector("SURF");
    detector.Detect(*image_ptr, &storage, descriptors);

    // Undo the shift in the detector
    for (cv::KeyPoint& key : storage) {
      key.pt.x += image.cols / 2.0;
      key.pt.y += image.rows / 2.0;
    }

  } else {
    LOG(FATAL) << "Unknown feature detector: " << FLAGS_refiner_feature_detector;
  }

  if (verbose) std::cout << "Features detected " << storage.size() << std::endl;

  // Copy to data structures expected by subsequent code
  keypoints->resize(2, storage.size());
  Eigen::Vector2d output;
  for (size_t j = 0; j < storage.size(); j++) {
    keypoints->col(j) = Eigen::Vector2d(storage[j].pt.x, storage[j].pt.y);
  }
}

// This really likes haz cam first and nav cam second
void matchFeatures(std::mutex* match_mutex, int left_image_index, int right_image_index,
                   cv::Mat const& left_descriptors, cv::Mat const& right_descriptors,
                   Eigen::Matrix2Xd const& left_keypoints,
                   Eigen::Matrix2Xd const& right_keypoints, bool verbose,
                   // output
                   MATCH_PAIR* matches) {
  std::vector<cv::DMatch> cv_matches;
  interest_point::FindMatches(left_descriptors, right_descriptors, &cv_matches);

  std::vector<cv::Point2f> left_vec;
  std::vector<cv::Point2f> right_vec;
  for (size_t j = 0; j < cv_matches.size(); j++) {
    int left_ip_index = cv_matches.at(j).queryIdx;
    int right_ip_index = cv_matches.at(j).trainIdx;

    // Get the keypoints from the good matches
    left_vec.push_back(cv::Point2f(left_keypoints.col(left_ip_index)[0], left_keypoints.col(left_ip_index)[1]));
    right_vec.push_back(cv::Point2f(right_keypoints.col(right_ip_index)[0], right_keypoints.col(right_ip_index)[1]));
  }

  if (left_vec.empty()) return;

  // These may need some tweaking but works reasonably well.
  double ransacReprojThreshold = 20.0;
  cv::Mat inlier_mask;
  int maxIters = 10000;
  double confidence = 0.8;

  // affine2D works better than homography
  // cv::Mat H = cv::findHomography(left_vec, right_vec, cv::RANSAC,
  // ransacReprojThreshold, inlier_mask, maxIters, confidence);
  cv::Mat H = cv::estimateAffine2D(left_vec, right_vec, inlier_mask, cv::RANSAC,
                                   ransacReprojThreshold, maxIters, confidence);

  std::vector<InterestPoint> left_ip, right_ip;
  for (size_t j = 0; j < cv_matches.size(); j++) {
    int left_ip_index = cv_matches.at(j).queryIdx;
    int right_ip_index = cv_matches.at(j).trainIdx;

    if (inlier_mask.at<uchar>(j, 0) == 0) continue;

    cv::Mat left_desc = left_descriptors.row(left_ip_index);
    cv::Mat right_desc = right_descriptors.row(right_ip_index);

    InterestPoint left;
    left.setFromCvKeypoint(left_keypoints.col(left_ip_index), left_desc);

    InterestPoint right;
    right.setFromCvKeypoint(right_keypoints.col(right_ip_index), right_desc);

    left_ip.push_back(left);
    right_ip.push_back(right);
  }

  // Update the shared variable using a lock
  match_mutex->lock();

  // Print the verbose message inside the lock, otherwise the text
  // may get messed up.
  if (verbose)
    std::cout << "Number of matches for pair "
              << left_image_index << ' ' << right_image_index << ": "
              << left_ip.size() << std::endl;

  *matches = std::make_pair(left_ip, right_ip);
  match_mutex->unlock();
}

// Match features while assuming that the input cameras can be used to filter out
// outliers by reprojection error.
void matchFeaturesWithCams(std::mutex* match_mutex, int left_image_index, int right_image_index,
                           camera::CameraParameters const& left_params,
                           camera::CameraParameters const& right_params,
                           Eigen::Affine3d const& left_world_to_cam,
                           Eigen::Affine3d const& right_world_to_cam,
                           double reprojection_error,
                           cv::Mat const& left_descriptors, cv::Mat const& right_descriptors,
                           Eigen::Matrix2Xd const& left_keypoints,
                           Eigen::Matrix2Xd const& right_keypoints,
                           bool verbose,
                           // output
                           MATCH_PAIR* matches) {
  // Match by using descriptors first
  std::vector<cv::DMatch> cv_matches;
  interest_point::FindMatches(left_descriptors, right_descriptors, &cv_matches);

  // Do filtering
  std::vector<cv::Point2f> left_vec;
  std::vector<cv::Point2f> right_vec;
  std::vector<cv::DMatch> filtered_cv_matches;
  for (size_t j = 0; j < cv_matches.size(); j++) {
    int left_ip_index = cv_matches.at(j).queryIdx;
    int right_ip_index = cv_matches.at(j).trainIdx;

    Eigen::Vector2d dist_left_ip(left_keypoints.col(left_ip_index)[0],
                                 left_keypoints.col(left_ip_index)[1]);

    Eigen::Vector2d dist_right_ip(right_keypoints.col(right_ip_index)[0],
                                  right_keypoints.col(right_ip_index)[1]);

    Eigen::Vector2d undist_left_ip;
    Eigen::Vector2d undist_right_ip;
    left_params.Convert<camera::DISTORTED,  camera::UNDISTORTED_C>
      (dist_left_ip, &undist_left_ip);
    right_params.Convert<camera::DISTORTED, camera::UNDISTORTED_C>
      (dist_right_ip, &undist_right_ip);

    Eigen::Vector3d X =
      dense_map::TriangulatePair(left_params.GetFocalLength(), right_params.GetFocalLength(),
                                 left_world_to_cam, right_world_to_cam,
                                 undist_left_ip, undist_right_ip);

    // Project back into the cameras
    Eigen::Vector3d left_cam_X = left_world_to_cam * X;
    Eigen::Vector2d undist_left_pix
      = left_params.GetFocalVector().cwiseProduct(left_cam_X.hnormalized());
    Eigen::Vector2d dist_left_pix;
    left_params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undist_left_pix,
                                                                  &dist_left_pix);

    Eigen::Vector3d right_cam_X = right_world_to_cam * X;
    Eigen::Vector2d undist_right_pix
      = right_params.GetFocalVector().cwiseProduct(right_cam_X.hnormalized());
    Eigen::Vector2d dist_right_pix;
    right_params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undist_right_pix,
                                                                   &dist_right_pix);

    // Filter out points whose reprojection error is too big
    bool is_good = ((dist_left_ip - dist_left_pix).norm() <= reprojection_error &&
                    (dist_right_ip - dist_right_pix).norm() <= reprojection_error);

    // If any values above are Inf or NaN, is_good will be false as well
    if (!is_good) continue;

    // Get the keypoints from the good matches
    left_vec.push_back(cv::Point2f(left_keypoints.col(left_ip_index)[0],
                                   left_keypoints.col(left_ip_index)[1]));
    right_vec.push_back(cv::Point2f(right_keypoints.col(right_ip_index)[0],
                                    right_keypoints.col(right_ip_index)[1]));

    filtered_cv_matches.push_back(cv_matches[j]);
  }

  if (left_vec.empty()) return;

  // Filter using geometry constraints
  // These may need some tweaking but works reasonably well.
  double ransacReprojThreshold = 20.0;
  cv::Mat inlier_mask;
  int maxIters = 10000;
  double confidence = 0.8;

  // affine2D works better than homography
  // cv::Mat H = cv::findHomography(left_vec, right_vec, cv::RANSAC,
  // ransacReprojThreshold, inlier_mask, maxIters, confidence);
  cv::Mat H = cv::estimateAffine2D(left_vec, right_vec, inlier_mask, cv::RANSAC,
                                   ransacReprojThreshold, maxIters, confidence);

  std::vector<InterestPoint> left_ip, right_ip;
  for (size_t j = 0; j < filtered_cv_matches.size(); j++) {
    int left_ip_index = filtered_cv_matches.at(j).queryIdx;
    int right_ip_index = filtered_cv_matches.at(j).trainIdx;

    if (inlier_mask.at<uchar>(j, 0) == 0) continue;

    cv::Mat left_desc = left_descriptors.row(left_ip_index);
    cv::Mat right_desc = right_descriptors.row(right_ip_index);

    InterestPoint left;
    left.setFromCvKeypoint(left_keypoints.col(left_ip_index), left_desc);

    InterestPoint right;
    right.setFromCvKeypoint(right_keypoints.col(right_ip_index), right_desc);

    left_ip.push_back(left);
    right_ip.push_back(right);
  }

  // Update the shared variable using a lock
  match_mutex->lock();

  // Print the verbose message inside the lock, otherwise the text
  // may get messed up.
  if (verbose)
    std::cout << "Number of matches for pair "
              << left_image_index << ' ' << right_image_index << ": "
              << left_ip.size() << std::endl;

  *matches = std::make_pair(left_ip, right_ip);
  match_mutex->unlock();
}

void writeIpRecord(std::ofstream& f, InterestPoint const& p) {
  f.write(reinterpret_cast<const char*>(&(p.x)), sizeof(p.x));
  f.write(reinterpret_cast<const char*>(&(p.y)), sizeof(p.y));
  f.write(reinterpret_cast<const char*>(&(p.ix)), sizeof(p.ix));
  f.write(reinterpret_cast<const char*>(&(p.iy)), sizeof(p.iy));
  f.write(reinterpret_cast<const char*>(&(p.orientation)), sizeof(p.orientation));
  f.write(reinterpret_cast<const char*>(&(p.scale)), sizeof(p.scale));
  f.write(reinterpret_cast<const char*>(&(p.interest)), sizeof(p.interest));
  f.write(reinterpret_cast<const char*>(&(p.polarity)), sizeof(p.polarity));
  f.write(reinterpret_cast<const char*>(&(p.octave)), sizeof(p.octave));
  f.write(reinterpret_cast<const char*>(&(p.scale_lvl)), sizeof(p.scale_lvl));
  uint64_t size = p.size();
  f.write(reinterpret_cast<const char*>((&size)), sizeof(uint64));
  for (size_t i = 0; i < p.descriptor.size(); ++i)
    f.write(reinterpret_cast<const char*>(&(p.descriptor[i])), sizeof(p.descriptor[i]));
}

// Write matches to disk
void writeMatchFile(std::string match_file, std::vector<InterestPoint> const& ip1,
                    std::vector<InterestPoint> const& ip2) {
  std::ofstream f;
  f.open(match_file.c_str(), std::ios::binary | std::ios::out);
  std::vector<InterestPoint>::const_iterator iter1 = ip1.begin();
  std::vector<InterestPoint>::const_iterator iter2 = ip2.begin();
  uint64 size1 = ip1.size();
  uint64 size2 = ip2.size();
  f.write(reinterpret_cast<const char*>(&size1), sizeof(uint64));
  f.write(reinterpret_cast<const char*>(&size2), sizeof(uint64));
  for (; iter1 != ip1.end(); ++iter1) writeIpRecord(f, *iter1);
  for (; iter2 != ip2.end(); ++iter2) writeIpRecord(f, *iter2);
  f.close();
}

void saveImagesAndMatches(std::string const& left_prefix, std::string const& right_prefix,
                          std::pair<int, int> const& index_pair, MATCH_PAIR const& match_pair,
                          std::vector<cv::Mat> const& images) {
  // Add 10000 to have them be listed nicely
  std::ostringstream oss_left;
  oss_left << left_prefix << "_image" << index_pair.first + 10000 << ".jpg";
  std::string left_image_file = oss_left.str();
  std::cout << "Writing: " << left_image_file << std::endl;
  cv::imwrite(left_image_file, images[index_pair.first]);

  std::ostringstream oss_right;
  oss_right << right_prefix << "_image" << index_pair.second + 10000 << ".jpg";
  std::string right_image_file = oss_right.str();
  std::cout << "Writing: " << right_image_file << std::endl;
  cv::imwrite(right_image_file, images[index_pair.second]);

  std::string left_stem = boost::filesystem::path(left_image_file).stem().string();
  std::string right_stem = boost::filesystem::path(right_image_file).stem().string();

  std::string match_file = left_stem + "__" + right_stem + ".match";
  std::cout << "Writing: " << left_image_file << ' ' << right_image_file << ' ' << match_file << std::endl;
  writeMatchFile(match_file, match_pair.first, match_pair.second);
}

// Triangulate rays emanating from given undistorted and centered pixels
Eigen::Vector3d TriangulatePair(double focal_length1, double focal_length2,
                                Eigen::Affine3d const& world_to_cam1,
                                Eigen::Affine3d const& world_to_cam2,
                                Eigen::Vector2d const& pix1,
                                Eigen::Vector2d const& pix2) {
  Eigen::Matrix3d k1;
  k1 << focal_length1, 0, 0, 0, focal_length1, 0, 0, 0, 1;

  Eigen::Matrix3d k2;
  k2 << focal_length2, 0, 0, 0, focal_length2, 0, 0, 0, 1;

  openMVG::Mat34 cid_to_p1, cid_to_p2;
  openMVG::P_From_KRt(k1, world_to_cam1.linear(), world_to_cam1.translation(), &cid_to_p1);
  openMVG::P_From_KRt(k2, world_to_cam2.linear(), world_to_cam2.translation(), &cid_to_p2);

  openMVG::Triangulation tri;
  tri.add(cid_to_p1, pix1);
  tri.add(cid_to_p2, pix2);

  Eigen::Vector3d solution = tri.compute();
  return solution;
}

// Triangulate n rays emanating from given undistorted and centered pixels
Eigen::Vector3d Triangulate(std::vector<double>          const& focal_length_vec,
                            std::vector<Eigen::Affine3d> const& world_to_cam_vec,
                            std::vector<Eigen::Vector2d> const& pix_vec) {
  if (focal_length_vec.size() != world_to_cam_vec.size() ||
      focal_length_vec.size() != pix_vec.size())
    LOG(FATAL) << "All inputs to Triangulate() must have the same size.";

  if (focal_length_vec.size() <= 1)
    LOG(FATAL) << "At least two rays must be passed to Triangulate().";

  openMVG::Triangulation tri;

  for (size_t it = 0; it < focal_length_vec.size(); it++) {
    Eigen::Matrix3d k;
    k << focal_length_vec[it], 0, 0, 0, focal_length_vec[it], 0, 0, 0, 1;

    openMVG::Mat34 cid_to_p;
    openMVG::P_From_KRt(k, world_to_cam_vec[it].linear(), world_to_cam_vec[it].translation(),
                        &cid_to_p);

    tri.add(cid_to_p, pix_vec[it]);
  }

  Eigen::Vector3d solution = tri.compute();
  return solution;
}

void detectMatchFeatures(  // Inputs
                         std::vector<dense_map::cameraImage> const& cams,
                         std::vector<std::string> const& cam_names,
                         std::vector<camera::CameraParameters> const& cam_params,
                         std::vector<Eigen::Affine3d> const& world_to_cam, int num_overlaps,
                         int initial_max_reprojection_error, int num_match_threads,
                         bool verbose,
                         // Outputs
                         std::vector<std::vector<std::pair<float, float>>>& keypoint_vec,
                         std::vector<std::map<int, int>>& pid_to_cid_fid,
                         std::vector<std::string> & image_files) {
  // Wipe the outputs
  keypoint_vec.clear();
  pid_to_cid_fid.clear();
  image_files.clear();

  if (verbose) {
    int count = 10000;
    for (size_t it = 0; it < cams.size(); it++) {
      std::ostringstream oss;
      oss << count << "_" << cam_names[cams[it].camera_type] << ".jpg";
      std::string name = oss.str();
      std::cout << "Writing: " << name << std::endl;
      cv::imwrite(name, cams[it].image);
      count++;
      image_files.push_back(name);
    }
  }

  // Detect features using multiple threads. Too many threads may result
  // in high memory usage.
  std::ostringstream oss;
  oss << num_match_threads;
  std::string num_threads = oss.str();
  google::SetCommandLineOption("num_threads", num_threads.c_str());
  if (!gflags::GetCommandLineOption("num_threads", &num_threads))
    LOG(FATAL) << "Failed to get the value of --num_threads in Astrobee software.\n";
  std::cout << "Using " << num_threads << " threads for feature detection/matching." << std::endl;

  std::cout << "Detecting features." << std::endl;

  std::vector<cv::Mat> cid_to_descriptor_map;
  std::vector<Eigen::Matrix2Xd> cid_to_keypoint_map;
  cid_to_descriptor_map.resize(cams.size());
  cid_to_keypoint_map.resize(cams.size());
  {
    // Make the thread pool go out of scope when not needed to not use up memory
    ff_common::ThreadPool thread_pool;
    for (size_t it = 0; it < cams.size(); it++) {
      thread_pool.AddTask
        (&dense_map::detectFeatures,    // multi-thread  // NOLINT
         // dense_map::detectFeatures(  // single-thread // NOLINT
         cams[it].image, verbose, &cid_to_descriptor_map[it], &cid_to_keypoint_map[it]);
    }
    thread_pool.Join();
  }

  MATCH_MAP matches;

  std::vector<std::pair<int, int> > image_pairs;
  for (size_t it1 = 0; it1 < cams.size(); it1++) {
    for (size_t it2 = it1 + 1; it2 < std::min(cams.size(), it1 + num_overlaps + 1); it2++) {
      image_pairs.push_back(std::make_pair(it1, it2));
    }
  }

  {
    std::cout << "Matching features." << std::endl;
    ff_common::ThreadPool thread_pool;
    std::mutex match_mutex;
    for (size_t pair_it = 0; pair_it < image_pairs.size(); pair_it++) {
      auto pair = image_pairs[pair_it];
      int left_image_it = pair.first, right_image_it = pair.second;
      thread_pool.AddTask
        (&dense_map::matchFeaturesWithCams,   // multi-threaded  // NOLINT
         // dense_map::matchFeaturesWithCams( // single-threaded // NOLINT
         &match_mutex, left_image_it, right_image_it, cam_params[cams[left_image_it].camera_type],
         cam_params[cams[right_image_it].camera_type], world_to_cam[left_image_it],
         world_to_cam[right_image_it], initial_max_reprojection_error,
         cid_to_descriptor_map[left_image_it], cid_to_descriptor_map[right_image_it],
         cid_to_keypoint_map[left_image_it], cid_to_keypoint_map[right_image_it], verbose,
         &matches[pair]);
    }
    thread_pool.Join();
  }
  cid_to_descriptor_map = std::vector<cv::Mat>();  // Wipe, takes memory

  // Give all interest points in a given image a unique id, and put
  // them in a vector with the id corresponding to the interest point
  std::vector<std::map<std::pair<float, float>, int>> keypoint_map(cams.size());
  for (auto it = matches.begin(); it != matches.end(); it++) {
    std::pair<int, int> const& index_pair = it->first;     // alias

    int left_index = index_pair.first;
    int right_index = index_pair.second;

    dense_map::MATCH_PAIR const& match_pair = it->second;  // alias
    std::vector<dense_map::InterestPoint> const& left_ip_vec = match_pair.first;
    std::vector<dense_map::InterestPoint> const& right_ip_vec = match_pair.second;
    for (size_t ip_it = 0; ip_it < left_ip_vec.size(); ip_it++) {
      auto dist_left_ip  = std::make_pair(left_ip_vec[ip_it].x,  left_ip_vec[ip_it].y);
      auto dist_right_ip = std::make_pair(right_ip_vec[ip_it].x, right_ip_vec[ip_it].y);
      // Initialize to zero for the moment
      keypoint_map[left_index][dist_left_ip] = 0;
      keypoint_map[right_index][dist_right_ip] = 0;
    }
  }
  keypoint_vec.resize(cams.size());
  for (size_t cid = 0; cid < cams.size(); cid++) {
    keypoint_vec[cid].resize(keypoint_map[cid].size());
    int fid = 0;
    for (auto ip_it = keypoint_map[cid].begin(); ip_it != keypoint_map[cid].end();
         ip_it++) {
      auto& dist_ip = ip_it->first;  // alias
      keypoint_map[cid][dist_ip] = fid;
      keypoint_vec[cid][fid] = dist_ip;
      fid++;
    }
  }

  // If feature A in image I matches feather B in image J, which
  // matches feature C in image K, then (A, B, C) belong together in
  // a track, and will have a single triangulated xyz. Build such a track.

  openMVG::matching::PairWiseMatches match_map;
  for (auto it = matches.begin(); it != matches.end(); it++) {
    std::pair<int, int> const& index_pair = it->first;     // alias

    int left_index = index_pair.first;
    int right_index = index_pair.second;

    dense_map::MATCH_PAIR const& match_pair = it->second;  // alias
    std::vector<dense_map::InterestPoint> const& left_ip_vec = match_pair.first;
    std::vector<dense_map::InterestPoint> const& right_ip_vec = match_pair.second;

    std::vector<openMVG::matching::IndMatch> mvg_matches;

    for (size_t ip_it = 0; ip_it < left_ip_vec.size(); ip_it++) {
      auto dist_left_ip  = std::make_pair(left_ip_vec[ip_it].x,  left_ip_vec[ip_it].y);
      auto dist_right_ip = std::make_pair(right_ip_vec[ip_it].x, right_ip_vec[ip_it].y);

      int left_id = keypoint_map[left_index][dist_left_ip];
      int right_id = keypoint_map[right_index][dist_right_ip];
      mvg_matches.push_back(openMVG::matching::IndMatch(left_id, right_id));
    }
    match_map[index_pair] = mvg_matches;
  }

  if (verbose) {
    for (auto it = matches.begin(); it != matches.end(); it++) {
      std::pair<int, int> index_pair = it->first;
      dense_map::MATCH_PAIR const& match_pair = it->second;

      int left_index = index_pair.first;
      int right_index = index_pair.second;

      std::string left_image = image_files[left_index];
      std::string right_image = image_files[right_index];

      std::string left_stem = boost::filesystem::path(left_image).stem().string();
      std::string right_stem = boost::filesystem::path(right_image).stem().string();

      std::string match_file = left_stem + "__" + right_stem + ".match";

      std::cout << "Writing: " << left_image << ' ' << right_image << ' '
                << match_file << std::endl;
      dense_map::writeMatchFile(match_file, match_pair.first, match_pair.second);
    }
  }

  // De-allocate data not needed anymore and take up a lot of RAM
  matches.clear(); matches = MATCH_MAP();
  keypoint_map.clear(); keypoint_map.shrink_to_fit();
  cid_to_keypoint_map.clear(); cid_to_keypoint_map.shrink_to_fit();

  {
    // Build tracks
    // De-allocate these as soon as not needed to save memory
    openMVG::tracks::TracksBuilder trackBuilder;
    trackBuilder.Build(match_map);  // Build:  Efficient fusion of correspondences
    trackBuilder.Filter();          // Filter: Remove tracks that have conflict
    // trackBuilder.ExportToStream(std::cout);
    // Export tracks as a map (each entry is a sequence of imageId and featureIndex):
    //  {TrackIndex => {(imageIndex, featureIndex), ... ,(imageIndex, featureIndex)}
    openMVG::tracks::STLMAPTracks map_tracks;
    trackBuilder.ExportToSTL(map_tracks);
    match_map = openMVG::matching::PairWiseMatches();  // wipe this, no longer needed
    trackBuilder = openMVG::tracks::TracksBuilder();   // wipe it

    if (map_tracks.empty())
      LOG(FATAL) << "No tracks left after filtering. Perhaps images are too dis-similar?\n";

    // Populate the filtered tracks
    size_t num_elems = map_tracks.size();
    pid_to_cid_fid.resize(num_elems);
    size_t curr_id = 0;
    for (auto itr = map_tracks.begin(); itr != map_tracks.end(); itr++) {
      for (auto itr2 = (itr->second).begin(); itr2 != (itr->second).end(); itr2++) {
        pid_to_cid_fid[curr_id][itr2->first] = itr2->second;
      }
      curr_id++;
    }
  }

  return;
}

}  // end namespace dense_map


#include "basic.h"

#if defined(__GNUC__)
#pragma GCC diagnostic push
#if __GNUC_PREREQ(9, 0)
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/log/trivial.hpp>

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#endif

#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable : 4554)
#pragma warning(disable : 26450)
#pragma warning(disable : 26451)
#pragma warning(disable : 26454)
#pragma warning(disable : 26495)
#pragma warning(disable : 26812)
#elif defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#include <unsupported/Eigen/CXX11/Tensor>

#if defined(_MSC_VER)
#pragma warning(pop)
#elif defined(__GNUC__)
#pragma GCC diagnostic pop
#endif

#include <OpenSimplexNoise.h>

namespace ba = boost::accumulators;

namespace WorldEngine
{

void AntiAlias(boost::multi_array<float, 2>& mapData, size_t steps)
{
   typedef Eigen::Tensor<float, 2>    Tensor2D;
   typedef Eigen::TensorMap<Tensor2D> Tensor2DMap;

   static const float w = 1.0f / 11.0f;

   Tensor2D kernel(3, 3);
   kernel.setValues({{w, w, w}, {w, w, w}, {w, w, w}});

   const uint32_t width  = static_cast<uint32_t>(mapData.shape()[1]);
   const uint32_t height = static_cast<uint32_t>(mapData.shape()[0]);

   Tensor2DMap map(mapData.data(), width, height);

   Tensor2D mapPart = map * (2.0f / 11.0f);

   auto AntiAliasStep = [&width,
                         &height,
                         &mapPart = std::as_const(mapPart),
                         &kernel  = std::as_const(kernel)](Tensor2DMap& map) {
      // Specify first and second dimension for convolution
      static const Eigen::array<int, 2> dimensions({0, 1});

      // Create a circular boundary
      const Eigen::array<std::pair<int, int>, 2> paddings(
         {std::make_pair(1, 1), std::make_pair(1, 1)});
      Tensor2D paddedMap = map.pad(paddings);

      for (uint32_t x = 0; x <= width + 1; x++)
      {
         paddedMap(x, 0)          = paddedMap(x, height);
         paddedMap(x, height + 1) = paddedMap(x, 1);
      }
      for (uint32_t y = 0; y <= height + 1; y++)
      {
         paddedMap(0, y)         = paddedMap(width, y);
         paddedMap(width + 1, y) = paddedMap(1, y);
      }

      map = paddedMap.convolve(kernel, dimensions);

      map += mapPart;
   };

   for (size_t i = 0; i < steps; i++)
   {
      AntiAliasStep(map);
   }
}

template<typename T>
boost::multi_array<uint32_t, 2>
CountNeighbors(const boost::multi_array<T, 2>& mask, int32_t radius)
{
   const int32_t width  = static_cast<int32_t>(mask.shape()[1]);
   const int32_t height = static_cast<int32_t>(mask.shape()[0]);

   boost::multi_array<uint32_t, 2> neighbors(boost::extents[height][width]);

   std::fill(neighbors.data(), neighbors.data() + neighbors.num_elements(), 0u);

   for (int32_t y = 0; y < height; y++)
   {
      for (int32_t x = 0; x < width; x++)
      {
         for (int32_t ny = y - radius; ny <= y + radius; ny++)
         {
            if (0 <= ny && ny < height)
            {
               for (int32_t nx = x - radius; nx <= x + radius; nx++)
               {
                  if ((nx != x || ny != y) && 0 <= nx && nx < width)
                  {
                     if (mask[ny][nx])
                     {
                        neighbors[y][x]++;
                     }
                  }
               }
            }
         }
      }
   }

   return neighbors;
}
template boost::multi_array<uint32_t, 2>
CountNeighbors<bool>(const boost::multi_array<bool, 2>& mask, int32_t radius);
template boost::multi_array<uint32_t, 2>
CountNeighbors<float>(const boost::multi_array<float, 2>& mask, int32_t radius);

float FindThresholdF(const boost::multi_array<float, 2>& mapData,
                     float                               landPercentage,
                     const OceanArrayType*               ocean)
{
   typedef ba::accumulator_set<float, ba::stats<ba::tag::p_square_quantile>>
      accumulator_t;

   const uint32_t width    = static_cast<uint32_t>(mapData.shape()[1]);
   const uint32_t height   = static_cast<uint32_t>(mapData.shape()[0]);
   float          quantile = 1.0f - landPercentage;

   accumulator_t accumulator(ba::quantile_probability = quantile);

   if (ocean == nullptr || ocean->size() != mapData.size())
   {
      BOOST_LOG_TRIVIAL(trace) << "Calculating threshold (" << landPercentage
                               << ") without ocean data...";

      for (uint32_t y = 0; y < height; y++)
      {
         for (uint32_t x = 0; x < width; x++)
         {
            accumulator(mapData[y][x]);
         }
      }
   }
   else
   {
      BOOST_LOG_TRIVIAL(trace) << "Calculating threshold (" << landPercentage
                               << ") with ocean data...";

      for (uint32_t y = 0; y < height; y++)
      {
         for (uint32_t x = 0; x < width; x++)
         {
            if (!(*ocean)[y][x])
            {
               accumulator(mapData[y][x]);
            }
         }
      }
   }

   float threshold = ba::p_square_quantile(accumulator);

   BOOST_LOG_TRIVIAL(trace) << "Threshold: " << threshold;

   return threshold;
}

double Noise(const OpenSimplexNoise::Noise& noise,
             double                         x,
             double                         y,
             uint32_t                       octaves)
{
   static const double persistence = 0.5;
   static const double lacunarity  = 2.0;

   double freq  = 1.0;
   double amp   = 1.0;
   double max   = 1.0;
   double total = noise.eval(x, y);

   for (uint32_t i = 1; i < octaves; i++)
   {
      freq *= lacunarity;
      amp *= persistence;
      max += amp;
      total += noise.eval(x * freq, y * freq) * amp;
   }

   return total / max;
}

} // namespace WorldEngine


//            Copyright Daniel Trebbien 2010.
// Distributed under the Boost Software License, Version 1.0.
//   (See accompanying file LICENSE_1_0.txt or the copy at
//         http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP
#define BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP 1

#include <cassert>
#include <set>
#include <vector>
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/buffer_concepts.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/typeof/typeof.hpp>

namespace boost {

  namespace detail {

    /**
     * \brief Performs a phase of the Stoer-Wagner min-cut algorithm
     *
     * Performs a phase of the Stoer-Wagner min-cut algorithm.
     *
     * As described by Stoer & Wagner (1997), a phase is simply a maximum adjacency search
     * (also called a maximum cardinality search), which results in the selection of two vertices
     * \em s and \em t, and, as a side product, a minimum s-t cut of
     * the input graph. Here, the input graph is basically \p g, but some vertices are virtually
     * assigned to others as a way of viewing \p g as a graph with some sets of
     * vertices merged together.
     *
     * This implementation is a translation of pseudocode by Professor Uri Zwick,
     * School of Computer Science, Tel Aviv University.
     *
     * \pre \p g is a connected, undirected graph
     * \param[in] g the input graph
     * \param[in] assignments a read/write property map from each vertex to the vertex that it is assigned to
     * \param[in] assignedVertices a list of vertices that are assigned to others
     * \param[in] weights a readable property map from each edge to its weight (a non-negative value)
     * \param[out] pq a keyed, updatable max-priority queue
     * \returns a tuple (\em s, \em t, \em w) of the "s" and "t"
     *     of the minimum s-t cut and the cut weight \em w
     *     of the minimum s-t cut.
     * \see http://www.cs.tau.ac.il/~zwick/grad-algo-08/gmc.pdf
     *
     * \author Daniel Trebbien
     * \date 2010-09-11
     */
    template <class UndirectedGraph, class VertexAssignmentMap, class WeightMap, class KeyedUpdatablePriorityQueue>
    boost::tuple<typename boost::graph_traits<UndirectedGraph>::vertex_descriptor, typename boost::graph_traits<UndirectedGraph>::vertex_descriptor, typename boost::property_traits<WeightMap>::value_type>
    stoer_wagner_phase(const UndirectedGraph& g, VertexAssignmentMap assignments, const std::set<typename boost::graph_traits<UndirectedGraph>::vertex_descriptor>& assignedVertices, WeightMap weights, KeyedUpdatablePriorityQueue& pq) {
      typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;

      assert(pq.empty());
      typename KeyedUpdatablePriorityQueue::key_map keys = pq.keys();

      BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
        if (v == get(assignments, v)) { // foreach u \in V do
          put(keys, v, weight_type(0));

          pq.push(v);
        }
      }

      assert(pq.size() >= 2);

      vertex_descriptor s, t;
      weight_type w;
      while (!pq.empty()) { // while PQ \neq {} do
        const vertex_descriptor u = pq.top(); // u = extractmax(PQ)
        w = get(keys, u);
        pq.pop();

        s = t; t = u;

        BGL_FORALL_OUTEDGES_T(u, e, g, UndirectedGraph) { // foreach (u, v) \in E do
          const vertex_descriptor v = get(assignments, target(e, g));

          if (pq.contains(v)) { // if v \in PQ then
            put(keys, v, get(keys, v) + get(weights, e)); // increasekey(PQ, v, wA(v) + w(u, v))
            pq.update(v);
          }
        }

        typename std::set<vertex_descriptor>::const_iterator assignedVertexIt, assignedVertexEnd = assignedVertices.end();
        for (assignedVertexIt = assignedVertices.begin(); assignedVertexIt != assignedVertexEnd; ++assignedVertexIt) {
          const vertex_descriptor uPrime = *assignedVertexIt;

          if (get(assignments, uPrime) == u) {
            BGL_FORALL_OUTEDGES_T(uPrime, e, g, UndirectedGraph) { // foreach (u, v) \in E do
              const vertex_descriptor v = get(assignments, target(e, g));

              if (pq.contains(v)) { // if v \in PQ then
                put(keys, v, get(keys, v) + get(weights, e)); // increasekey(PQ, v, wA(v) + w(u, v))
                pq.update(v);
              }
            }
          }
        }
      }

      return boost::make_tuple(s, t, w);
    }

    /**
     * \brief Computes a min-cut of the input graph
     *
     * Computes a min-cut of the input graph using the Stoer-Wagner algorithm.
     *
     * \pre \p g is a connected, undirected graph
     * \pre pq.empty()
     * \param[in] g the input graph
     * \param[in] weights a readable property map from each edge to its weight (a non-negative value)
     * \param[out] parities a writable property map from each vertex to a bool type object for
     *     distinguishing the two vertex sets of the min-cut
     * \param[out] assignments a read/write property map from each vertex to a \c vertex_descriptor object. This
     *     map serves as work space, and no particular meaning should be derived from property values
     *     after completion of the algorithm.
     * \param[out] pq a keyed, updatable max-priority queue
     * \returns the cut weight of the min-cut
     * \see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.114.6687&rep=rep1&type=pdf
     * \see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.31.614&rep=rep1&type=pdf
     *
     * \author Daniel Trebbien
     * \date 2010-09-11
     */
    template <class UndirectedGraph, class WeightMap, class ParityMap, class VertexAssignmentMap, class KeyedUpdatablePriorityQueue>
    typename boost::property_traits<WeightMap>::value_type
    stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, ParityMap parities, VertexAssignmentMap assignments, KeyedUpdatablePriorityQueue& pq) {
      BOOST_CONCEPT_ASSERT((boost::IncidenceGraphConcept<UndirectedGraph>));
      BOOST_CONCEPT_ASSERT((boost::VertexListGraphConcept<UndirectedGraph>));
      typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
      typedef typename boost::graph_traits<UndirectedGraph>::vertices_size_type vertices_size_type;
      typedef typename boost::graph_traits<UndirectedGraph>::edge_descriptor edge_descriptor;
      BOOST_CONCEPT_ASSERT((boost::Convertible<typename boost::graph_traits<UndirectedGraph>::directed_category, boost::undirected_tag>));
      BOOST_CONCEPT_ASSERT((boost::ReadablePropertyMapConcept<WeightMap, edge_descriptor>));
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;
      BOOST_CONCEPT_ASSERT((boost::WritablePropertyMapConcept<ParityMap, vertex_descriptor>));
      typedef typename boost::property_traits<ParityMap>::value_type parity_type;
      BOOST_CONCEPT_ASSERT((boost::ReadWritePropertyMapConcept<VertexAssignmentMap, vertex_descriptor>));
      BOOST_CONCEPT_ASSERT((boost::Convertible<vertex_descriptor, typename boost::property_traits<VertexAssignmentMap>::value_type>));
      BOOST_CONCEPT_ASSERT((boost::KeyedUpdatableQueueConcept<KeyedUpdatablePriorityQueue>));

      vertices_size_type n = num_vertices(g);
      if (n < 2)
        throw boost::bad_graph("the input graph must have at least two vertices.");
      else if (!pq.empty())
        throw std::invalid_argument("the max-priority queue must be empty initially.");

      std::set<vertex_descriptor> assignedVertices;

      // initialize `assignments` (all vertices are initially assigned to themselves)
      BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
        put(assignments, v, v);
      }

      vertex_descriptor s, t;
      weight_type bestW;

      boost::tie(s, t, bestW) = boost::detail::stoer_wagner_phase(g, assignments, assignedVertices, weights, pq);
      assert(s != t);
      BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
        put(parities, v, parity_type(v == t ? 1 : 0));
      }
      put(assignments, t, s);
      assignedVertices.insert(t);
      --n;

      for (; n >= 2; --n) {
        weight_type w;
        boost::tie(s, t, w) = boost::detail::stoer_wagner_phase(g, assignments, assignedVertices, weights, pq);
        assert(s != t);

        if (w < bestW) {
          BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
            put(parities, v, parity_type(get(assignments, v) == t ? 1 : 0));

            if (get(assignments, v) == t) // all vertices that were assigned to t are now assigned to s
              put(assignments, v, s);
          }

          bestW = w;
        } else {
          BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
            if (get(assignments, v) == t) // all vertices that were assigned to t are now assigned to s
              put(assignments, v, s);
          }
        }
        put(assignments, t, s);
        assignedVertices.insert(t);
      }

      assert(pq.empty());

      return bestW;
    }

  } // end `namespace detail` within `namespace boost`

  template <class UndirectedGraph, class WeightMap, class P, class T, class R>
  inline typename boost::property_traits<WeightMap>::value_type
  stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, const boost::bgl_named_params
& params) {
    typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
    typedef typename std::vector<vertex_descriptor>::size_type heap_container_size_type;
    typedef typename boost::property_traits<WeightMap>::value_type weight_type;

    typedef boost::bgl_named_params
 params_type;
    BOOST_GRAPH_DECLARE_CONVERTED_PARAMETERS(params_type, params)

    BOOST_AUTO(pq, (boost::detail::make_priority_queue_from_arg_pack_gen<boost::graph::keywords::tag::max_priority_queue, weight_type, vertex_descriptor, std::greater<weight_type> >(choose_param(get_param(params, boost::distance_zero_t()), weight_type(0)))(g, arg_pack)));

    return boost::detail::stoer_wagner_min_cut(g,
        weights,
        choose_param(get_param(params, boost::parity_map_t()), boost::dummy_property_map()),
        boost::detail::make_property_map_from_arg_pack_gen<boost::graph::keywords::tag::vertex_assignment_map, vertex_descriptor>(vertex_descriptor())(g, arg_pack),
        pq
      );
  }

  template <class UndirectedGraph, class WeightMap>
  inline typename boost::property_traits<WeightMap>::value_type
  stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights) {
    return boost::stoer_wagner_min_cut(g, weights, boost::vertex_index_map(get(boost::vertex_index, g)));
  }

} // end `namespace boost`

#include <boost/graph/iteration_macros_undef.hpp>

#endif // !BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP