#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 once

#include <armadillo>

#include "composition.hpp"

/*!
 * \brief The EquationOfStateBase is an abstract class, the base class for
 * different equations of state.
 *
 * The equation of state is used to determine the compressibility factor Z,
 * partial derivatives of Z (
 * \f$Z\f$, \f$\frac{\partial Z}{\partial T}|_p\f$,
 * \f$\frac{\partial Z}{\partial p}|_T\f$, and
 * \f$\frac{\partial Z}{\partial T}|_\rho\f$
 * ), as well as the heat capacity at constant pressure
 * (\f$c_p\f$) and the heat capacity at constant volume (\f$c_v\f$), from the gas
 * composition, pressure and temperature.
 */
class EquationOfStateBase
{
public:
    //! Have to declare virtual destructor to avoid compiler warnings.
    //! Only declared here, to avoid the inline compiler-generated default destructor.
    virtual ~EquationOfStateBase();

    /*!
     * \brief EquationOfStateBase constructor.
     * \param composition Gas composition (fraction).
     */
    explicit EquationOfStateBase(const arma::vec& composition = Composition::defaultComposition);

    /*!
     * \brief Pure virtual function for evaluation the EOS at constant composition.
     *
     * This function is a stencil for a method for calculating the
     * compressibility factor Z, three partial derivatives, and the heat
     * capacity at constant pressure and constant volume.
     * \param pressure Gas pressure [Pa].
     * \param temperature Gas temperature [K].
     * \return arma::vec containing \f$Z\f$, \f$\frac{\partial Z}{\partial T}|_p\f$,
     * \f$\frac{\partial Z}{\partial p}|_T\f$, \f$\frac{\partial Z}{\partial T}|_\rho\f$,
     * \f$c_p\f$ and \f$c_v\f$
     */
    virtual arma::vec evaluate(const double pressure, const double temperature) const = 0;

    /*!
     * \brief Virtual function for evaluating the EOS at a new composition.
     *
     * This function is a stencil for the method that calculates the
     * compressibility factor Z, three partial derivatives, and the heat
     * capacity at constant pressure and constant volume.
     *
     * This function only calls setComposition() before calling
     * EquationOfStateBase::evaluate(),
     * so it can be overriden if more efficient ways of evaluating the equation
     * of state at a new composition is known.
     *
     * \param pressure Gas pressure [Pa].
     * \param temperature Gas temperature [K].
     * \param composition Gas composition fractions [-].
     * \return arma::vec containing \f$Z\f$, \f$\frac{\partial Z}{\partial T}|_p\f$,
     * \f$\frac{\partial Z}{\partial p}|_T\f$, \f$\frac{\partial Z}{\partial T}|_\rho\f$,
     * \f$c_p\f$ and \f$c_v\f$
     */
    virtual arma::vec evaluate(const double pressure, const double temperature, const arma::vec& composition);

    /*!
     * \brief Method for calculating just the compressibility factor Z at a given pressure and temperature.
     * \param pressure Gas pressure [Pa].
     * \param temperature Gas temperature [K].
     * \return Gas compressibility at the given pressure and temperature.
     */
    virtual double calculateCompressibility(const double pressure, const double temperature) const = 0;

    virtual double calculateStandardDensity() const;

    /*!
     * \brief Set a new composition for the equation of state.
     * \param composition The new composition, fractions in order C1, C2, C3, iC4, nC4, iC5, nC5, C6, N2, CO2.
     * \param force If the composition should be changed even if it's within machine precision of the previous composition.
     * \return True if composition was changed, else false.
     */
    virtual bool setComposition(const arma::vec& composition, const bool force = true);

    double getMolarMassOfMixture() const; //!< Get the molar mass of the gas.
    const arma::vec& getComposition() const; //!< Get the current composition stored in the EOS instance.

protected:
    // components               C1,         C2,         C3,         iC4,        nC4,        iC5,        nC5,        C6          N2,         CO2
    //! The molar mass [g/mol] of the different gas components, in order C1, C2, C3, iC4, nC4, iC5, nC5, C6, N2, CO2.
    arma::vec m_molarMass = {   16.04,      30.07,      44.1,       58.12,      58.12,      72.15,      72.15,      86.18,      28.13,      44.01}; // molar weight of components [g/mol]
    //! The composition of the gas as fractions, in order C1, C2, C3, iC4, nC4, iC5, nC5, C6, N2, CO2.
    arma::vec::fixed<10> m_composition = arma::vec(10);

    //! The molar mass of the gas mixture [g/mol]
    double m_molarMassOfMixture;

    //! Cache density for optimization [kg/m3]
    mutable double m_density = 0;
};


/*
 * Copyright (c) 2020 OmniSci, Inc.
 *
 * 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 "DateTimeParser.h"
#include "StringTransform.h"

#include <boost/algorithm/string/predicate.hpp>

#include <algorithm>
#include <array>
#include <cctype>
#include <charconv>
#include <limits>
#include <sstream>
#include <vector>

namespace {

constexpr std::array<int, 12> month_prefixes{{int('j') << 16 | int('a') << 8 | int('n'),
                                              int('f') << 16 | int('e') << 8 | int('b'),
                                              int('m') << 16 | int('a') << 8 | int('r'),
                                              int('a') << 16 | int('p') << 8 | int('r'),
                                              int('m') << 16 | int('a') << 8 | int('y'),
                                              int('j') << 16 | int('u') << 8 | int('n'),
                                              int('j') << 16 | int('u') << 8 | int('l'),
                                              int('a') << 16 | int('u') << 8 | int('g'),
                                              int('s') << 16 | int('e') << 8 | int('p'),
                                              int('o') << 16 | int('c') << 8 | int('t'),
                                              int('n') << 16 | int('o') << 8 | int('v'),
                                              int('d') << 16 | int('e') << 8 | int('c')}};

constexpr std::array<std::string_view, 13> month_suffixes{
    {""
     "uary",
     "ruary",
     "ch",
     "il",
     "",
     "e",
     "y",
     "ust",
     "tember",
     "ober",
     "ember",
     "ember"}};

constexpr unsigned
    pow_10[10]{1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};

// Return y-m-d minus 1970-01-01 in days according to Gregorian calendar.
// Credit: http://howardhinnant.github.io/date_algorithms.html#days_from_civil
int64_t daysFromCivil(int64_t y, unsigned const m, unsigned const d) {
  y -= m <= 2;
  int64_t const era = (y < 0 ? y - 399 : y) / 400;
  unsigned const yoe = static_cast<unsigned>(y - era * 400);             // [0, 399]
  unsigned const doy = (153 * (m + (m <= 2 ? 9 : -3)) + 2) / 5 + d - 1;  // [0, 365]
  unsigned const doe = yoe * 365 + yoe / 4 - yoe / 100 + doy;            // [0, 146096]
  return era * 146097 + static_cast<int64_t>(doe) - 719468;
}

// Order of entries correspond to enum class FormatType { Date, Time, Timezone }.
std::vector<std::vector<std::string_view>> formatViews() {
  return {{{"%Y-%m-%d", "%m/%d/%Y", "%Y/%m/%d", "%d-%b-%y", "%d/%b/%Y"},
           {"%I:%M:%S %p",
            "%H:%M:%S",
            "%I:%M %p",
            "%H:%M",
            "%H%M%S",
            "%I %p",
            "%I . %M . %S %p"},
           {"%z"}}};
}

// Optionally eat month name after first 3 letters.  Assume first 3 letters are correct.
void eatMonth(unsigned const month, std::string_view& str) {
  str.remove_prefix(3);
  std::string_view const& suffix = month_suffixes[month];
  if (boost::algorithm::istarts_with(str, suffix)) {
    str.remove_prefix(suffix.size());
  }
}

void eatSpace(std::string_view& str) {
  while (!str.empty() && isspace(str.front())) {
    str.remove_prefix(1);
  }
}

// Parse str as a number of maxlen and type T.
// Return value and consume from str on success,
// otherwise return std::nullopt and do not change str.
template <typename T>
std::optional<T> fromChars(std::string_view& str,
                           size_t maxlen = std::numeric_limits<size_t>::max()) {
  T retval;
  maxlen = std::min(maxlen, str.size());
  auto const result = std::from_chars(str.data(), str.data() + maxlen, retval);
  if (result.ec == std::errc()) {
    str.remove_prefix(result.ptr - str.data());
    return retval;
  } else {
    return std::nullopt;
  }
}

// Parse str into int64_t or throw exception.
int64_t unixTime(std::string_view const& str) {
  int64_t time{0};
  auto const result = std::from_chars(str.data(), str.data() + str.size(), time);
  if (result.ec == std::errc()) {
    return time;
  } else {
    throw std::runtime_error(cat("Invalid DATE/TIMESTAMP string (", str, ')'));
  }
}
}  // namespace

// Interpret str according to DateTimeParser::FormatType::Time.
// Return number of (s,ms,us,ns) since midnight based on dim in (0,3,6,9) resp.
template <>
int64_t dateTimeParse<kTIME>(std::string_view str, unsigned const dim) {
  if (!str.empty() && str.front() == 'T') {
    str.remove_prefix(1);
  }
  DateTimeParser parser;
  parser.setFormatType(DateTimeParser::FormatType::Time);
  std::optional<int64_t> time = parser.parse(str, dim);
  if (!time) {
    throw std::runtime_error(cat("Invalid TIME string (", str, ')'));
  }
  // Parse optional timezone
  std::string_view timezone = parser.unparsed();
  parser.setFormatType(DateTimeParser::FormatType::Timezone);
  std::optional<int64_t> tz = parser.parse(timezone, dim);
  if (!parser.unparsed().empty()) {
    throw std::runtime_error(cat("Invalid TIME string (", str, ')'));
  }
  return *time + tz.value_or(0);
}

// Interpret str according to DateTimeParser::FormatType::Date and Time.
// Return number of (s,ms,us,ns) since epoch based on dim in (0,3,6,9) resp.
template <>
int64_t dateTimeParse<kTIMESTAMP>(std::string_view str, unsigned const dim) {
  if (!str.empty() && str.front() == 'T') {
    str.remove_prefix(1);
  }
  DateTimeParser parser;
  // Parse date
  parser.setFormatType(DateTimeParser::FormatType::Date);
  std::optional<int64_t> date = parser.parse(str, dim);
  if (!date) {
    return unixTime(str);
  }
  // Parse optional time-of-day
  std::string_view time_of_day = parser.unparsed();
  if (time_of_day.empty()) {
    throw std::runtime_error(cat("TIMESTAMP requires a time-of-day (", str, ')'));
  } else if (time_of_day.front() == 'T' || time_of_day.front() == ':') {
    time_of_day.remove_prefix(1);
  }
  parser.setFormatType(DateTimeParser::FormatType::Time);
  std::optional<int64_t> time = parser.parse(time_of_day, dim);
  // Parse optional timezone
  std::string_view timezone = parser.unparsed();
  parser.setFormatType(DateTimeParser::FormatType::Timezone);
  std::optional<int64_t> tz = parser.parse(timezone, dim);
  return *date + time.value_or(0) + tz.value_or(0);
}

// Interpret str according to DateTimeParser::FormatType::Date.
// Return number of (s,ms,us,ns) since epoch based on dim in (0,3,6,9) resp.
template <>
int64_t dateTimeParse<kDATE>(std::string_view str, unsigned const dim) {
  DateTimeParser parser;
  // Parse date
  parser.setFormatType(DateTimeParser::FormatType::Date);
  std::optional<int64_t> date = parser.parse(str, dim);
  if (!date) {
    return unixTime(str);
  }
  // Parse optional timezone
  std::string_view timezone = parser.unparsed();
  parser.setFormatType(DateTimeParser::FormatType::Timezone);
  std::optional<int64_t> tz = parser.parse(timezone, dim);
  return *date + tz.value_or(0);
}

// Return number of (s,ms,us,ns) since epoch based on dim in (0,3,6,9) resp.
int64_t DateTimeParser::DateTime::getTime(unsigned const dim) const {
  int64_t const days = daysFromCivil(Y, m, d);
  int const seconds =
      static_cast<int>(3600 * H + 60 * M + S) - z +
      (p ? *p && H != 12 ? 12 * 3600 : !*p && H == 12 ? -12 * 3600 : 0 : 0);
  return (24 * 3600 * days + seconds) * pow_10[dim] + n / pow_10[9 - dim];
}

// Return true if successful parse, false otherwise.  Update dt_ and str.
// OK to be destructive to str on failed match.
bool DateTimeParser::parseWithFormat(std::string_view format, std::string_view& str) {
  while (!format.empty()) {
    if (format.front() == '%') {
      eatSpace(str);
      if (!updateDateTimeAndStr(format[1], str)) {
        return false;
      }
      format.remove_prefix(2);
    } else if (isspace(format.front())) {
      eatSpace(format);
      eatSpace(str);
    } else if (!str.empty() && format.front() == str.front()) {
      format.remove_prefix(1);
      str.remove_prefix(1);
    } else {
      return false;
    }
  }
  return true;
}

// Update dt_ based on given str and current value of format_type_.
// Return number of (s,ms,us,ns) since epoch based on dim in (0,3,6,9) resp.
// or std::nullopt if no format matches str.
// In either case, update unparsed_ to the remaining part of str that was not matched.
std::optional<int64_t> DateTimeParser::parse(std::string_view const& str, unsigned dim) {
  static std::vector<std::vector<std::string_view>> const& format_views = formatViews();
  auto const& formats = format_views.at(static_cast<int>(format_type_));
  for (std::string_view const& format : formats) {
    std::string_view str_unparsed = str;
    if (parseWithFormat(format, str_unparsed)) {
      unparsed_ = str_unparsed;
      return dt_.getTime(dim);
    }
  }
  unparsed_ = str;
  return std::nullopt;
}

void DateTimeParser::resetDateTime() {
  dt_ = DateTime();
}

void DateTimeParser::setFormatType(FormatType format_type) {
  resetDateTime();
  format_type_ = format_type;
}

std::string_view DateTimeParser::unparsed() const {
  return unparsed_;
}

// Return true if successful parse, false otherwise.  Update dt_ and str on success.
// OK to be destructive to str on failed parse.
bool DateTimeParser::updateDateTimeAndStr(char const field, std::string_view& str) {
  switch (field) {
    case 'Y':
      if (auto const year = fromChars<int64_t>(str)) {
        dt_.Y = *year;
        return true;
      }
      return false;
    case 'y':
      if (2 <= str.size() && isdigit(str[0]) && isdigit(str[1])) {
        if (auto const year = fromChars<unsigned>(str, 2)) {
          dt_.Y = (*year < 69 ? 2000 : 1900) + *year;
          return true;
        }
      }
      return false;
    case 'm':
      if (auto const month = fromChars<unsigned>(str, 2)) {
        if (1 <= *month && *month <= 12) {
          dt_.m = *month;
          return true;
        }
      }
      return false;
    case 'b':
      if (3 <= str.size()) {
        int const key =
            std::tolower(str[0]) << 16 | std::tolower(str[1]) << 8 | std::tolower(str[2]);
        constexpr auto end = month_prefixes.data() + month_prefixes.size();
        // This is faster than a lookup into a std::unordered_map.
        auto const ptr = std::find(month_prefixes.data(), end, key);
        if (ptr != end) {
          dt_.m = ptr - month_prefixes.data() + 1;
          eatMonth(dt_.m, str);
          return true;
        }
      }
      return false;
    case 'd':
      if (auto const day = fromChars<unsigned>(str, 2)) {
        if (1 <= *day && *day <= 31) {
          dt_.d = *day;
          return true;
        }
      }
      return false;
    case 'H':
      if (auto const hour = fromChars<unsigned>(str, 2)) {
        if (*hour <= 23) {
          dt_.H = *hour;
          return true;
        }
      }
      return false;
    case 'I':
      if (auto const hour = fromChars<unsigned>(str, 2)) {
        if (1 <= *hour && *hour <= 12) {
          dt_.H = *hour;
          return true;
        }
      }
      return false;
    case 'M':
      if (auto const minute = fromChars<unsigned>(str, 2)) {
        if (*minute <= 59) {
          dt_.M = *minute;
          return true;
        }
      }
      return false;
    case 'S':
      if (auto const second = fromChars<unsigned>(str, 2)) {
        if (*second <= 61) {
          dt_.S = *second;
          if (!str.empty() && str.front() == '.') {
            str.remove_prefix(1);
            size_t len = str.size();
            if (auto const ns = fromChars<unsigned>(str, 9)) {
              len -= str.size();
              dt_.n = *ns * pow_10[9 - len];
            } else {
              return false;  // Reject period not followed by a digit
            }
          }
          return true;
        }
      }
      return false;
    case 'z':
      // [-+]\d\d:?\d\d
      if (5 <= str.size() && (str.front() == '-' || str.front() == '+') &&
          isdigit(str[1]) && isdigit(str[2]) && isdigit(str[4]) &&
          (str[3] == ':' ? 6 <= str.size() && isdigit(str[5]) : isdigit(str[3]))) {
        char const* sep = &str[3];
        int hours{0}, minutes{0};
        std::from_chars(str.data() + 1, sep, hours);
        sep += *sep == ':';
        std::from_chars(sep, sep + 2, minutes);
        dt_.z = (str.front() == '-' ? -60 : 60) * (60 * hours + minutes);
        str.remove_prefix(sep - str.data() + 2);
        return true;
      }
      return false;
    case 'p':
      // %p implies optional, so never return false
      if (boost::algorithm::istarts_with(str, "am") ||
          boost::algorithm::istarts_with(str, "pm") ||
          boost::algorithm::istarts_with(str, "a.m.") ||
          boost::algorithm::istarts_with(str, "p.m.")) {
        dt_.p = std::tolower(str.front()) == 'p';
        str.remove_prefix(std::tolower(str[1]) == 'm' ? 2 : 4);
      } else {
        dt_.p.reset();
      }
      return true;
    default:
      throw std::runtime_error(cat("Unrecognized format: %", field));
  }
}

std::ostream& operator<<(std::ostream& out, DateTimeParser::DateTime const& dt) {
  return out << dt.Y << '-' << dt.m << '-' << dt.d << ' ' << dt.H << ':' << dt.M << ':'
             << dt.S << '.' << dt.n << " p("
             << (dt.p ? *dt.p ? "true" : "false" : "unset") << ") z(" << dt.z << ')';
}


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


#include "drake/multibody/parsing/detail_urdf_parser.h"

#include <limits>
#include <map>
#include <memory>
#include <set>
#include <stdexcept>
#include <string>
#include <utility>

#include <Eigen/Dense>
#include <fmt/format.h>
#include <tinyxml2.h>

#include "drake/common/sorted_pair.h"
#include "drake/math/rotation_matrix.h"
#include "drake/multibody/parsing/detail_path_utils.h"
#include "drake/multibody/parsing/detail_tinyxml.h"
#include "drake/multibody/parsing/detail_urdf_geometry.h"
#include "drake/multibody/parsing/package_map.h"
#include "drake/multibody/tree/ball_rpy_joint.h"
#include "drake/multibody/tree/fixed_offset_frame.h"
#include "drake/multibody/tree/planar_joint.h"
#include "drake/multibody/tree/prismatic_joint.h"
#include "drake/multibody/tree/revolute_joint.h"
#include "drake/multibody/tree/universal_joint.h"
#include "drake/multibody/tree/weld_joint.h"

namespace drake {
namespace multibody {
namespace internal {

using Eigen::Matrix3d;
using Eigen::Vector3d;
using Eigen::Vector4d;
using math::RigidTransformd;
using tinyxml2::XMLDocument;
using tinyxml2::XMLElement;

namespace {

const char* kWorldName = "world";

SpatialInertia<double> ExtractSpatialInertiaAboutBoExpressedInB(
    XMLElement* node) {

  RigidTransformd X_BBi;

  XMLElement* origin = node->FirstChildElement("origin");
  if (origin) {
    X_BBi = OriginAttributesToTransform(origin);
  }

  double body_mass = 0;
  XMLElement* mass = node->FirstChildElement("mass");
  if (mass) {
    ParseScalarAttribute(mass, "value", &body_mass);
  }

  double ixx = 0;
  double ixy = 0;
  double ixz = 0;
  double iyy = 0;
  double iyz = 0;
  double izz = 0;

  XMLElement* inertia = node->FirstChildElement("inertia");
  if (inertia) {
    ParseScalarAttribute(inertia, "ixx", &ixx);
    ParseScalarAttribute(inertia, "ixy", &ixy);
    ParseScalarAttribute(inertia, "ixz", &ixz);
    ParseScalarAttribute(inertia, "iyy", &iyy);
    ParseScalarAttribute(inertia, "iyz", &iyz);
    ParseScalarAttribute(inertia, "izz", &izz);
  }

  const RotationalInertia<double> I_BBcm_Bi(ixx, iyy, izz, ixy, ixz, iyz);

  // If this is a massless body, return a zero SpatialInertia.
  if (body_mass == 0. && I_BBcm_Bi.get_moments().isZero() &&
      I_BBcm_Bi.get_products().isZero()) {
    return SpatialInertia<double>(body_mass, {0., 0., 0.}, {0., 0., 0});
  }
  // B and Bi are not necessarily aligned.
  const math::RotationMatrix<double> R_BBi(X_BBi.rotation());

  // Re-express in frame B as needed.
  const RotationalInertia<double> I_BBcm_B = I_BBcm_Bi.ReExpress(R_BBi);

  // Bi's origin is at the COM as documented in
  // http://wiki.ros.org/urdf/XML/link#Elements
  const Vector3d p_BoBcm_B = X_BBi.translation();

  return SpatialInertia<double>::MakeFromCentralInertia(
      body_mass, p_BoBcm_B, I_BBcm_B);
}

void ParseBody(const multibody::PackageMap& package_map,
               const std::string& root_dir,
               ModelInstanceIndex model_instance,
               XMLElement* node,
               MaterialMap* materials,
               MultibodyPlant<double>* plant) {
  std::string drake_ignore;
  if (ParseStringAttribute(node, "drake_ignore", &drake_ignore) &&
      drake_ignore == std::string("true")) {
    return;
  }

  std::string body_name;
  if (!ParseStringAttribute(node, "name", &body_name)) {
    throw std::runtime_error(
        "ERROR: link tag is missing name attribute.");
  }

  if (body_name == kWorldName) {
    return;
  }

  SpatialInertia<double> M_BBo_B;
  XMLElement* inertial_node = node->FirstChildElement("inertial");
  if (!inertial_node) {
    M_BBo_B = SpatialInertia<double>(
        0, Vector3d::Zero(), UnitInertia<double>(0, 0, 0));
  } else {
    M_BBo_B = ExtractSpatialInertiaAboutBoExpressedInB(inertial_node);
  }

  // Add a rigid body to model each link.
  const RigidBody<double>& body =
      plant->AddRigidBody(body_name, model_instance, M_BBo_B);

  if (plant->geometry_source_is_registered()) {
    for (XMLElement* visual_node = node->FirstChildElement("visual");
         visual_node;
         visual_node = visual_node->NextSiblingElement("visual")) {
      geometry::GeometryInstance geometry_instance =
          ParseVisual(body_name, package_map, root_dir, visual_node, materials);
      // The parsing should *always* produce an IllustrationProperties
      // instance, even if it is empty.
      DRAKE_DEMAND(geometry_instance.illustration_properties() != nullptr);
      plant->RegisterVisualGeometry(
          body, geometry_instance.pose(), geometry_instance.shape(),
          geometry_instance.name(),
          *geometry_instance.illustration_properties());
    }

    for (XMLElement* collision_node = node->FirstChildElement("collision");
         collision_node;
         collision_node = collision_node->NextSiblingElement("collision")) {
      geometry::GeometryInstance geometry_instance =
          ParseCollision(body_name, package_map, root_dir, collision_node);
      DRAKE_DEMAND(geometry_instance.proximity_properties());
      plant->RegisterCollisionGeometry(
          body, geometry_instance.pose(), geometry_instance.shape(),
          geometry_instance.name(),
          std::move(*geometry_instance.mutable_proximity_properties()));
    }
  }
}

// Parse the collision filter group tag information into the collision filter
// groups and a set of pairs between which the collisions will be excluded.
// @pre plant.geometry_source_is_registered() is `true`.
void RegisterCollisionFilterGroup(
    ModelInstanceIndex model_instance,
    const MultibodyPlant<double>& plant, XMLElement* node,
    std::map<std::string, geometry::GeometrySet>* collision_filter_groups,
    std::set<SortedPair<std::string>>* collision_filter_pairs) {
  DRAKE_DEMAND(plant.geometry_source_is_registered());
  std::string drake_ignore;
  if (ParseStringAttribute(node, "ignore", &drake_ignore) &&
      drake_ignore == std::string("true")) {
    return;
  }

  std::string group_name;
  if (!ParseStringAttribute(node, "name", &group_name)) {
    throw std::runtime_error("ERROR: group tag is missing name attribute.");
  }

  geometry::GeometrySet collision_filter_geometry_set;
  for (XMLElement* member_node = node->FirstChildElement("drake:member");
       member_node;
       member_node = member_node->NextSiblingElement("drake:member")) {
    const char* body_name = member_node->Attribute("link");
    if (!body_name) {
      throw std::runtime_error(
          fmt::format("'{}':'{}':'{}': Collision filter group '{}' provides a "
                      "member tag without specifying the \"link\" attribute.",
                      __FILE__, __func__, node->GetLineNum(), group_name));
    }
    const auto& body = plant.GetBodyByName(body_name, model_instance);
    collision_filter_geometry_set.Add(
        plant.GetBodyFrameIdOrThrow(body.index()));
  }
  collision_filter_groups->insert({group_name, collision_filter_geometry_set});

  for (XMLElement* ignore_node =
           node->FirstChildElement("drake:ignored_collision_filter_group");
       ignore_node; ignore_node = ignore_node->NextSiblingElement(
                        "drake:ignored_collision_filter_group")) {
    const char* target_name = ignore_node->Attribute("name");
    if (!target_name) {
      throw std::runtime_error(fmt::format(
          "'{}':'{}':'{}': Collision filter group provides a tag specifying a "
          "group to ignore without specifying the \"name\" attribute.",
          __FILE__, __func__, node->GetLineNum()));
    }
    // These two group names are allowed to be identical, which means the bodies
    // inside this collision filter group should be collision excluded among
    // each other.
    collision_filter_pairs->insert({group_name, target_name});
  }
}

// @pre plant->geometry_source_is_registered() is `true`.
void ParseCollisionFilterGroup(ModelInstanceIndex model_instance,
                               XMLElement* node,
                               MultibodyPlant<double>* plant) {
  DRAKE_DEMAND(plant->geometry_source_is_registered());
  std::map<std::string, geometry::GeometrySet> collision_filter_groups;
  std::set<SortedPair<std::string>> collision_filter_pairs;
  for (XMLElement* group_node =
           node->FirstChildElement("drake:collision_filter_group");
       group_node; group_node = group_node->NextSiblingElement(
                       "drake:collision_filter_group")) {
    RegisterCollisionFilterGroup(model_instance, *plant, group_node,
                                 &collision_filter_groups,
                                 &collision_filter_pairs);
  }
  for (const auto& collision_filter_pair : collision_filter_pairs) {
    const auto collision_filter_group_a =
        collision_filter_groups.find(collision_filter_pair.first());
    DRAKE_DEMAND(collision_filter_group_a != collision_filter_groups.end());
    const auto collision_filter_group_b =
        collision_filter_groups.find(collision_filter_pair.second());
    DRAKE_DEMAND(collision_filter_group_b != collision_filter_groups.end());

    plant->ExcludeCollisionGeometriesWithCollisionFilterGroupPair(
        {collision_filter_group_a->first, collision_filter_group_a->second},
        {collision_filter_group_b->first, collision_filter_group_b->second});
  }
}

// Parses a joint URDF specification to obtain the names of the joint, parent
// link, child link, and the joint type. An exception is thrown if any of these
// names cannot be determined.
//
// @param[in] node The XML node parsing the URDF joint description.
// @param[out] name A reference to a string where the name of the joint
// should be saved.
// @param[out] type A reference to a string where the joint type should be
// saved.
// @param[out] parent_link_name A reference to a string where the name of the
// parent link should be saved.
// @param[out] child_link_name A reference to a string where the name of the
// child link should be saved.
void ParseJointKeyParams(XMLElement* node,
                         std::string* name,
                         std::string* type,
                         std::string* parent_link_name,
                         std::string* child_link_name) {
  if (!ParseStringAttribute(node, "name", name)) {
    throw std::runtime_error(
        "ERROR: joint tag is missing name attribute");
  }

  if (!ParseStringAttribute(node, "type", type)) {
    throw std::runtime_error(
        "ERROR: joint " + *name + " is missing type attribute");
  }

  // Obtains the name of the joint's parent link.
  XMLElement* parent_node = node->FirstChildElement("parent");
  if (!parent_node) {
    throw std::runtime_error(
        "ERROR: joint " + *name + " doesn't have a parent node!");
  }
  if (!ParseStringAttribute(parent_node, "link", parent_link_name)) {
    throw std::runtime_error(
        "ERROR: joint " + *name + "'s parent does not have a link attribute!");
  }

  // Obtains the name of the joint's child link.
  XMLElement* child_node = node->FirstChildElement("child");
  if (!child_node) {
    throw std::runtime_error(
        "ERROR: joint " + *name + " doesn't have a child node");
  }
  if (!ParseStringAttribute(child_node, "link", child_link_name)) {
    throw std::runtime_error(
        "ERROR: joint " + *name + "'s child does not have a link attribute!");
  }
}

void ParseJointLimits(XMLElement* node, double* lower, double* upper,
                      double* velocity, double* effort) {
  *lower = -std::numeric_limits<double>::infinity();
  *upper = std::numeric_limits<double>::infinity();
  *velocity = std::numeric_limits<double>::infinity();
  *effort = std::numeric_limits<double>::infinity();

  XMLElement* limit_node = node->FirstChildElement("limit");
  if (limit_node) {
    ParseScalarAttribute(limit_node, "lower", lower);
    ParseScalarAttribute(limit_node, "upper", upper);
    ParseScalarAttribute(limit_node, "velocity", velocity);
    ParseScalarAttribute(limit_node, "effort", effort);
  }
}

void ParseJointDynamics(const std::string& joint_name,
                        XMLElement* node, double* damping) {
  *damping = 0.0;
  double coulomb_friction = 0.0;
  double coulomb_window = std::numeric_limits<double>::epsilon();

  XMLElement* dynamics_node = node->FirstChildElement("dynamics");
  if (dynamics_node) {
    ParseScalarAttribute(dynamics_node, "damping", damping);
    if (ParseScalarAttribute(dynamics_node, "friction", &coulomb_friction) &&
        coulomb_friction != 0.0) {
      drake::log()->warn("Joint {} specifies non-zero friction, which is "
                         "not supported by MultibodyPlant", joint_name);
    }
    if (ParseScalarAttribute(dynamics_node, "coulomb_window",
                             &coulomb_window) &&
        coulomb_window != std::numeric_limits<double>::epsilon()) {
      drake::log()->warn("Joint {} specifies non-zero coulomb_window, which is "
                         "not supported by MultibodyPlant", joint_name);
    }
  }
}

const Body<double>& GetBodyForElement(
    const std::string& element_name,
    const std::string& link_name,
    ModelInstanceIndex model_instance,
    MultibodyPlant<double>* plant) {
  if (link_name == kWorldName) {
    return plant->world_body();
  }

  if (!plant->HasBodyNamed(link_name, model_instance)) {
    throw std::runtime_error(
        "ERROR: Could not find link named\"" +
        link_name + "\" with model instance ID " +
        std::to_string(model_instance) + " for element " + element_name + ".");
  }
  return plant->GetBodyByName(link_name, model_instance);
}

void ParseJoint(ModelInstanceIndex model_instance,
                std::map<std::string, double>* joint_effort_limits,
                XMLElement* node,
                MultibodyPlant<double>* plant) {
  std::string drake_ignore;
  if (ParseStringAttribute(node, "drake_ignore", &drake_ignore) &&
      drake_ignore == std::string("true")) {
    return;
  }

  // Parses the parent and child link names.
  std::string name, type, parent_name, child_name;
  ParseJointKeyParams(node, &name, &type, &parent_name, &child_name);

  const Body<double>& parent_body = GetBodyForElement(
      name, parent_name, model_instance, plant);
  const Body<double>& child_body = GetBodyForElement(
      name, child_name, model_instance, plant);

  RigidTransformd X_PJ;
  XMLElement* origin = node->FirstChildElement("origin");
  if (origin) {
    X_PJ = OriginAttributesToTransform(origin);
  }

  Vector3d axis(1, 0, 0);
  XMLElement* axis_node = node->FirstChildElement("axis");
  if (axis_node && type.compare("fixed") != 0 &&
      type.compare("floating") != 0 && type.compare("ball") != 0) {
    ParseVectorAttribute(axis_node, "xyz", &axis);
    if (axis.norm() < 1e-8) {
      throw std::runtime_error(
          "ERROR: Joint " + name + "axis is zero.  Don't do that.");
    }
    axis.normalize();
  }

  // Joint properties -- these are only used by some joint types.

  // Dynamic properties
  double damping = 0;

  // Limits
  double upper = std::numeric_limits<double>::infinity();
  double lower = -std::numeric_limits<double>::infinity();
  double velocity = std::numeric_limits<double>::infinity();

  // In MultibodyPlant, the effort limit is a property of the actuator, which
  // isn't created until the transmission element is parsed.  Stash a value
  // for all joints when parsing the joint element so that we can look it up
  // later if/when an actuator is created.
  double effort = std::numeric_limits<double>::infinity();

  auto throw_on_custom_joint = [node, name, type](bool want_custom_joint) {
    const std::string node_name(node->Name());
    const bool is_custom_joint = node_name == "drake:joint";
    if (want_custom_joint && !is_custom_joint) {
      throw std::runtime_error(
          "ERROR: Joint " + name + " of type " + type +
          " is a custom joint type, and should be a <drake:joint>");
    } else if (!want_custom_joint && is_custom_joint) {
      throw std::runtime_error(
          "ERROR: Joint " + name + " of type " + type +
          " is a standard joint type, and should be a <joint>");
    }
  };

  if (type.compare("revolute") == 0 || type.compare("continuous") == 0) {
    throw_on_custom_joint(false);
    ParseJointLimits(node, &lower, &upper, &velocity, &effort);
    ParseJointDynamics(name, node, &damping);
    const JointIndex index = plant->AddJoint<RevoluteJoint>(
        name, parent_body, X_PJ,
        child_body, std::nullopt, axis, lower, upper, damping).index();
    Joint<double>& joint = plant->get_mutable_joint(index);
    joint.set_velocity_limits(Vector1d(-velocity), Vector1d(velocity));
  } else if (type.compare("fixed") == 0) {
    throw_on_custom_joint(false);
    plant->AddJoint<WeldJoint>(name, parent_body, X_PJ,
                               child_body, std::nullopt,
                               RigidTransformd::Identity());
  } else if (type.compare("prismatic") == 0) {
    throw_on_custom_joint(false);
    ParseJointLimits(node, &lower, &upper, &velocity, &effort);
    ParseJointDynamics(name, node, &damping);
    const JointIndex index = plant->AddJoint<PrismaticJoint>(
        name, parent_body, X_PJ,
        child_body, std::nullopt, axis, lower, upper, damping).index();
    Joint<double>& joint = plant->get_mutable_joint(index);
    joint.set_velocity_limits(Vector1d(-velocity), Vector1d(velocity));
  } else if (type.compare("floating") == 0) {
    throw_on_custom_joint(false);
    drake::log()->warn("Joint {} specified as type floating which is not "
                       "supported by MultibodyPlant.  Leaving {} as a "
                       "free body.", name, child_name);
  } else if (type.compare("ball") == 0) {
    throw_on_custom_joint(true);
    ParseJointDynamics(name, node, &damping);
    plant->AddJoint<BallRpyJoint>(name, parent_body, X_PJ,
                                  child_body, std::nullopt, damping);
  } else if (type.compare("planar") == 0) {
    throw_on_custom_joint(true);
    Vector3d damping_vec(0, 0, 0);
    XMLElement* dynamics_node = node->FirstChildElement("dynamics");
    if (dynamics_node) {
      ParseVectorAttribute(dynamics_node, "damping", &damping_vec);
    }
    plant->AddJoint<PlanarJoint>(name, parent_body, X_PJ,
                                 child_body, std::nullopt, damping_vec);
  } else if (type.compare("universal") == 0) {
    throw_on_custom_joint(true);
    ParseJointDynamics(name, node, &damping);
    plant->AddJoint<UniversalJoint>(name, parent_body, X_PJ,
                                    child_body, std::nullopt, damping);
  } else {
    throw std::runtime_error(
        "ERROR: Joint " + name + " has unrecognized type: " + type);
  }

  joint_effort_limits->emplace(name, effort);
}

void ParseTransmission(
    ModelInstanceIndex model_instance,
    const std::map<std::string, double>& joint_effort_limits,
    XMLElement* node,
    MultibodyPlant<double>* plant) {
  // Determines the transmission type.
  std::string type;
  XMLElement* type_node = node->FirstChildElement("type");
  if (type_node) {
    type = type_node->GetText();
  } else {
    // Old URDF format, kept for convenience
    if (!ParseStringAttribute(node, "type", &type)) {
      throw std::runtime_error(
          std::string(__FILE__) + ": " + __func__ +
          "ERROR: Transmission element is missing a type.");
    }
  }

  // Checks if the transmission type is not SimpleTransmission. If it is not,
  // print a warning and then abort this method call since only simple
  // transmissions are supported at this time.
  if (type.find("SimpleTransmission") == std::string::npos) {
    drake::log()->warn(
        "Only SimpleTransmissions are supported right now.  This element "
        "will be skipped.");
    return;
  }

  // Determines the actuator's name.
  XMLElement* actuator_node = node->FirstChildElement("actuator");
  if (!actuator_node) {
    throw std::runtime_error(
        "ERROR: Transmission is missing an actuator element.");
  }

  std::string actuator_name;
  if (!ParseStringAttribute(actuator_node, "name", &actuator_name)) {
    throw std::runtime_error(
        "ERROR: Transmission is missing an actuator name.");
  }

  // Determines the name of the joint to which the actuator is attached.
  XMLElement* joint_node = node->FirstChildElement("joint");
  if (!joint_node) {
    throw std::runtime_error(
        "ERROR: Transmission is missing a joint element.");
  }

  std::string joint_name;
  if (!ParseStringAttribute(joint_node, "name", &joint_name)) {
    throw std::runtime_error(
        "ERROR: Transmission is missing a joint name.");
  }

  if (!plant->HasJointNamed(joint_name, model_instance)) {
    throw std::runtime_error(
        "ERROR: Transmission specifies joint " + joint_name +
        " which does not exist.");
  }
  const Joint<double>& joint = plant->GetJointByName(
      joint_name, model_instance);

  // Checks if the actuator is attached to a fixed joint. If so, abort this
  // method call.
  if (joint.num_positions() == 0) {
    drake::log()->warn(
        "WARNING: Skipping transmission since it's attached to "
        "a fixed joint \"" + joint_name + "\".");
    return;
  }

  const auto effort_iter = joint_effort_limits.find(joint_name);
  DRAKE_DEMAND(effort_iter != joint_effort_limits.end());
  if (effort_iter->second < 0) {
    throw std::runtime_error(
        "ERROR: Transmission specifies joint " + joint_name +
        " which has a negative effort limit.");
  }

  if (effort_iter->second <= 0) {
    drake::log()->warn(
        "WARNING: Skipping transmission since it's attached to "
        "joint \"" + joint_name + "\" which has a zero "
        "effort limit {}.", effort_iter->second);
    return;
  }

  const JointActuator<double>& actuator =
      plant->AddJointActuator(actuator_name, joint, effort_iter->second);

  // Parse and add the optional drake:rotor_inertia parameter.
  XMLElement* rotor_inertia_node =
      actuator_node->FirstChildElement("drake:rotor_inertia");
  if (rotor_inertia_node) {
    double rotor_inertia;
    if (!ParseScalarAttribute(rotor_inertia_node, "value", &rotor_inertia)) {
      throw std::runtime_error(
          "ERROR: joint actuator " + actuator_name +
          "'s drake:rotor_inertia does not have a \"value\" attribute!");
    }
    plant->get_mutable_joint_actuator(actuator.index())
        .set_default_rotor_inertia(rotor_inertia);
  }

  // Parse and add the optional drake:gear_ratio parameter.
  XMLElement* gear_ratio_node =
      actuator_node->FirstChildElement("drake:gear_ratio");
  if (gear_ratio_node) {
    double gear_ratio;
    if (!ParseScalarAttribute(gear_ratio_node, "value", &gear_ratio)) {
      throw std::runtime_error(
          "ERROR: joint actuator " + actuator_name +
          "'s drake:gear_ratio does not have a \"value\" attribute!");
    }
    plant->get_mutable_joint_actuator(actuator.index())
        .set_default_gear_ratio(gear_ratio);
  }
}

void ParseFrame(ModelInstanceIndex model_instance,
                XMLElement* node,
                MultibodyPlant<double>* plant) {
  std::string name;
  if (!ParseStringAttribute(node, "name", &name)) {
    throw std::runtime_error("ERROR parsing frame name.");
  }

  std::string body_name;
  if (!ParseStringAttribute(node, "link", &body_name)) {
    throw std::runtime_error(
        "ERROR: missing link name for frame " + name + ".");
  }

  const Body<double>& body =
      GetBodyForElement(name, body_name, model_instance, plant);

  RigidTransformd X_BF = OriginAttributesToTransform(node);
  plant->AddFrame(std::make_unique<FixedOffsetFrame<double>>(
      name, body.body_frame(), X_BF));
}

void ParseBushing(XMLElement* node, MultibodyPlant<double>* plant) {
  // Functor to read a child element with a vector valued `value` attribute
  // Throws an error if unable to find the tag or if the value attribute is
  // improperly formed.
  auto read_vector = [node](const char* element_name) -> Eigen::Vector3d {
    const XMLElement* value_node = node->FirstChildElement(element_name);
    if (value_node != nullptr) {
      Eigen::Vector3d value;
      if (ParseVectorAttribute(value_node, "value", &value)) {
        return value;
      } else {
        throw std::runtime_error(
            fmt::format("Unable to read the 'value' attribute for the <{}> "
                        "tag on line {}",
                        element_name, value_node->GetLineNum()));
      }
    } else {
      throw std::runtime_error(
          fmt::format("Unable to find the <{}> "
                      "tag on line {}",
                      element_name, node->GetLineNum()));
    }
  };

  // Functor to read a child element with a string valued `name` attribute.
  // Throws an error if unable to find the tag of if the name attribute is
  // improperly formed.
  auto read_frame = [node,
                     plant](const char* element_name) -> const Frame<double>& {
    XMLElement* value_node = node->FirstChildElement(element_name);

    if (value_node != nullptr) {
      std::string frame_name;
      if (ParseStringAttribute(value_node, "name", &frame_name)) {
        if (!plant->HasFrameNamed(frame_name)) {
          throw std::runtime_error(fmt::format(
              "Frame: {} specified for <{}> does not exist in the model.",
              frame_name, element_name));
        }
        return plant->GetFrameByName(frame_name);

      } else {
        throw std::runtime_error(
            fmt::format("Unable to read the 'name' attribute for the <{}> "
                        "tag on line {}",
                        element_name, value_node->GetLineNum()));
      }

    } else {
      throw std::runtime_error(
          fmt::format("Unable to find the <{}> tag on line {}", element_name,
                      node->GetLineNum()));
    }
  };

  ParseLinearBushingRollPitchYaw(read_vector, read_frame, plant);
}

ModelInstanceIndex ParseUrdf(
    const std::string& model_name_in,
    const multibody::PackageMap& package_map,
    const std::string& root_dir,
    XMLDocument* xml_doc,
    MultibodyPlant<double>* plant) {

  XMLElement* node = xml_doc->FirstChildElement("robot");
  if (!node) {
    throw std::runtime_error("ERROR: URDF does not contain a robot tag.");
  }

  std::string model_name = model_name_in;
  if (model_name.empty() && !ParseStringAttribute(node, "name", &model_name)) {
      throw std::runtime_error(
          "ERROR: Your robot must have a name attribute or a model name "
          "must be specified.");
  }

  // Parses the model's material elements. Throws an exception if there's a
  // material name clash regardless of whether the associated RGBA values are
  // the same.
  MaterialMap materials;
  for (XMLElement* material_node = node->FirstChildElement("material");
       material_node;
       material_node = material_node->NextSiblingElement("material")) {
    ParseMaterial(material_node, true /* name_required */, package_map,
                  root_dir, &materials);
  }

  const ModelInstanceIndex model_instance =
      plant->AddModelInstance(model_name);

  // Parses the model's link elements.
  for (XMLElement* link_node = node->FirstChildElement("link");
       link_node;
       link_node = link_node->NextSiblingElement("link")) {
    ParseBody(package_map, root_dir, model_instance, link_node,
              &materials, plant);
  }

  // Parses the collision filter groups only if the scene graph is registered.
  if (plant->geometry_source_is_registered()) {
    ParseCollisionFilterGroup(model_instance, node, plant);
  }

  // Joint effort limits are stored with joints, but used when creating the
  // actuator (which is done when parsing the transmission).
  std::map<std::string, double> joint_effort_limits;

  // Parses the model's joint elements.  While it may not be strictly required
  // to be true in MultibodyPlant, generally the JointIndex for any given
  // joint follows the declaration order in the model (and users probably
  // should avoid caring about the ordering of JointIndex), we still parse the
  // joints in model order regardless of the element type so that the results
  // are consistent with an SDF version of the same model.
  for (XMLElement* joint_node = node->FirstChildElement(); joint_node;
       joint_node = joint_node->NextSiblingElement()) {
    const std::string node_name(joint_node->Name());
    if (node_name == "joint" || node_name == "drake:joint") {
      ParseJoint(model_instance, &joint_effort_limits, joint_node, plant);
    }
  }

  // Parses the model's transmission elements.
  for (XMLElement* transmission_node = node->FirstChildElement("transmission");
       transmission_node;
       transmission_node =
           transmission_node->NextSiblingElement("transmission")) {
    ParseTransmission(model_instance, joint_effort_limits,
                      transmission_node, plant);
  }

  if (node->FirstChildElement("loop_joint")) {
    throw std::runtime_error(
        "ERROR: loop joints are not supported in MultibodyPlant");
  }

  // Parses the model's Drake frame elements.
  for (XMLElement* frame_node = node->FirstChildElement("frame"); frame_node;
       frame_node = frame_node->NextSiblingElement("frame")) {
    ParseFrame(model_instance, frame_node, plant);
  }

  // Parses the model's custom Drake bushing tags.
  for (XMLElement* bushing_node =
           node->FirstChildElement("drake:linear_bushing_rpy");
       bushing_node; bushing_node = bushing_node->NextSiblingElement(
                         "drake:linear_bushing_rpy")) {
    ParseBushing(bushing_node, plant);
  }

  return model_instance;
}

}  // namespace

ModelInstanceIndex AddModelFromUrdf(
    const DataSource& data_source,
    const std::string& model_name_in,
    const PackageMap& package_map,
    MultibodyPlant<double>* plant,
    geometry::SceneGraph<double>* scene_graph) {
  DRAKE_THROW_UNLESS(plant != nullptr);
  DRAKE_THROW_UNLESS(!plant->is_finalized());
  data_source.DemandExactlyOne();

  // When the data_source is a filename, we'll use its parent directory to be
  // the root directory to search for files referenced within the URDF file.
  // If data_source is a string, this will remain unset and relative-path
  // resources that would otherwise require a root directory will not be found.
  std::string root_dir;

  // Opens the URDF file and feeds it into the XML parser.
  XMLDocument xml_doc;
  if (data_source.file_name) {
    const std::string full_path = GetFullPath(*data_source.file_name);
    size_t found = full_path.find_last_of("/\\");
    if (found != std::string::npos) {
      root_dir = full_path.substr(0, found);
    } else {
      // TODO(jwnimmer-tri) This is not unit tested.  In any case, we should be
      // using drake::filesystem for path manipulation, not string searching.
      root_dir = ".";
    }
    xml_doc.LoadFile(full_path.c_str());
    if (xml_doc.ErrorID()) {
      throw std::runtime_error(fmt::format(
          "Failed to parse XML file {}:\n{}",
          full_path, xml_doc.ErrorName()));
    }
  } else {
    DRAKE_DEMAND(data_source.file_contents);
    xml_doc.Parse(data_source.file_contents->c_str());
    if (xml_doc.ErrorID()) {
      throw std::runtime_error(fmt::format(
          "Failed to parse XML string: {}",
          xml_doc.ErrorName()));
    }
  }

  if (scene_graph != nullptr && !plant->geometry_source_is_registered()) {
    plant->RegisterAsSourceForSceneGraph(scene_graph);
  }

  return ParseUrdf(model_name_in, package_map, root_dir,
                   &xml_doc, plant);
}

}  // namespace internal
}  // namespace multibody
}  // namespace drake


#include <boost/test/unit_test.hpp>
#include <iostream>
#include <test/util/setup_common.h>
#include <throttle.h>

BOOST_FIXTURE_TEST_SUITE(throttle_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(test_quota_usage)
{
    const double units_per_sec = 10;
    // NOTE: use a low rate of units per second such that the accumulated quota
    // does not vary significantly on slow builds
    const unsigned int wait_ms = 400;
    uint32_t nominal_quota = (units_per_sec * wait_ms / 1000);

    Throttle throttle(units_per_sec);

    // Start with zero quota
    auto quota = throttle.GetQuota();
    BOOST_CHECK(quota == 0);

    // Accumulate quota over time
    std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));

    // Get the accumulated quota
    quota = throttle.GetQuota();
    BOOST_CHECK(quota == nominal_quota);

    // Use the full quota
    BOOST_CHECK(throttle.UseQuota(quota));
    quota = throttle.GetQuota();
    BOOST_CHECK(quota == 0);

    // Accumulate quota once again
    std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));
    quota = throttle.GetQuota();
    BOOST_CHECK(quota == nominal_quota);

    // Use only half of the quota
    BOOST_CHECK(throttle.UseQuota(quota / 2));

    // Check that the other half remains
    quota = throttle.GetQuota();
    BOOST_CHECK(quota == (nominal_quota / 2));
}

BOOST_AUTO_TEST_CASE(test_quota_capping)
{
    const double units_per_sec = 20000;
    const double max_quota = 100;
    const unsigned int wait_ms = 10;

    Throttle throttle(units_per_sec);
    throttle.SetMaxQuota(max_quota);

    std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));
    uint32_t uncapped_quota = (units_per_sec * wait_ms / 1000);
    BOOST_CHECK(!throttle.HasQuota(uncapped_quota));
    BOOST_CHECK(throttle.HasQuota(max_quota));
}

BOOST_AUTO_TEST_CASE(test_quota_wait_estimate)
{
    const double units_per_sec = 100;
    const uint32_t target_wait_ms = 10;
    const uint32_t expected_quota = (units_per_sec * target_wait_ms / 1000);

    Throttle throttle(units_per_sec);

    for (int i = 0; i < 10; i++) {
        // Predict the wait to accumulate the expected quota
        const uint32_t wait_ms = throttle.EstimateWait(expected_quota);
        BOOST_CHECK(wait_ms <= target_wait_ms);
        // The quota accumulates continuously as a fractional number. However,
        // only integer quotas can be consumed. As a result, some fractional
        // quota remains after every quota consumption. Over time, this residual
        // quota accumulates such that the estimated wait on any iteration can
        // become lower than the nominal wait.

        // Wait and accumulate
        std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));

        // Now there should be no need to wait any longer
        BOOST_CHECK(throttle.EstimateWait(expected_quota) == 0);
        BOOST_CHECK(throttle.UseQuota(expected_quota));
    }
}

BOOST_AUTO_TEST_SUITE_END()