#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>

#include "low-match-rate-pub-sub-gen.h"
#include "test-harness/ta3/fields/field-set.h"
#include "test-harness/ta3/fields/integer-field.h"
#include "num-predicate-generators.h"
#include "common/safe-file-stream.h"
#include "common/string-algo.h"
#include "common/statics.h"
#include "common/logging.h"

#include <algorithm>

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

// Helper function used to create inequality subscriptions
string CreateInequality(const FieldBase* field_base,
                        NumGenerator* gt_prob_gen,
                        NumGenerator* inclusive_prob_gen,
                        default_random_engine* generic_gen,
                        string& value_string) {
  if (!(*gt_prob_gen)()) {
    if ((*inclusive_prob_gen)()) {
      uniform_int_distribution<int> un_dist(
          SafeAtoi(value_string),
          ((IntegerField*) field_base)->MaxValue());
      return field_base->name() + " <= " + itoa(un_dist(*generic_gen));
    } else {
      uniform_int_distribution<int> un_dist(
          SafeAtoi(value_string) + 1,
          ((IntegerField*) field_base)->MaxValue());
      return field_base->name() + " < " + itoa(un_dist(*generic_gen));
    }
  } else {
    if ((*inclusive_prob_gen)()) {
      uniform_int_distribution<int> un_dist(
          ((IntegerField*) field_base)->MinValue(),
          SafeAtoi(value_string));
      return field_base->name() + " >= " + itoa(un_dist(*generic_gen));
    } else {
      uniform_int_distribution<int> un_dist(
          ((IntegerField*) field_base)->MinValue(),
          SafeAtoi(value_string) + 1);
      return field_base->name() + " > " + itoa(un_dist(*generic_gen));
    }
  }
}

// Helper function to generate matching publications for each item in a given
// set of subscriptions.
void AppendMatchingPublications(vector<EqualitySubscription>& subs_to_match,
  LowMatchRatePubSubGen* psg, vector<string>* pubs_for_sub,
  size_t num_pubs_per_conj) {
  size_t max_pub_len = psg->MaxPublicationSize();
  char pub[max_pub_len];
  pub[0] = '\0';
  for (size_t l = 0; l < num_pubs_per_conj; l++) {
    while ((*pub) == '\0') {
      psg->GetPublication(subs_to_match.begin(), subs_to_match.end(),
                          max_pub_len, pub);
    }
    pubs_for_sub->push_back(string(pub));
    pub[0] = '\0';
  }
}

// Helper function to expand generated subscriptions into equality and/or
// inequality predicates.
void GeneratePredicates(EqualitySubscription* sub, FieldSet* field_set,
    NumGenerator* ineq_prob_gen, NumGenerator* gt_prob_gen,
    NumGenerator* inclusive_prob_gen, default_random_engine* generic_gen,
    vector<string>* conj_subs) {
  // Obtain all fields currently in conj_subs
  set<string> curr_fields;
  for (auto sub : *conj_subs) {
    curr_fields.insert(Split(sub, ' ')[0]);
  }
  int num_conj = sub->NumPredicates();
  for (int l = 0; l < num_conj; l++) {
    const EqualityCondition* cond = sub->GetCondition(l);
    const FieldBase* field_base = field_set->Get(cond->field());
    // If field is already in conj_subs, don't add this term
    if (curr_fields.find(field_base->name()) != curr_fields.end()) {
      continue;
    }
    curr_fields.insert(field_base->name());
    string value_string = cond->value();
    if (field_base->type() != "Integer" ||
        field_base->name() == "zip" ||
        field_base->name() == "ssn" ||
        field_base->name() == "dob") {
      value_string = "'" + value_string + "'";
    }
    // INEQUALITY TERM
    if (field_base->type() == "Integer" && (*ineq_prob_gen)()) {
      conj_subs->push_back(CreateInequality(field_base,
            gt_prob_gen, inclusive_prob_gen, generic_gen,
            value_string));
    // REGULAR TERM
    } else {
      conj_subs->push_back(field_base->name() + " = " + value_string);
    }
  }
}

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

  po::options_description desc("Options:");
  desc.add_options()
      ("help,h", "Print help message")
      ("field_file,f", po::value<string>(),
       "Path to the file that specifies which metadata fields will be active")
      ("output_dir,o", po::value<string>(),
       "The path to where output files should be placed")
      ("num_clients,c", po::value<size_t>()->default_value(5),
       "Number of clients to create subscriptions for")
      ("subs_per_client", po::value<string>()->default_value("POISSON 2"),
       "Distribution to use for # of subscriptions per client. "
       "Can be a string of the form 'CONSTANT x' or 'POISSON x'.")
      ("conjunction_size", po::value<string>()->default_value("POISSON 2"),
       "Distribution to use for # of predicates per conjunction. "
       "See the requirements for subs_per_client.")
      ("disjunction_prob", po::value<float>()->default_value(0.5),
       "Probability that a subscription will be a disjunction of terms. Note that if "
       "this is greater than 0, the effective match rate will likely be higher "
       "than what match_rate is set to.")
      ("disjunction_size", po::value<string>()->default_value("POISSON 2"),
       "Distribution to use for # of terms per disjunction. "
       "See the requirements for subs_per_client.")
      ("threshold_prob", po::value<float>()->default_value(0.0),
       "Probability that a disjoined term will be an at least "
       "threshold subscription. Only supported by ACS with single equalities.")
      ("num_pubs,p", po::value<size_t>()->default_value(1000),
       "Number of publications to generate.")
      ("inequality_prob", po::value<float>()->default_value(0.0),
       "Probability that a subscription involving a number field will be "
       "an inequality check. Only supported by Argon. Note that if this is "
       "greater than 0, the effective match rate will likely be higher than "
       "what match_rate is set to.")
      ("inclusive_prob", po::value<float>()->default_value(0.5),
       "Probability that an inequality subscription will be based on an "
       "inclusive range (otherwise, will be exclusive). Only supported by Argon.")
      ("gt_prob", po::value<float>()->default_value(0.5),
       "Probability that an inequality subscription will be based on a "
       "greater than comparison (otherwise, will be less than). Only supported "
       "by Argon.")
      ("negation_prob", po::value<float>()->default_value(0.0),
       "Probability that a multi-term conjunction or disjunction is "
       "negated via DeMorgan's Law.")
      ("match_rate", po::value<string>()->default_value("POISSON 2"),
       "Distribution to use for # of conjunctive equalities to match for each "
       "publication.")
      ("client_targeter", po::value<string>()->default_value("UNIFORM"),
       "Distribution to use for targeting clients with subscriptions. "
       "Currently unused.")
      ("rand_seed", po::value<int>()->default_value(0),
       "Random seed to use.")
      ("num_pubs_per_conj", po::value<size_t>()->default_value(0),
       "Number of publications to generate for each conjunctive equality; "
       "used to produce a set of publications that are guaranteed to match "
       "each client.")
      ("replication_factor", po::value<size_t>()->default_value(1),
       "Number of times each conjunctive equality will be used in the "
       "subscription set. Useful for generating high match rates.")
      ("overgeneration_factor", po::value<float>()->default_value(1.5),
       "Multiplier applied to the number of subscriptions to generate."
       "Should be set to 1 if there is no randomness in the generation "
       "parameters; otherwise, this should be greater than 1 to account "
       "for randomness in the cardinality of conjunctions, number of "
       "subscribers per client, etc. Note that if this is greater than 1, "
       "the effective match rate will be lower than what match_rate is set "
       "to.")
      ("keep_exhausted_fields", "If set, will ensure generation will keep "
       "exhausted fields for future generation. Useful for generating data "
       "with high cardinality conjunctions and/or limited field cardinalites.")
      ("verbose,v", "If set, will output all messages at DEBUG level and "
       "above. Otherwise, will output all messages at INFO level and above.");

  po::variables_map vm;

  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
  }
  catch (const boost::bad_any_cast &ex) {
    LOG(FATAL) << ex.what();
  }
  catch (const boost::program_options::invalid_option_value& ex) {
    LOG(FATAL) << ex.what();
  }

  po::notify(vm);

  // Validate input arguments
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    exit(0);
  }
  if (vm.count("verbose") > 0) {
    Log::SetApplicationLogLevel(DEBUG);
  } else {
    Log::SetApplicationLogLevel(INFO);
  }
  if (vm.count("field_file") == 0) {
    LOG(FATAL) << "You must supply the --field_file/-f option";
  }
  if (vm.count("output_dir") == 0) {
    LOG(FATAL) << "You must supply the --output_dir/-o option";
  }

  size_t num_clients = vm["num_clients"].as<size_t>();
  float disjunction_prob = vm["disjunction_prob"].as<float>();
  float threshold_prob = vm["threshold_prob"].as<float>();
  float inequality_prob = vm["inequality_prob"].as<float>();
  float gt_prob = vm["gt_prob"].as<float>();
  float inclusive_prob = vm["inclusive_prob"].as<float>();
  float negation_prob = vm["negation_prob"].as<float>();
  size_t num_pubs = vm["num_pubs"].as<size_t>();
  int rand_seed = vm["rand_seed"].as<int>();
  size_t num_pubs_per_conj = vm["num_pubs_per_conj"].as<size_t>();
  size_t replication_factor = vm["replication_factor"].as<size_t>();
  float overgeneration_factor = vm["overgeneration_factor"].as<float>();

  if (num_clients <= 0 || disjunction_prob < 0 || threshold_prob < 0 ||
      num_pubs <= 0 || num_pubs_per_conj < 0 || inequality_prob < 0) {
    LOG(FATAL) << "Check that you are not passing any negative/zero value "
               << "options that would not make sense.";
  }
  if (replication_factor < 1) {
    LOG(FATAL) << "Replication factor must be at least 1";
  }
  if (overgeneration_factor < 1.0) {
    LOG(FATAL) << "Overgeneration factor must be at least 1.0";
  }

  // Parse schema file and populate fields
  FieldSet field_set;
  SafeIFStream field_file(vm["field_file"].as<string>().c_str());
  field_set.AppendFromFile(&field_file);
  field_set.SetSeed(rand_seed);

  // Instantiate various random generators
  unique_ptr<NumGenerator> subs_per_client_gen;
  unique_ptr<NumGenerator> disj_card_gen;
  unique_ptr<NumGenerator> conj_card_gen;
  unique_ptr<NumGenerator> match_rate_gen;
  unique_ptr<NumGenerator> disj_prob_gen;
  unique_ptr<NumGenerator> thresh_prob_gen;
  unique_ptr<NumGenerator> ineq_prob_gen;
  unique_ptr<NumGenerator> gt_prob_gen;
  unique_ptr<NumGenerator> inclusive_prob_gen;
  unique_ptr<NumGenerator> negation_prob_gen;
  default_random_engine generic_gen(rand_seed);
  size_t avg_subs_per_client = 0;
  size_t avg_disj_card = 0;
  size_t avg_conj_card = 0;
  size_t avg_match_rate = 0;

  vector<string> parts = Split(vm["subs_per_client"].as<string>(), ' ');
  if (parts[0] == "POISSON") {
    avg_subs_per_client = SafeAtoi(parts[1]);
    if (replication_factor > num_clients) {
      LOG(WARNING) << "Replication factor may be such that the number of "
                   << "unique subscriptions is insufficient to prevent the "
                   << "same subscription from appearing on the same client. "
                   << "Consider setting overgeneration_factor larger than 1.";
    }
    subs_per_client_gen.reset(new
        TruncatedPoissonNumGenerator(avg_subs_per_client, rand_seed));
  } else if (parts[0] == "CONSTANT") {
    avg_subs_per_client = SafeAtoi(parts[1]);
    if (overgeneration_factor == 1) {
      CHECK(replication_factor <= num_clients) << "Replication factor must be "
        << "less than or equal to number of clients. Consider setting "
        << "overgeneration_factor larger than 1.";
    }
    subs_per_client_gen.reset(new ConstantNumGenerator(avg_subs_per_client));
  } else {
    LOG(FATAL) << "Did not recognize subs_per_client generator";
  }

  parts = Split(vm["disjunction_size"].as<string>(), ' ');
  if (parts[0] == "POISSON") {
    avg_disj_card = SafeAtoi(parts[1]);
    disj_card_gen.reset(new TruncatedPoissonNumGenerator(avg_disj_card, rand_seed));
  } else if (parts[0] == "CONSTANT") {
    avg_disj_card = SafeAtoi(parts[1]);
    disj_card_gen.reset(new ConstantNumGenerator(avg_disj_card));
  } else {
    LOG(FATAL) << "Did not recognize disjunction_size generator";
  }

  parts = Split(vm["conjunction_size"].as<string>(), ' ');
  if (parts[0] == "POISSON") {
    avg_conj_card = SafeAtoi(parts[1]);
    conj_card_gen.reset(new TruncatedPoissonNumGenerator(avg_conj_card, rand_seed));
  } else if (parts[0] == "CONSTANT") {
    avg_conj_card = SafeAtoi(parts[1]);
    conj_card_gen.reset(new ConstantNumGenerator(avg_conj_card));
  } else {
    LOG(FATAL) << "Did not recognize conjunction_size generator";
  }

  parts = Split(vm["match_rate"].as<string>(), ' ');
  if (parts[0] == "POISSON") {
    avg_match_rate = SafeAtoi(parts[1]);
    if (avg_match_rate <= num_clients*avg_subs_per_client - 1) {
      LOG(WARNING) << "Match rate will likely exceed expected total "
                   << "number of subscriptions, which may make "
                   << "publication generation take longer";
    }
    match_rate_gen.reset(new TruncatedPoissonNumGenerator(avg_match_rate,
          rand_seed));
  } else if (parts[0] == "CONSTANT") {
    avg_match_rate = SafeAtoi(parts[1]);
    CHECK(avg_match_rate <= num_clients*avg_subs_per_client) <<
      "Match rate exceeds expected total number of subscriptions.";
    match_rate_gen.reset(new ConstantNumGenerator(avg_match_rate));
  } else {
    LOG(FATAL) << "Did not recognize match_rate generator";
  }

  disj_prob_gen.reset(
      new CoinTossNumGenerator(1, 0, disjunction_prob, rand_seed));
  thresh_prob_gen.reset(
      new CoinTossNumGenerator(1, 0, threshold_prob, rand_seed));
  ineq_prob_gen.reset(
      new CoinTossNumGenerator(1, 0, inequality_prob, rand_seed));
  gt_prob_gen.reset(
      new CoinTossNumGenerator(1, 0, gt_prob, rand_seed));
  inclusive_prob_gen.reset(
      new CoinTossNumGenerator(1, 0, inclusive_prob, rand_seed));
  negation_prob_gen.reset(
      new CoinTossNumGenerator(1, 0, negation_prob, rand_seed));

  // Determine number of EqualitySubscriptions to create
  size_t num_subs =
    size_t(
      float(num_clients * avg_subs_per_client) * (
        (1 - disjunction_prob) +
        disjunction_prob * float(avg_disj_card) * (
          threshold_prob + 1)) *
      overgeneration_factor / float(replication_factor));

  // Print out summary of parameter selections
  LOG(INFO) << "Test generation parameters:";
  LOG(INFO) << "    Random seed of " << rand_seed;
  LOG(INFO) << "    " << field_set.Size() << " possible fields";
  LOG(INFO) << "    " << num_clients << " clients";
  LOG(INFO) << "    ~" << avg_subs_per_client << " subs per client";
  LOG(INFO) << "    ~" << disjunction_prob*100 << "% of subs will be disjunctions "
                      << "(rest will be conjunctions)";
  LOG(INFO) << "    ~" << threshold_prob*100 << "% of disjoined terms will be thresholds";
  LOG(INFO) << "    ~" << inequality_prob*100 << "% of terms on number fields "
            << "will be inequalities";
  LOG(INFO) << "    ~" << gt_prob*100 << "% of inequalities on number fields "
            << "will be 'greater than'";
  LOG(INFO) << "    ~" << inclusive_prob*100 << "% of inequalities on number fields "
            << "will be inclusive";
  LOG(INFO) << "    ~" << negation_prob*100 << "% of multi-term top-level "
            << "conjunctions or disjunctions will be negated via DeMorgan's";
  LOG(INFO) << "    ~" << avg_conj_card << " terms in each conjunction";
  LOG(INFO) << "    ~" << avg_disj_card << " terms in each disjunction";
  LOG(INFO) << "    " << num_pubs << " publications";
  LOG(INFO) << "    ~" << avg_match_rate << " conjunctions matched "
            << "per publication";
  LOG(INFO) << "    Using replication factor of " << replication_factor;
  LOG(INFO) << "    Using overgeneration factor of " << overgeneration_factor;
  if (vm.count("keep_exhausted_fields") > 0) {
    LOG(INFO) << "    Recycling exhausted fields";
  }
  LOG(INFO) << "Creating " << num_subs << " conjunctive subscriptions. "
            << "These will be used as the basis for more complex "
            << "subscriptions (e.g., disjunctions of conjunctions, "
            << "at least thresholds, etc.)";

  // Instantiate subscription generator and generate all conjunctive
  // subscriptions
  LowMatchRatePubSubGen psg(&field_set, rand_seed, vm.count("keep_exhausted_fields") > 0);
  psg.GenerateSubscriptions(num_subs, std::ref(*conj_card_gen));

  // Print out more info about the generated subscriptions for debug purposes
  const vector<vector<EqualitySubscription>>* all_subsets = psg.GetAllSubsets();
  LOG(DEBUG) << "Generated " << all_subsets->size() << " subsets";
  for (size_t i = 0; i < all_subsets->size(); i++) {
    LOG(DEBUG) << "Subset " << i << " has " <<  (all_subsets->at(i)).size()
               << " subscriptions";
  }
  LowMatchRatePubSubGen::SubsetIterator subset_start, subset_end;
  for (size_t i = 1; i <= field_set.Size(); i++) {
    psg.GetSubsetsGE(i, &subset_start, &subset_end);
    LOG(DEBUG) << (subset_end - subset_start) << " sets with >= " << i <<
      " subscriptions";
  }
  LOG(DEBUG) << "Printing out all generated conjunctive subscriptions "
             << "([subset number][subscription number])...";
  vector<EqualitySubscription>* all_subs = new vector<EqualitySubscription>();
  for (size_t i = 0; i < all_subsets->size(); i++) {
    for (size_t j = 0; j < (all_subsets->at(i)).size(); j++) {
      EqualitySubscription sub = (all_subsets->at(i)).at(j);
      all_subs->push_back(sub);
      LOG(DEBUG) << "[" << i << "][" << j << "] " << sub.ToString();
    }
  }

  // Shuffle subscriptions randomly distribute compatible subsets to different
  // clients
  random_shuffle(all_subs->begin(), all_subs->end(),
      [&] (int i) { std::uniform_int_distribution<int> distribution(0,i-1);
                    return distribution(generic_gen);});

  // Replicate subscriptions as needed
  size_t num_orig_subs = all_subs->size();
  for (size_t i = 0; i < replication_factor - 1; i++) {
    for (size_t j = 0; j < num_orig_subs; j++) {
      all_subs->push_back(all_subs->at(j));
    }
  }

  // Generate more complex subscriptions as needed and map them to clients
  num_subs = num_subs*replication_factor;
  int rem_subs = num_subs;
  map<size_t, vector<string>> all_sql_subs;
  map<size_t, vector<vector<string>>> all_pub_per_sub;
  for (size_t i = 0; i < num_clients; i++) {
    // Determine number of subscriptions for this client
    int num_client_subs = (*subs_per_client_gen)();
    LOG(DEBUG) << "Client #" << i << " has " << num_client_subs <<
      " subscriptions";
    vector<string> sql_subs;
    // Generate each subscription for this client
    for (int j = 0; j < num_client_subs; j++) {
      vector<string> pubs_for_sub;
      // Determine if this subscription will be a disjunction of terms
      int value = (*disj_prob_gen)();

      // DISJUNCTION OF CONJUNCTIONS
      if (value) {
        // Determine number of disjoined conjunctions
        CHECK(rem_subs > 0) << "Ran out of generated conjunctive equalities. "
          << "Consider setting the overgeneration factor to something higher.";
        int num_disj = std::min((*disj_card_gen)(), rem_subs);
        LOG(DEBUG) << "Generating disjunction with " << num_disj << " terms";
        vector<string> disj_subs;
        // Build disjunction out of individual conjunctions
        for (int k = 0; k < num_disj; k++) {
          CHECK(all_subs->size() > num_subs-rem_subs) << "Ran out of "
            << "generated conjunctive equalities. Consider setting the "
            << "overgeneration factor to something higher.";
          EqualitySubscription sub = all_subs->at(num_subs-rem_subs);

          // Generate matching publications for this conjunction
          vector<EqualitySubscription> subs_to_match;
          subs_to_match.push_back(sub);
          AppendMatchingPublications(subs_to_match, &psg, &pubs_for_sub,
              num_pubs_per_conj);

          // Generate each predicate for this conjunction
          vector<string> conj_subs;
          GeneratePredicates(&sub, &field_set, ineq_prob_gen.get(),
              gt_prob_gen.get(), inclusive_prob_gen.get(), &generic_gen,
              &conj_subs);
          int num_conj = sub.NumPredicates();
          LOG(DEBUG) << "Processing conjunction with " << num_conj << " terms";

          // Build SQL string for this conjunction
          if (num_conj == 0) {
            LOG(FATAL) << "This should never happen!";

          // DISJUNCTIVE TERM BASED ON SINGLE-CONJUNCTION PREDICATE
          } else if (num_conj == 1) {

            // AT LEAST THRESHOLD DISJUNCTIVE TERM
            if ((*thresh_prob_gen)()) {
              LOG(DEBUG) << "Creating a threshold subscription with one "
                         << "positive term";
              string thresh_sql;
              vector<string> thresh_preds;
              rem_subs -= 1;
              CHECK(all_subs->size() > num_subs-rem_subs) << "Ran out of "
                << "generated conjunctive equalities. Consider setting the "
                << "overgeneration factor to something higher.";
              EqualitySubscription thresh_sub = all_subs->at(num_subs-rem_subs);
              thresh_sql += "M_OF_N(1,";
              int num_thresh_conj = thresh_sub.NumPredicates();
              LOG(DEBUG) << "Processing conjunction with " << num_thresh_conj << " terms";
              // Add matching predicates of threshold subscription
              thresh_preds.push_back(conj_subs[0]);
              // Determine non-matching predicates of threshold subscription
              GeneratePredicates(&thresh_sub, &field_set, ineq_prob_gen.get(),
                  gt_prob_gen.get(), inclusive_prob_gen.get(), &generic_gen,
                  &thresh_preds);
              thresh_sql += itoa(thresh_preds.size()) + ",";
              // Shuffle threshold predicates
              random_shuffle(thresh_preds.begin(), thresh_preds.end(),
                [&] (int i) { uniform_int_distribution<int> distribution(0,i-1);
                    return distribution(generic_gen);});
              // Complete SQL string
              thresh_sql += Join(thresh_preds, string(","));
              thresh_sql += ")";
              disj_subs.push_back(thresh_sql);

            // SIMPLE DISJUNCTIVE TERM
            } else {
              disj_subs.push_back(conj_subs[0]);
            }

          // DISJUNCTIVE TERM BASED ON MULTI-CONJUNCTION PREDICATE
          } else {

            // AT LEAST THRESHOLD DISJUNCTIVE TERM
            if ((*thresh_prob_gen)()) {
              LOG(DEBUG) << "Creating a threshold subscription with "
                         << num_conj << " positive terms";
              string thresh_sql;
              vector<string> thresh_preds;
              rem_subs -= 1;
              CHECK(all_subs->size() > num_subs-rem_subs) << "Ran out of "
                << "generated conjunctive equalities. Consider setting the "
                << "overgeneration factor to something higher.";
              LOG(DEBUG) << "Using index " << num_subs-rem_subs;
              EqualitySubscription thresh_sub = all_subs->at(num_subs-rem_subs);
              LOG(DEBUG) << "Read out sub: " << thresh_sub.ToString();
              thresh_sql += "M_OF_N(" + itoa(num_conj) + ",";
              int num_thresh_conj = thresh_sub.NumPredicates();
              LOG(DEBUG) << "Processing conjunction with " << num_thresh_conj << " terms";
              // Add matching predicates of threshold subscription
              for (int l = 0; l < num_conj; l++) {
                thresh_preds.push_back(conj_subs[l]);
              }
              // Determine non-matching predicates of threshold subscription
              GeneratePredicates(&thresh_sub, &field_set, ineq_prob_gen.get(),
                  gt_prob_gen.get(), inclusive_prob_gen.get(), &generic_gen,
                  &thresh_preds);
              thresh_sql += itoa(thresh_preds.size()) + ",";
              // Shuffle threshold predicates
              random_shuffle(thresh_preds.begin(), thresh_preds.end(),
                [&] (int i) { uniform_int_distribution<int> distribution(0,i-1);
                    return distribution(generic_gen);});
              // Complete SQL string
              thresh_sql += Join(thresh_preds, string(","));
              thresh_sql += ")";
              disj_subs.push_back(thresh_sql);

            // SIMPLE DISJUNCTIVE TERM
            } else {
              string conj_sql;
              int value = (*negation_prob_gen)();
              // Group each conjunction by parentheses
              for (int k = 0; k < num_conj; k++) {
                if (value) {
                  conj_sql += "(NOT " + conj_subs[k] + ")";
                } else {
                  conj_sql += "(" + conj_subs[k] + ")";
                }
                if (k < num_conj - 1) {
                  if (value) {
                    conj_sql += " OR ";
                  } else {
                    conj_sql += " AND ";
                  }
                }
              }
              if (value) {
                conj_sql = "NOT (" + conj_sql + ")";
              }
              disj_subs.push_back(conj_sql);
            }
          }

          // Update index to use for next subscription retrieval
          rem_subs -= 1;
        }

        // Build SQL-esque string for disjunction
        if (num_disj == 0) {
          LOG(FATAL) << "This should never happen!";

        // SINGLE TERM DISJUNCTION
        } else if (num_disj == 1) {
          sql_subs.push_back(disj_subs[0]);

        // MULTIPLE TERM DISJUNCTION
        } else {
          string disj_sql;
          int value = (*negation_prob_gen)();
          // Group each conjunction by parentheses
          for (int k = 0; k < num_disj; k++) {
            if (value) {
              disj_sql += "(NOT " + disj_subs[k] + ")";
            } else {
              disj_sql += "(" + disj_subs[k] + ")";
            }
            if (k < num_disj - 1) {
              if (value) {
                disj_sql += " AND ";
              } else {
                disj_sql += " OR ";
              }
            }
          }
          if (value) {
            disj_sql = "NOT (" + disj_sql + ")";
          }
          sql_subs.push_back(disj_sql);
        }

      // CONJUNCTION-ONLY SUBSCRIPTION
      } else {
        LOG(DEBUG) << "Using index " << num_subs-rem_subs;
        CHECK(all_subs->size() > num_subs-rem_subs) << "Ran out of "
          << "generated conjunctive equalities. Consider setting the "
          << "overgeneration factor to something higher.";
        EqualitySubscription sub = all_subs->at(num_subs-rem_subs);
        LOG(DEBUG) << "Read out sub: " << sub.ToString();

        // Generate matching publications for this conjunction
        vector<EqualitySubscription> subs_to_match;
        subs_to_match.push_back(sub);
        AppendMatchingPublications(subs_to_match, &psg, &pubs_for_sub,
            num_pubs_per_conj);

          // Generate each predicate for this conjunction
        vector<string> conj_subs;
        GeneratePredicates(&sub, &field_set, ineq_prob_gen.get(),
            gt_prob_gen.get(), inclusive_prob_gen.get(), &generic_gen,
            &conj_subs);
        int num_conj = sub.NumPredicates();

        // Build SQL-esque string for this conjunction
        if (num_conj == 0) {
          LOG(FATAL) << "This should never happen!";

        // SINGLE-TERM CONJUNCTION
        } else if (num_conj == 1) {
          sql_subs.push_back(conj_subs[0]);

        // MULTI-TERM CONJUNCTION
        } else {
          string conj_sql;
          int value = (*negation_prob_gen)();
          // Group each conjunction by parentheses
          for (int k = 0; k < num_conj; k++) {
            if (value) {
              conj_sql += "(NOT " + conj_subs[k] + ")";
            } else {
              conj_sql += "(" + conj_subs[k] + ")";
            }
            if (k < num_conj - 1) {
              if (value) {
                conj_sql += " OR ";
              } else {
                conj_sql += " AND ";
              }
            }
          }
          if (value) {
            conj_sql = "NOT (" + conj_sql + ")";
          }
          sql_subs.push_back(conj_sql);
        }

        // Update index to use for next subscription retrieval
        rem_subs -= 1;
      }

      // Update publications for the conjunctive term that this subscription is
      // based on
      all_pub_per_sub[i].push_back(pubs_for_sub);
    }

    // Update vector of all subscriptions
    all_sql_subs[i] = sql_subs;
  }

  LOG(INFO) << "Wasted " << rem_subs << " equality conjunctions";

  // Print pubs per sub to output file
  if (num_pubs_per_conj > 0) {
    SafeOFStream pub_per_sub_file;
    string pub_per_sub_path = vm["output_dir"].as<string>() +
      "all-pubs-for-each-sub";
    pub_per_sub_file.open(pub_per_sub_path.c_str());
    for (auto pub_per_sub_map : all_pub_per_sub) {
      for (size_t i = 0; i < pub_per_sub_map.second.size(); i++) {
        for (size_t j = 0; j < pub_per_sub_map.second[i].size(); j++) {
        pub_per_sub_file << pub_per_sub_map.first << " "
                         << i << " " << pub_per_sub_map.second[i][j] << endl;
        }
      }
    }
  }

  // Print subscription mapping to output file
  SafeOFStream sub_output_file;
  string sub_output_path = vm["output_dir"].as<string>() + "all-subscriptions";
  sub_output_file.open(sub_output_path.c_str());
  for (auto sub_map : all_sql_subs) {
    for (size_t i = 0; i < sub_map.second.size(); i++) {
      sub_output_file << sub_map.first << " " << i << " " << sub_map.second[i] << endl;
    }
  }
  sub_output_file.close();

  // Generate publications for all conjunctive subscriptions
  LOG(INFO) << "Generating publications. If this hangs for a long time, it's "
            << "possible the field set size, number of requested "
            << "subscriptions, and cardinality of conjunctions is such that "
            << "it is impossible to achieve the desired match rate. Consider "
            << "lowering the match rate, setting the keep_exhausted_fields "
            << "flag, expanding the field set, or lowering the number of "
            << "requested subscriptions and/or cardinality of conjunctions.";
  SafeOFStream pub_output_file;
  string pub_output_path = vm["output_dir"].as<string>() + "all-publications";
  pub_output_file.open(pub_output_path.c_str());
  SafeOFStream pub_match_file;
  string pub_match_path = vm["output_dir"].as<string>() + "all-pub-matches";
  pub_match_file.open(pub_match_path.c_str());
  // TODO how to do better targeting of clients?
  size_t max_pub_len = psg.MaxPublicationSize();
  char pub[max_pub_len];
  pub[0] = '\0';
  vector<EqualitySubscription> matching_subs;
  for (size_t i = 0; i < num_pubs; i++) {
    // Loop until a publication is found that can match the desired number of
    // matches
    while ((*pub) == '\0') {
      int num_matches = (*match_rate_gen)();
      matching_subs = psg.GetPublication(num_matches, max_pub_len, pub);
    }

    // Print out which conjunctive expressions this publication will match
    // TODO easier way to map this to actual subscriptions, and not just the
    // conjunctive expression they were based on?
    for (auto match_sub : matching_subs) {
      vector<string> conj_subs;
      GeneratePredicates(&match_sub, &field_set, ineq_prob_gen.get(),
          gt_prob_gen.get(), inclusive_prob_gen.get(), &generic_gen,
          &conj_subs);
      int num_conj = match_sub.NumPredicates();

      // Build SQL-esque string for this conjunction
      if (num_conj == 0) {
      } else if (num_conj == 1) {
        pub_match_file << i << " " << conj_subs[0] << endl;
      } else {
        string conj_sql;
        for (int k = 0; k < num_conj; k++) {
          // Group each conjunction by parentheses
          conj_sql += "(" + conj_subs[k] + ")";
          if (k < num_conj - 1) {
            conj_sql += " AND ";
          }
        }
        pub_match_file << i << " " << conj_sql << endl;
      }
    }

    // Print out publication to file
    pub_output_file << "METADATA\n" << pub << endl;
    pub[0] = '\0';
  }

  // Wrap up
  pub_output_file.close();
  pub_match_file.close();
  LOG(INFO) << "Data generation complete.";
}


/**
 * @file ceres/EpipolarFactor.hpp
 * @brief Header file for the EpipolarFactor class.
 * @author Jianzhu Huai
 */

#ifndef INCLUDE_OKVIS_CERES_EPIPOLAR_FACTOR_HPP_
#define INCLUDE_OKVIS_CERES_EPIPOLAR_FACTOR_HPP_

#include <vector>
#include <memory>

#include <Eigen/Core>
#include <Eigen/StdVector>

#include <ceres/ceres.h>
#include <okvis/Measurements.hpp>

#include <okvis/assert_macros.hpp>
#include <okvis/ceres/ErrorInterface.hpp>
#include <okvis/ceres/PoseLocalParameterization.hpp>
#include <okvis/ceres/ReprojectionErrorBase.hpp>

namespace okvis {
namespace ceres {
class EpipolarFactorBase : public ReprojectionErrorBase {
public:
  static const int kModelId = 1;
  /// \brief Number of residuals (2)
  static const int kNumResiduals = 1;
};

/**
 * \brief The 1D epipolar error.
 * \tparam GEOMETRY_TYPE The camera gemetry type.
 * The constant params are passed into the residual error through the constructor interface.
 * The variable params are reflected in terms of dim in the SizedCostFunction base class.
 * The Jacobians are computed according to these dims except for the reparameterized pose.
 * The variable params will be passed to the evaluate function as scalar
 * pointers so they can be stored as vector<scalar> or Eigen::Vector.
 */
template <class GEOMETRY_TYPE, class EXTRINSIC_MODEL,
          class PROJ_INTRINSIC_MODEL>
class EpipolarFactor
    : public ::ceres::SizedCostFunction<
          1 /* residuals */, 7 /* left pose */, 7 /* right pose */,
          7 /* extrinsics */,
          PROJ_INTRINSIC_MODEL::kNumParams /* projecction intrinsics */,
          GEOMETRY_TYPE::distortion_t::NumDistortionIntrinsics,
          1 /* readout time */,
          1 /* camera time delay */>,
      public EpipolarFactorBase /* use this base to simplify handling visual
                                      constraints in marginalization. */
{
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)

  /// \brief Make the camera geometry type accessible.
  typedef GEOMETRY_TYPE camera_geometry_t;

  static const int kDistortionDim = GEOMETRY_TYPE::distortion_t::NumDistortionIntrinsics;

  /// \brief The base class type.
  typedef ::ceres::SizedCostFunction<
      1, 7, 7, 7, PROJ_INTRINSIC_MODEL::kNumParams,
      kDistortionDim, 1, 1>
      base_t;

  /// \brief The keypoint type (measurement type).
  typedef Eigen::Vector2d keypoint_t;

  /// \brief Measurement type (1D).
  typedef double measurement_t;

  /// \brief Covariance / information matrix type.
  typedef double covariance_t;

  /// \brief Default constructor.
  EpipolarFactor();

  /**
   * @brief EpipolarFactor Construct with measurement and information matrix
   * @param cameraGeometry
   * @warning The camera geometry will be modified in evaluating Jacobians.
   * @param measurement12 left and right 2d measurements
   * @param covariance12 left and right 2d covariance for 2d meas
   * @param imuMeasCanopy imu measurements in neighborhoods of the left and
   *     right stateEpochs
   * @param stateEpoch left and right state timestamps
   * @param imageTimes left and right image timestamps
   * @param gravityMag magnitude of gravity
   */
  EpipolarFactor(
      std::shared_ptr<camera_geometry_t> cameraGeometry,
      uint64_t landmarkId,
      const std::vector<Eigen::Vector2d,
                        Eigen::aligned_allocator<Eigen::Vector2d>>&
          measurement12,
      const std::vector<Eigen::Matrix2d,
                        Eigen::aligned_allocator<Eigen::Matrix2d>>&
          covariance12,
      std::vector<std::shared_ptr<const okvis::ImuMeasurementDeque>>&
          imuMeasCanopy,
      const std::vector<okvis::Time>& stateEpoch,
      const std::vector<okvis::Time>& imageTimes,
      const std::vector<Eigen::Matrix<double, 9, 1>,
                        Eigen::aligned_allocator<Eigen::Matrix<double, 9, 1>>>&
          speedAndBiases,
      double gravityMag);

  /// \brief Trivial destructor.
  virtual ~EpipolarFactor()
  {
  }

  /// \brief Set the underlying camera model.
  /// @param[in] cameraGeometry The camera geometry.
  void setCameraGeometry(
      std::shared_ptr<camera_geometry_t> cameraGeometry)
  {
    cameraGeometryBase_ = cameraGeometry;
  }

  // error term and Jacobian implementation
  /**
   * @brief This evaluates the error term and additionally computes the Jacobians.
   * @param parameters Pointer to the parameters (see ceres)
   * @param residuals Pointer to the residual vector (see ceres)
   * @param jacobians Pointer to the Jacobians (see ceres)
   * @return success of th evaluation.
   */
  virtual bool Evaluate(double const* const * parameters, double* residuals,
                        double** jacobians) const;

  /**
   * @brief This evaluates the error term and additionally computes
   *        the Jacobians in the minimal internal representation.
   * @param parameters Pointer to the parameters (see ceres)
   * @param residuals Pointer to the residual vector (see ceres)
   * @param jacobians Pointer to the Jacobians (see ceres)
   * @param jacobiansMinimal Pointer to the minimal Jacobians (equivalent to jacobians).
   * @return Success of the evaluation.
   */
  virtual bool EvaluateWithMinimalJacobians(double const* const * parameters,
                                            double* residuals,
                                            double** jacobians,
                                            double** jacobiansMinimal) const;

  void setJacobiansZero(double** jacobians, double** jacobiansMinimal) const;

  // sizes
  /// \brief Residual dimension.
  size_t residualDim() const
  {
    return kNumResiduals;
  }

  /// \brief Number of parameter blocks.
  size_t parameterBlocks() const
  {
    return base_t::parameter_block_sizes().size();
  }

  /// \brief Dimension of an individual parameter block.
  /// @param[in] parameterBlockId ID of the parameter block of interest.
  /// \return The dimension.
  size_t parameterBlockDim(size_t parameterBlockId) const
  {
    return base_t::parameter_block_sizes().at(parameterBlockId);
  }

  /// @brief Residual block type as string
  virtual std::string typeInfo() const
  {
    return "EpipolarFactor";
  }

 protected:

  // An camera model shared by all EpipolarFactor.
  // The camera model is volatile and updated in every Evaluate() step.
  mutable std::shared_ptr<camera_geometry_t> cameraGeometryBase_;

  std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> measurement_;
  std::vector<Eigen::Matrix2d, Eigen::aligned_allocator<Eigen::Matrix2d>> covariance_;

  // const after initialization
  std::vector<std::shared_ptr<const okvis::ImuMeasurementDeque>> imuMeasCanopy_;

  // weighting related, they will be computed along with the residual
//  double information_; ///< The information matrix.
  mutable double squareRootInformation_; ///< The square root information matrix.

  std::vector<okvis::Time> stateEpoch_; ///< The timestamp of the set of robot states related to this error term.
  std::vector<okvis::Time> imageTimes_;
  ///< To avoid complexity in marginalizing speed and biases, first estimates
  ///  of speed and biases are used for computing pose at exposure.
  std::vector<Eigen::Matrix<double, 9, 1>,
              Eigen::aligned_allocator<Eigen::Matrix<double, 9, 1>>>
      speedAndBiases_;
  const double gravityMag_; ///< gravity in the world frame is [0, 0, -gravityMag_].
  double dtij_dtr_[2];  ///< kpN for left and right obs

  /**
   * @brief poseAndVelocityAtObservation compute T_WS nad velocity at observing a feature.
   * @param[in/out] pairT_WS in order to avoid use of okvis::Transformation
   * @param[in/out] velAndOmega records the linear and angular velocity compensated by the gyro bias
   * @param[in] parameters as used in Evaluate()
   * @param[in] index 0 for left camera, 1 for right
   */
  void poseAndVelocityAtObservation(
      std::pair<Eigen::Matrix<double, 3, 1>, Eigen::Quaternion<double>>*
          pairT_WS,
      Eigen::Matrix<double, 6, 1>* velAndOmega,
      double const* const* parameters, int index) const;
};

}  // namespace ceres
}  // namespace okvis

#include "implementation/EpipolarFactor.hpp"
#endif /* INCLUDE_OKVIS_CERES_EPIPOLAR_FACTOR_HPP_ */


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 3, 4> test_double_3_4(const Eigen::Matrix<double, 3, 4>& M) { return M; }
void export_double_3_4() { boost::python::def("test_double_3_4", test_double_3_4); }


#pragma once
#include <algorithm>
#include <string>
#include <vector>
#include <list>
//#include <Eigen/Geometry>
#include <boost/circular_buffer.hpp>
#include "field_obj_tracker/kalman.hpp"
#include "field_obj_tracker/objtype.hpp"

const size_t TrackedObjectHistoryLength = 20;

// Class to hold info on a tracked object
// Keeps a history per previous <historyLength> frames
// of whether the object was seen or not, and the
// calcualated distance and angle for each
// detection.
// Keep track of position (in 3 dimensions) of last detection - used
// to compare against new hits to see if this is the
// same object.
// Has method to compensate for robot rotation and translation with
// data from the fovis code
class TrackedObject
{
	public :
		TrackedObject(int                                       id,
					  const ObjectType                         &type_in,
					  const cv::Rect                           &screen_position,
					  double                                    avg_depth,
					  const image_geometry::PinholeCameraModel &model,
					  float                                     dt = 0.5,
					  float                                     accel_noise_mag = 0.25,
					  size_t                                    historyLength = TrackedObjectHistoryLength);

		//~TrackedObject();

		// Mark the object as detected in this frame
		void setDetected(void);

		// Clear the object detect flag for this frame.
		// Probably should only happen when moving to a new
		// frame, but may be useful in other cases
		void clearDetected(void);

		// Return the percent of last N frames
		// in which the object was seen
		double getDetectedRatio(void) const;

		bool tooManyMissedFrames(void) const;

		//contour area is the area of the contour stored in ObjectType
		//scaled into the bounding rect
		//only different in cases where contour is not a rectangle
		double contourArea(const image_geometry::PinholeCameraModel &model) const; //P.S. underestimates slightly

		// Update current object position based on a 3d position or
		//input rect on screen and depth
		void setPosition(const cv::Point3f &new_position);
		void setPosition(const cv::Rect &screen_position, double avg_depth, const image_geometry::PinholeCameraModel &model);

		// Adjust tracked object position based on motion
		// of the camera
		//void adjustPosition(const Eigen::Transform<double, 3, Eigen::Isometry> &delta_robot);
		void adjustPosition(const cv::Mat &transform_mat, float depth, const image_geometry::PinholeCameraModel &model);

		//get position of a rect on the screen corresponding to the object size and location
		//inverse of setPosition(Rect,depth)
		cv::Rect getScreenPosition(const image_geometry::PinholeCameraModel &model) const;
		cv::Point3f getPosition(void) const { return position_; }

		//void adjustKF(const Eigen::Transform<double, 3, Eigen::Isometry> &delta_robot);
		void adjustKF(cv::Point3f delta_pos);

		cv::Point3f predictKF(void);
		cv::Point3f updateKF(cv::Point3f pt);
		std::vector<cv::Point> getScreenPositionHistory(const image_geometry::PinholeCameraModel &model) const;

		std::string getId(void) const { return id_; }
		ObjectType getType(void) const { return type_; }

	private :
		ObjectType type_;

		cv::Point3f position_; // last position of tracked object

		// whether or not the object was seen in a given frame -
		// used to flag entries in other history arrays as valid
		// and to figure out which tracked objects are persistent
		// enough to care about
		boost::circular_buffer<bool> detectHistory_;
		boost::circular_buffer<cv::Point3f> positionHistory_;

		// Kalman filter for tracking and noise filtering
		TKalmanFilter KF_;

		std::string id_; //unique target ID - use a string rather than numbers so it isn't confused
						 // with individual frame detect indexes
		int missedFrameCount_;

		float cameraElevation_;

		void addToPositionHistory(const cv::Point3f &pt);
};

// Used to return info to display
struct TrackedObjectDisplay
{
	std::string id;
	cv::Rect rect;
	double ratio;
	cv::Point3f position;
	std::string name;
};

// Tracked object array -
//
// Need to create array of tracked objects.
// For each frame,
//   use fovis data to determine camera translation and rotation
//   update each object's position to "undo" that motion
//   for each detected rectangle
//      try to find a close match in the list of previously detected objects
//      if found
//         update that entry's distance and angle
//      else
//         add new entry
//   find a way to clear out images "lost" - look at history, how far
//   off the screen is has been rotated, etc.  Don't be too aggressive
//   since we could rotate back and "refind" an object which has disappeared
//
class TrackedObjectList
{
	public :
		// Create a tracked object list.  Set the object width in inches
		// (feet, meters, parsecs, whatever) and imageWidth in pixels since
		// those stay constant for the entire length of the run
		TrackedObjectList(const std::string &trackingBaseFrame);

		// Adjust the angle of each tracked object based on
		// the rotation of the robot straight from fovis
		//void adjustLocation(const Eigen::Transform<double, 3, Eigen::Isometry> &delta_robot);
		void adjustLocation(const cv::Mat &transformMat, const image_geometry::PinholeCameraModel &model);

		// Get position history for each tracked object - used for visualization
		std::vector<std::vector<cv::Point>> getScreenPositionHistories(const image_geometry::PinholeCameraModel &model) const;
		// Simple printout of list into
		void print(void) const;

		// Return list of detect info for external processing
		void getDisplay(std::vector<TrackedObjectDisplay> &displayList, const image_geometry::PinholeCameraModel &model) const;

		// Process a set of detected rectangles
		// Each will either match a previously detected object or
		// if not, be added as new object to the list
		void processDetect(const std::vector<cv::Rect> &detectedRects,
						   const std::vector<float> &depths,
						   const std::vector<ObjectType> &types,
						   const image_geometry::PinholeCameraModel &model);

	private :
		std::list<TrackedObject> list_; // list of currently valid detected objects
		int detectCount_;               // ID of next object to be created

		std::string trackingBaseFrame_; // ID for the frame to track objects in
};


#include "STLSceneLoader.h"
#include "SceneDrawables.h"
#include "SceneLoader.h"
#include "NullOut.h"
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>
#include <fstream>
#include "gettext.h"

using namespace std;
using namespace boost::algorithm;
using boost::format;
using namespace cnoid;

namespace {

struct Registration {
    Registration(){
        SceneLoader::registerLoader(
            "stl",
            []() -> shared_ptr<AbstractSceneLoader> { return make_shared<STLSceneLoader>(); });
    }
} registration;

}

static void readVector3(string text, SgVectorArray<Vector3f>* array)
{
    trim(text);
    Vector3f value;
    int i = 0;
    split_iterator<string::iterator> iter = make_split_iterator(text, token_finder(is_space(), token_compress_on));
    while(iter != split_iterator<string::iterator>()){
        value[i++] = boost::lexical_cast<float>(*iter++);
        if(i == 3){
            array->push_back(value);
            break;
        }
    }
}

STLSceneLoader::STLSceneLoader()
{
    os_ = &nullout();
}

void STLSceneLoader::setMessageSink(std::ostream& os)
{
    os_ = &os;
}

SgNode* STLSceneLoader::load(const std::string& filename)
{
    std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary);
    if(!ifs.is_open()){
        os() << str(format(_("Unable to open file \"%1%\".")) % filename) << endl;
        return 0;
    }

    ifs.seekg(0, fstream::end);
    unsigned int fileSize = ifs.tellg();
    ifs.seekg(0, fstream::beg);

    SgVertexArrayPtr vertices = new SgVertexArray;
    SgNormalArrayPtr normals = new SgNormalArray;

    bool isBinary = false;
    unsigned int numFaces = 0;
    uint8_t buf[84];
    ifs.read((char*)buf, 84);
    if(ifs.gcount() == 84){
        numFaces = buf[80] + (buf[81] << 8) + (buf[82] << 16) + (buf[83] << 24);
        unsigned int expectedSize = numFaces * 50 + 84;
        if(expectedSize == fileSize){
            isBinary = true;
        }
    }

    if(isBinary){
        for(size_t i = 0; i < numFaces; i++){
            Vector3f value;
            for(size_t j = 0; j < 3; j++){
                float v;
                ifs.read((char *)&v, 4);
                value[j] = v;
            }
            normals->push_back(value);
            for(size_t k = 0; k < 3; k++){
                Vector3f value;
                for(size_t j = 0; j < 3; j++){
                    float v;
                    ifs.read((char *)&v, 4);
                    value[j] = v;
                }
                vertices->push_back(value);
            }
            uint16_t attrib;
            ifs.read((char *)&attrib, 2);
        }
    } else {
        // text format
        std::ifstream ifs(filename.c_str(), std::ios::in);
        std::string line;
        while(!ifs.eof() && getline(ifs, line)){
            trim(line);
            if(boost::istarts_with(line, "vertex")){
                readVector3(line.substr(6), vertices);
            } else if(boost::istarts_with(line, "facet normal")){
                readVector3(line.substr(12), normals);
            }
        }
    }

    SgShape* shape = 0;

    if(vertices->empty()){
        os() << "Empty vertices." << endl;

    } else {
        shape = new SgShape;
        SgMesh* mesh = shape->getOrCreateMesh();
        mesh->setVertices(vertices);
        const int numTriangles = vertices->size() / 3;
        mesh->reserveNumTriangles(numTriangles);
        for(int i = 0; i < numTriangles; ++i){
            const int j = i * 3;
            mesh->addTriangle(j, j + 1, j + 2);
        }
        if(!normals->empty()){
            mesh->setNormals(normals);
            SgIndexArray& indices = mesh->normalIndices();
            indices.reserve(normals->size() * 3);
            for(size_t i = 0; i < normals->size(); ++i) {
                indices.push_back(i);
                indices.push_back(i);
                indices.push_back(i);
            }
        }
        mesh->updateBoundingBox();
    }

    return shape;
}


/**
 * Implements a octree data structure using PCL as a backend
 * for pushbroom stereo data.
 *
 * Supports checking trajectories against the octree to determine nearest neighbor.
 *
 * (C) 2015 Andrew Barry <abarry@csail.mit.edu>
 */

#ifndef STEREO_OCTOMAP_H_
#define STEREO_OCTOMAP_H_

#include <iostream>

#include "opencv2/opencv.hpp"

#include <Eigen/Dense>

#include <GL/gl.h>
#include <bot_lcmgl_client/lcmgl.h>
#include <bot_core/rotations.h>
#include <bot_frames/bot_frames.h>
#include <bot_param/param_client.h>
#include <lcmtypes/octomap_raw_t.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include "../../LCM/lcmt_stereo_with_xy.h"
#include "../../LCM/lcmt/stereo.hpp"
#include "../../sensors/stereo/opencv-stereo-util.hpp"

#define OCTREE_LIFE 4000000 // in usec

using Eigen::Matrix3d;
using Eigen::Vector3d;

class StereoOctomap {

    public:

        StereoOctomap(BotFrames *bot_frames);

        void ProcessStereoMessage(const lcmt::stereo *msg);

        void PublishOctomap(lcm_t *lcm);
        //void PublishToStereo(lcm_t *lcm, int frame_number, int video_number);
        void PublishToHud(lcm_t *lcm) const;

        void SetStereoConfig(OpenCvStereoConfig stereo_config, OpenCvStereoCalibration stereo_calibration) {
            stereo_config_  = stereo_config;
            stereo_calibration_ = stereo_calibration;
            stereo_calibration_set_ = true;
        }

        //static void GetOctomapPoints(OcTree *octomap, vector<cv::Point3f> *octomap_points, BotTrans *transform = NULL, bool discard_behind = false);

        void PrintAllPoints() const;
        void Draw(lcm_t *lcm) const;

        double NearestNeighbor(double point[3]) const;

    private:

        void InsertPointsIntoOctree(const lcmt::stereo *msg, BotTrans *to_open_cv);
        void RemoveOldPoints(int64_t last_msg_time);

        OpenCvStereoCalibration stereo_calibration_;
        OpenCvStereoConfig stereo_config_;
        bool stereo_calibration_set_;

        pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> *current_octree_;
        pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> *building_octree_;

        int64_t current_octree_timestamp_, building_octree_timestamp_;

        BotFrames *bot_frames_;

        pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud_;
        pcl::PointCloud<pcl::PointXYZ>::Ptr building_cloud_;

};

#endif


/* discriminative_trainer_test.cc
   Jeremy Barnes, 28 October 2009
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

   Unit tests for discriminative training of a neural netrowk.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#undef NDEBUG

#include <boost/test/unit_test.hpp>
#include <boost/multi_array.hpp>
#include "jml/neural/dense_layer.h"
#include "jml/utils/testing/serialize_reconstitute_include.h"
#include <boost/assign/list_of.hpp>
#include <limits>

using namespace ML;
using namespace ML::DB;
using namespace std;

using boost::unit_test::test_suite;

BOOST_AUTO_TEST_CASE( xor_test )
{
}


#include "LinearApproximation.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

LinearApproximation::LinearApproximation(const size_t t_numVars)
  : m_numVars(t_numVars)
{
}

LinearApproximation::~LinearApproximation()
{
}

double LinearApproximation::average() const
{
  double sum = 0;
  for (std::vector<std::vector<double> >::const_iterator lhsitr = m_values.begin();
      lhsitr != m_values.end();
      ++lhsitr)
  {
    sum += (*lhsitr)[m_numVars];
  }
  return sum / m_numVars;
}

LinearApproximation LinearApproximation::operator-(const LinearApproximation &t_rhs) const
{
  if (m_numVars != t_rhs.m_numVars)
  {
    throw std::runtime_error("Unable to subtract, different number of variables");
  }

  LinearApproximation retval(m_numVars);

  for (std::vector<std::vector<double> >::const_iterator lhsitr = m_values.begin();
      lhsitr != m_values.end();
      ++lhsitr)
  {
    for (std::vector<std::vector<double> >::const_iterator rhsitr = t_rhs.m_values.begin();
        rhsitr != t_rhs.m_values.end();
        ++rhsitr)
    {
      std::vector<double> vals(m_numVars, 0);

      bool match = true;
      for (size_t i = 0; i < m_numVars; ++i)
      {
        if ( (*lhsitr)[i] != (*rhsitr)[i] )
        {
          match = false;
          break;
        } else {
          vals[i] = (*lhsitr)[i];
        }
      }

      if (match)
      {
        double value = (*lhsitr)[m_numVars] - (*rhsitr)[m_numVars];
        retval.addVals(vals, value);
        break;
      }
    }
  }

  return retval;
}

void LinearApproximation::addVals(std::vector<double> t_vals, const double t_result)
{
  validateVariableSize(t_vals);

  t_vals.push_back(t_result);

  for (std::vector<std::vector<double> >::const_iterator lhsitr = m_values.begin();
      lhsitr != m_values.end();
      ++lhsitr)
  {
    bool match = true;
    for (size_t i = 0; i < m_numVars; ++i)
    {
      if ( (*lhsitr)[i] != t_vals[i] )
      {
        match = false;
        break;
      }
    }

    if (match)
    {
      if ((*lhsitr)[m_numVars] != t_result)
      {
        throw std::runtime_error("Data already exists with a different result value");
      } else {
        // it already exists
        return;
      }
    }
  }

  // it didn't already exist, add it now
  m_values.push_back(t_vals);

}

std::vector<std::vector<double> > LinearApproximation::findMinimalDifferences(
    size_t t_numDifferences,
    const std::vector<std::vector<double> > &t_vals,
    const std::vector<std::vector<double> > &t_data) const
{
  // t_vals includes the baseline requested value, so we want one more than that
  if (t_data.empty() || t_numDifferences + 2 == t_vals.size())
  {
    // this is as good as we're going to get / can use right now
    std::vector<std::vector<double> > retval = t_vals;

    if (!t_vals.empty())
    {
      retval.erase(retval.begin());
    }
    return retval;
  }

  std::vector<std::vector<double> > candidates;
  std::vector<std::vector<double> > remainder;

  for (size_t i = 0; i < t_data.size(); ++i)
  {
    size_t numDifferences = 0;

    for (size_t j = 0; j < m_numVars; ++j)
    {
      for (size_t i2 = 0; i2 < t_vals.size(); ++i2)
      {
        if (t_data[i][j] != t_vals[i2][j])
        {
          ++numDifferences;
          break; // don't check any further input rows
        }
      }

      if (numDifferences > t_numDifferences)
      {
        // don't continue with this data row
        break;
      }
    }

    if (numDifferences <= t_numDifferences)
    {
      candidates.push_back(t_data[i]);
    } else {
      remainder.push_back(t_data[i]);
    }
  }

  std::vector<std::vector<double> > keptresult = t_vals;
  keptresult.erase(keptresult.begin());

  for (size_t i = 0; i < candidates.size(); ++i)
  {
    std::vector<std::vector<double> > newvals(t_vals);
    newvals.push_back(candidates[i]);

    std::vector<std::vector<double> > newdata(t_data);
    newdata.erase(std::remove(newdata.begin(), newdata.end(), candidates[i]), newdata.end());

    std::vector<std::vector<double> > result = findMinimalDifferences(t_numDifferences, newvals, newdata);
    if (result.size() > keptresult.size())
    {
      keptresult = result;
    }
  }

  return keptresult;
}

std::vector<std::vector<double> > LinearApproximation::filterForProblemReduction(const std::vector<double> &t_vals,
    const std::vector<std::vector<double> > &t_data) const
{
  std::vector<std::vector<double> > testData;
  testData.push_back(t_vals);

  for (size_t i = 1; i < m_numVars; ++i)
  {
    std::vector<std::vector<double> > results = findMinimalDifferences(i, testData, t_data);
    if (results.size() > i)
    {
      return results;
    }
  }

  return t_data;
}

double LinearApproximation::approximate(const std::vector<double> &t_vals) const
{
  // Linear Approximation
  // A(x-x0) + B(y-y0) + C(z-z0) = 0
  validateVariableSize(t_vals);

//  print("Approximating: ", t_vals);

  std::vector<std::vector<double> > sortedPoints = sortByDistance(t_vals, m_values);

  if (!sortedPoints.empty() && distance(t_vals, sortedPoints[0]) == 0)
  {
    //return exact match
    return sortedPoints[0].back();
  }

  std::vector<std::vector<double> > optimalPoints = filterForProblemReduction(t_vals, sortedPoints);

//  print("Starting points: ", m_values);

//  print("Sorted Points: ", sortedPoints);
  std::vector<std::vector<double> > chosenPoints = filterForSimilarity(t_vals, optimalPoints);

//  print("Chosen points: ", chosenPoints);

  std::vector<std::vector<std::vector<double> > > coefficientMatrices = buildCoefficientMatrices(chosenPoints);

//  print("Coefficients: ", coefficientMatrices);

  std::vector<double> coefficients = solveDeterminates(coefficientMatrices);

//  print("Coefficients: ", coefficients);
  assert(coefficients.size() == m_numVars + 1);

  double approximation = 0;
  for (size_t i = 0; i < m_numVars; ++i)
  {
    // A(x-x0) + B(y-y0) + C(z-z0) ...
    approximation += coefficients[i] * (t_vals[i] - chosenPoints[0][i]);
  }

  // the last variable is the one we are solving for
  // z=(z0*C+(y0-y)*B+(x0-x)*A)/C
  if (coefficients[m_numVars] == 0)
  {
    throw std::runtime_error("Unabled to approximate, not enough data");
  }

  approximation = (approximation + chosenPoints[0][m_numVars] * coefficients[m_numVars]) / coefficients[m_numVars];

//  std::cout << "final approximation: " << approximation << std::endl;

  return approximation;
}

void LinearApproximation::validateVariableSize(const std::vector<double> &t_vals) const
{
  if (t_vals.size() != m_numVars)
  {
    throw std::range_error("Unexpected number of variables supplied");
  }
}

const std::vector<std::vector<double> > LinearApproximation::filterForSimilarity(const std::vector<double> &t_point, const std::vector<std::vector<double> > &t_vals) const
{
  std::vector<std::vector<double> > goodPoints = t_vals;
  int pointsNeeded = m_numVars + 1;

  for (size_t i = 0; i < m_numVars; ++i)
  {
    bool diversityFound = false;
    bool allTheSame = true;

    int differencePosition = -1;

    int rowCount = 0;
    for (std::vector<std::vector<double> >::const_iterator itr = t_vals.begin();
        itr != t_vals.end();
        ++itr, ++rowCount)
    {
      if (t_vals[0][i] != (*itr)[i])
      {
        diversityFound = true; // not all of the inputs match each other
      }

      if (t_point[i] != (*itr)[i])
      {
        allTheSame = false; // not all of the inputs for this position match the requested approximation
        differencePosition = rowCount;
      }
    }

    if (!diversityFound && !allTheSame)
    {
      // There is not enough data available to calculate the value for this position
      std::stringstream ss;
      ss << "Not enough data available for the " << i << " variable position";
      throw std::runtime_error(ss.str());
    }

    if (allTheSame || (differencePosition >= pointsNeeded))
    {
      if (pointsNeeded > 2)
      {
        --pointsNeeded;
        goodPoints.resize(std::min(pointsNeeded, static_cast<int>(goodPoints.size()))); // never expand it
      }
    }
  }

  goodPoints.resize(std::min(pointsNeeded, static_cast<int>(goodPoints.size())));
  return goodPoints;
}

std::vector<std::vector<std::vector<double> > > LinearApproximation::buildCoefficientMatrices(
    const std::vector<std::vector<double> > &t_points) const
{
  std::set<size_t> discardedColumns;

  for (size_t i = 0; i <= m_numVars; ++i)
  {
    bool allthesame = true;
    for (std::vector<std::vector<double> >::const_iterator itr = t_points.begin();
        itr != t_points.end();
        ++itr)
    {
      if ((*itr)[i] != t_points[0][i])
      {
        allthesame = false;
      }
    }

    if (allthesame)
    {
      discardedColumns.insert(i);
    }
  }

  std::vector<std::vector<std::vector<double> > > retvals;

  for (size_t i = 0; i <= m_numVars; ++i)
  {
    if (discardedColumns.count(i) == 1)
    {
      // this is a discarded column its value is 0
      std::vector<double> v(1,0); // a vector of one element of value 0
      std::vector<std::vector<double> > m;
      m.push_back(v);
      retvals.push_back(m);
    } else {
      std::vector<std::vector<double> > m;
      for (size_t curRow = 0; curRow < m_numVars - discardedColumns.size(); ++curRow)
      {
        std::vector<double> row;
        for (size_t j = 0; j <= m_numVars; ++j)
        {
          if (j != i && discardedColumns.count(j) == 0)
          {
            // This is a column we want to work with
            row.push_back(t_points[curRow+1][j] - t_points[0][j]);
          }
        }
        m.push_back(row);
      }
      retvals.push_back(m);
    }
  }

  return retvals;
}

std::vector<std::vector<double> > LinearApproximation::removeCol(const std::vector<std::vector<double> > &t_matrix, const size_t col) const
{
  std::vector<std::vector<double> > retval(t_matrix.size()-1, std::vector<double>(t_matrix.size()-1, 0));

  for (size_t i = 1; i < t_matrix.size(); ++i)
  {
    size_t colnum = 0;
    for (size_t j = 0; j < t_matrix[i].size(); ++j)
    {
      if (j != col)
      {
        retval[i-1][colnum] = t_matrix[i][j];
        ++colnum;
      }
    }
  }

  return retval;
}

double LinearApproximation::determinate(const std::vector<std::vector<double> > &t_matrix) const
{
/*
  bool docache = false;

  if (docache)
  {
    std::map<std::vector<std::vector<double> >, double>::const_iterator itr = m_cache.find(t_matrix);
    if (itr != m_cache.end())
    {
      std::cout << "Cache hit" << std::endl;
      return itr->second;
    }
  }

  if (t_matrix.size() == 1)
  {
    return t_matrix[0][0];
  } else if (t_matrix.size() == 2) {
    return t_matrix[0][0] * t_matrix[1][1] - t_matrix[0][1] * t_matrix[1][0];
  } else {
    double d = 0;

    for (size_t i = 0; i < t_matrix.size(); ++i)
    {
      //          d += pow(-1, i) * t_matrix[0][i] * determinate(removeCol(t_matrix, i));
      d += (i%2==1?-1:1) * t_matrix[0][i] * determinate(removeCol(t_matrix, i));
    }

    if (docache)
    {
      m_cache[t_matrix] = d;
    }
    return d;
  }
  */

  namespace bnu = boost::numeric::ublas;

  class Determinant
  {

    static int determinant_sign(const bnu::permutation_matrix<std::size_t>& pm)
    {
      int pm_sign=1;
      std::size_t size = pm.size();
      for (std::size_t i = 0; i < size; ++i)
      {
        if (i != pm(i)) {
          pm_sign *= -1; // swap_rows would swap a pair of rows here, so we change sign
        }
      }
      return pm_sign;
    }

    static double determinant( bnu::matrix<double>& m ) {
      bnu::permutation_matrix<std::size_t> pm(m.size1());
      double det = 1.0;
      if( bnu::lu_factorize(m,pm) ) {
        det = 0.0;
      } else {
        for(size_t i = 0; i < m.size1(); i++) {
          det *= m(i,i); // multiply by elements on diagonal
        }

        det = det * determinant_sign( pm );
      }
      return det;
    }

    public:
    static double determinant(const std::vector<std::vector<double> > &t_matrix)
    {
      bnu::matrix<double> matrix(t_matrix.size(), t_matrix.size());

      for (size_t i = 0; i < t_matrix.size(); ++i)
      {
        for (size_t j = 0; j < t_matrix[i].size(); ++j)
        {
          matrix(i, j) = t_matrix[i][j];
        }
      }

      return determinant(matrix);
    }
  };

  double retval = Determinant::determinant(t_matrix);

  /*
  if (docache)
  {
    m_cache[t_matrix] = retval;
  }
  */

  return retval;
}

std::vector<double> LinearApproximation::solveDeterminates(const std::vector<std::vector<std::vector<double> > > &t_matrices) const
{
  std::vector<double> retval;

  for (std::vector<std::vector<std::vector<double> > >::const_iterator itr = t_matrices.begin();
      itr != t_matrices.end();
      ++itr)
  {
    retval.push_back(determinate(*itr));
  }

  return retval;
}

std::vector<std::vector<double> > LinearApproximation::sortByDistance(const std::vector<double> &t_point,
    const std::vector<std::vector<double> > &t_values) const
{
  std::set<std::pair<double, std::vector<double> > > distanceSorted;

  for (std::vector<std::vector<double> >::const_iterator itr = t_values.begin();
      itr != t_values.end();
      ++itr)
  {
    distanceSorted.insert(std::make_pair(distance(t_point, *itr), *itr));
  }

  std::vector<std::vector<double> > retval;

  for (std::set<std::pair<double, std::vector<double> > >::const_iterator itr = distanceSorted.begin();
      itr != distanceSorted.end();
      ++itr)
  {
    retval.push_back(itr->second);
  }

  return retval;
}

double LinearApproximation::distance(const std::vector<double> &t_p1, const std::vector<double> &t_p2) const
{
  double result = 0;
  for (size_t i = 0; i < m_numVars; ++i)
  {
    double part = t_p1[i] - t_p2[i];
    part *= part;

    result += part;
  }

  return sqrt(result);
}

void LinearApproximation::print(const std::string &t_str, const std::vector<double> &t_vals)
{
  if (!t_str.empty())
  {
    std::cout << t_str << std::endl;
  }

  std::cout << "[ ";
  for (std::vector<double>::const_iterator itr = t_vals.begin();
      itr != t_vals.end();
      ++itr)
  {
    std::cout << std::setw(10) << *itr << " ";
  }
  std::cout << "]" << std::endl;
}

void LinearApproximation::print(const std::string &t_str, const std::vector<std::vector<double> > &t_vals)
{
  if (!t_str.empty())
  {
    std::cout << t_str << std::endl;
  }

  for (std::vector<std::vector<double> >::const_iterator itr = t_vals.begin();
      itr != t_vals.end();
      ++itr)
  {
    print("", *itr);
  }
}

std::pair<double, double> LinearApproximation::nearestFurthestNeighborDistances(const std::vector<double> &t_vals) const
{
  std::vector<std::vector<double> > sorted = sortByDistance(t_vals, m_values);

  if (sorted.size() < 1)
  {
    throw std::range_error("no neighbors");
  }

  return std::make_pair(distance(t_vals, sorted.front()), distance(t_vals, sorted.back()));
}

void LinearApproximation::print(const std::string &t_str, const std::vector<std::vector<std::vector<double> > > &t_vals)
{
  if (!t_str.empty())
  {
    std::cout << t_str << std::endl;
  }

  for (std::vector<std::vector<std::vector<double> > >::const_iterator itr = t_vals.begin();
      itr != t_vals.end();
      ++itr)
  {
    print("", *itr);
    std::cout << std::endl;
  }
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Eigen. http://eigen.tuxfamily.org/dox/TopicUnalignedArrayAssert.html, last accessed: 18th
 *          March, 2013.
 *
 *    Notes
 *      This implementation uses the KeplerianElements class, which is marked for deprecation. The
 *      code will be updated to use a simple "Vector6d" object from the Eigen library instead.
 *
 */

#include <stdexcept>

#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerStateExtractor.h"
#include "Tudat/InputOutput/parsedDataVectorUtilities.h"

namespace tudat
{
namespace ephemerides
{

//! Extract the Keplerian Elements.
std::shared_ptr< Eigen::Vector6d > KeplerStateExtractor::extract(
        ParsedDataLineMapPtr dataLineMap )
{
    // Short-hand notation.
    namespace parsed_data_vector_utilities = input_output::parsed_data_vector_utilities;
    using Eigen::Vector6d;

    // Create a new KeplerianElements object.
    std::shared_ptr< Vector6d > keplerianElements
            = std::allocate_shared< Vector6d >( Eigen::aligned_allocator< Vector6d >( ) );

    // Find and set semi-major axis.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::semiMajorAxis ) )
    {
        ( *keplerianElements )( orbital_element_conversions::semiMajorAxisIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::semiMajorAxis );
    }

    else
    {
        throw std::runtime_error( "No semi-major axis entry found." );
    }

    // Find and set eccentricity.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::eccentricity ) )
    {
        ( *keplerianElements )( orbital_element_conversions::eccentricityIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::eccentricity );
    }

    else
    {
        throw std::runtime_error( "No eccentricity entry found." );
    }

    // Find and set inclination.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::inclination ) )
    {
        ( *keplerianElements )( orbital_element_conversions::inclinationIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::inclination );
    }

    else
    {
        throw std::runtime_error( "No inclination entry found." );
    }

    // Find and set longitude of ascending node.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::longitudeOfAscendingNode ) )
    {
        ( *keplerianElements )( orbital_element_conversions::longitudeOfAscendingNodeIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::longitudeOfAscendingNode );
    }

    else
    {
        throw std::runtime_error( "No longitude of ascending node entry found." );
    }

    // Find and set argument of periapsis.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::argumentOfPeriapsis ) )
    {
        ( *keplerianElements )( orbital_element_conversions::argumentOfPeriapsisIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::argumentOfPeriapsis );
    }

    else
    {
        throw std::runtime_error( "No argument of periapsis entry found." );
    }

    // Find and set true anomaly.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::trueAnomaly ) )
    {
        ( *keplerianElements )( orbital_element_conversions::trueAnomalyIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::trueAnomaly );
    }

    // If the true anomaly is not present, check for mean anomaly.
    else if ( checkOptionalFieldType( dataLineMap, 1,
                                      input_output::field_types::state::meanAnomaly ) )
    {
        // Store mean anomaly.
        const double meanAnomaly = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::meanAnomaly );

        // Retrieve eccentricity.
        const double eccentricity
                = ( *keplerianElements )( orbital_element_conversions::eccentricityIndex );

        // Convert to eccentric anomaly.
        const double eccentricAnomaly = orbital_element_conversions::
                convertMeanAnomalyToEccentricAnomaly( eccentricity, meanAnomaly );

        // Convert eccentric anomaly to true anomaly and set the latter.
        ( *keplerianElements )( orbital_element_conversions::trueAnomalyIndex )
                = orbital_element_conversions::
                convertEccentricAnomalyToTrueAnomaly( eccentricAnomaly, eccentricity );
    }

    else
    {
        throw std::runtime_error( "No true anomaly or mean anomaly entries found." );
    }

    return keplerianElements;
}

} // namespace ephemerides
} // namespace tudat


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

/// \file rmax_adjust.cc
///

#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/fstream.hpp"
namespace po = boost::program_options;
namespace fs = boost::filesystem;

#include <vw/Camera/CAHVORModel.h>
#include <vw/BundleAdjustment.h>
#include <vw/Math.h>
#include <vw/Math/LevenbergMarquardt.h>

using namespace vw;
using namespace vw::camera;
using namespace vw::ba;

#include <stdlib.h>
#include <iostream>

#include <asp/Sessions/RMAX/RMAX.h>

// This sifts out from a vector of strings, a listing of GCPs.  This
// should be useful for those programs who accept their data in a mass
// input vector.
std::vector<std::string>
sort_out_gcps( std::vector<std::string>& image_files ) {
  std::vector<std::string> gcp_files;
  std::vector<std::string>::iterator it = image_files.begin();
  while ( it != image_files.end() ) {
    if ( boost::iends_with(*it, ".gcp") ){
      gcp_files.push_back( *it );
      it = image_files.erase( it );
    } else
      it++;
  }

  return gcp_files;
}

static std::string prefix_from_filename(std::string const& filename) {
  std::string result = filename;
  int index = result.rfind(".");
  if (index != -1)
    result.erase(index, result.size());
  return result;
}

class HelicopterBundleAdjustmentModel : public ba::ModelBase<HelicopterBundleAdjustmentModel, 6, 3> {

  typedef Vector<double,6> camera_vector_t;
  typedef Vector<double,3> point_vector_t;

  std::vector<ImageInfo> m_image_infos;

  boost::shared_ptr<ControlNetwork> m_network;

  std::vector<camera_vector_t> a;
  std::vector<point_vector_t> b;
  std::vector<camera_vector_t> a_target;
  std::vector<point_vector_t> b_target;
  int m_num_pixel_observations;

public:

  HelicopterBundleAdjustmentModel(std::vector<ImageInfo> const& image_infos,
                                  boost::shared_ptr<ControlNetwork> network) :
    m_image_infos(image_infos), m_network(network),
    a(image_infos.size()), b(network->size()),
    a_target(image_infos.size()), b_target(network->size()) {

    // Compute the number of observations from the bundle.
    m_num_pixel_observations = 0;
    for (unsigned i = 0; i < network->size(); ++i)
      m_num_pixel_observations += (*m_network)[i].size();

    // Set up the a and b vectors, storing the initial values.
    for (unsigned j = 0; j < m_image_infos.size(); ++j) {
      a[j] = camera_vector_t();
      a_target[j] = a[j];
    }

    for (unsigned i = 0; i < m_network->size(); ++i) {
      b[i] = (*m_network)[i].position();
      b_target[i] = b[i];
    }

  }

  // Return a reference to the camera and point parameters.
  camera_vector_t A_parameters(int j) const { return a[j]; }
  point_vector_t B_parameters(int i) const { return b[i]; }
  void set_A_parameters(int j, camera_vector_t const& a_j) {
    a[j] = a_j;
  }
  void set_B_parameters(int i, point_vector_t const& b_i) {
    b[i] = b_i;
  }

  // Return the initial parameters
  camera_vector_t A_target(int j) const { return a_target[j]; }
  point_vector_t B_target(int i) const { return b_target[i]; }

  // Return general sizes
  unsigned num_cameras() const { return a.size(); }
  unsigned num_points() const { return b.size(); }
  unsigned num_pixel_observations() const { return m_num_pixel_observations; }

  // Return pixel observations
  unsigned num_observations_of_point(const int& i) const { return (*m_network)[i].size(); }
  unsigned corresponding_camera_for_measure(const int& i, const int& m){
    return (*m_network)[i][m].image_id();
  }
  Vector2 pixel_observation_of_point(const int& i, const int& m) const {
    //Finding out which camera accounts for the m_th observations of point i
    unsigned int camera_id = (*m_network)[i][m].image_id();

    Vector<double,6> a_j = a[camera_id];
    Vector3 position_correction = subvector(a_j,0,3);
    Vector3 pose_correction = subvector(a_j,3,3);
    camera::CAHVORModel cam = rmax_image_camera_model(m_image_infos[camera_id],position_correction,pose_correction);

    Vector4 point_estimation = B_parameters(i);
    point_estimation = point_estimation/point_estimation(3);
    return cam.point_to_pixel(subvector(point_estimation,0,3));
  }

  // Return the covariance of the camera parameters for camera j.
  inline Matrix<double,camera_params_n,camera_params_n> A_inverse_covariance ( unsigned /*j*/ ) {
    Matrix<double,camera_params_n,camera_params_n> result;
    result(0,0) = 1/1;  // Position sigma = 1 meter
    result(1,1) = 1/1;
    result(2,2) = 1/1;
    result(3,3) = 1;  // Pose sigma = 1 degrees
    result(4,4) = 1;
    result(5,5) = 1;
    return result;
  }

  // Return the covariance of the point parameters for point i.
  inline Matrix<double,point_params_n,point_params_n> B_inverse_covariance ( unsigned /*i*/ ) {
    Matrix<double,point_params_n,point_params_n> result;
    result(0,0) = 1/1000.0;  // Point sigma = 1000 meters ( we set this to be
    result(1,1) = 1/1000.0;  // so large that it essentially removes point position
    result(2,2) = 1/1000.0;  // constraints from the bundle adjustment entirely. )
    return result;
  }

  void write_adjustment(int j, std::string const& filename) {
    std::ofstream ostr(filename.c_str());
    ostr << a[j][0] << " " << a[j][1] << " " << a[j][2] << "\n";
    ostr << a[j][3] << " " << a[j][4] << " " << a[j][5] << "\n";
  }

  void write_adjusted_camera(int j, std::string const& filename) {
    Vector<double,6> a_j = a[j];
    Vector3 position_correction = subvector(a_j, 0, 3);
    Vector3 pose_correction = subvector(a_j, 3,3);
    camera::CAHVORModel cam = rmax_image_camera_model(m_image_infos[j], position_correction, pose_correction);
    cam.write(filename);
  }

  void write_adjusted_cameras_append(std::string const& filename) {
    std::ofstream ostr(filename.c_str(),std::ios::app);
    for (unsigned j=0; j < a.size();++j){
      Vector<double,6> a_j = a[j];
      Vector3 position_correction = subvector(a_j,0,3);
      Vector3 pose_correction = subvector(a_j,3,3);
      camera::CAHVORModel cam = rmax_image_camera_model(m_image_infos[j],position_correction,pose_correction);
      ostr << j << "\t" << cam.C(0) << "\t" << cam.C(1) << "\t" << cam.C(2) << "\n";
      ostr << j << "\t" << cam.A(0) << "\t" << cam.A(1) << "\t" << cam.A(2) << "\n";
      ostr << j << "\t" << cam.H(0) << "\t" << cam.H(1) << "\t" << cam.H(2) << "\n";
      ostr << j << "\t" << cam.V(0) << "\t" << cam.V(1) << "\t" << cam.V(2) << "\n";
      ostr << j << "\t" << cam.O(0) << "\t" << cam.O(1) << "\t" << cam.O(2) << "\n";
      ostr << j << "\t" << cam.R(0) << "\t" << cam.R(1) << "\t" << cam.R(2) << "\n";
    }
  }

  std::vector<boost::shared_ptr<camera::CameraModel> > adjusted_cameras() {
    std::vector<boost::shared_ptr<camera::CameraModel> > result(a.size());
    for (unsigned j = 0; j < result.size(); ++j) {
      Vector3 position_correction = subvector(a[j], 0, 3);
      Vector3 pose_correction = subvector(a[j], 3,3);

      camera::CAHVORModel *cahvor = new camera::CAHVORModel;
      *cahvor = rmax_image_camera_model(m_image_infos[j], position_correction, pose_correction);

      result[j] = boost::shared_ptr<camera::CameraModel>( cahvor );
    }
    return result;
  }

  // Given the 'a' vector (camera model parameters) for the j'th
  // image, and the 'b' vector (3D point location) for the i'th
  // point, return the location of b_i on imager j in pixel
  // coordinates.
  Vector2 operator() ( unsigned /*i*/, unsigned j, Vector<double,6> const& a_j, Vector<double,3> const& b_i ) const {
    Vector3 position_correction = subvector(a_j, 0, 3);
    Vector3 pose_correction = subvector(a_j, 3,3);
    camera::CAHVORModel cam = rmax_image_camera_model(m_image_infos[j], position_correction, pose_correction);
    return cam.point_to_pixel(b_i);
  }

  // Errors on the image plane
  void image_errors( std::vector<double>& pix_errors ) {
    pix_errors.clear();
    for (unsigned i = 0; i < m_network->size(); ++i)
      for(unsigned m = 0; m < (*m_network)[i].size(); ++m) {
        int camera_idx = (*m_network)[i][m].image_id();
        Vector2 pixel_error = (*m_network)[i][m].position() - (*this)(i, camera_idx,
                                                                   a[camera_idx],b[i]);
        pix_errors.push_back(norm_2(pixel_error));
      }
  }

  // Errors for camera position
  void camera_position_errors( std::vector<double>& camera_position_errors ) {
    camera_position_errors.clear();
    for (unsigned j=0; j < this->num_cameras(); ++j) {
      Vector3 position_initial, position_now;
      position_initial = subvector(a_target[j],0,3);
      position_now = subvector(a[j],0,3);

      camera_position_errors.push_back(norm_2(position_initial-position_now));
    }
  }

  // Errors for camera pose
  void camera_pose_errors( std::vector<double>& camera_pose_errors ) {
    camera_pose_errors.clear();
    for (unsigned j=0; j < this->num_cameras(); ++j) {
      Vector3 pose_initial, pose_now;
      pose_initial = subvector(a_target[j],3,3);
      pose_now = subvector(a[j],3,3);

      camera_pose_errors.push_back(norm_2(pose_initial-pose_now));
    }
  }

  // Errors for gcp errors
  void gcp_errors( std::vector<double>& gcp_errors ) {
    gcp_errors.clear();
    for (unsigned i=0; i < this->num_points(); ++i)
      if ((*m_network)[i].type() == ControlPoint::GroundControlPoint) {
        point_vector_t p1 = b_target[i]/b_target[i](3);
        point_vector_t p2 = b[i]/b[i](3);
        gcp_errors.push_back(norm_2(subvector(p1,0,3) - subvector(p2,0,3)));
      }
  }

  // Give access to the control network
  boost::shared_ptr<ControlNetwork> control_network() {
    return m_network;
  }
};

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

  std::vector<std::string> image_files;
  std::vector<std::string> gcp_files;
  std::string cnet_file;
  boost::shared_ptr<ControlNetwork> cnet( new ControlNetwork("My first control network"));
  double lambda;
  int min_matches;
  double robust_outlier_threshold;
  int max_iterations;
  int report_level;

  po::options_description general_options("Options");
  general_options.add_options()
    ("cnet,c", po::value<std::string>(&cnet_file), "Load a control network from a file")
    ("lambda,l", po::value<double>(&lambda), "Set the initial value of the LM parameter lambda")
    ("min-matches", po::value<int>(&min_matches)->default_value(5), "Set the mininmum number of matches between images that will be considered.")
    ("robust-threshold", po::value<double>(&robust_outlier_threshold)->default_value(10.0), "Set the threshold for robust cost functions.")
    ("max-iterations", po::value<int>(&max_iterations)->default_value(25), "Set the maximum number of iterations.")
    ("save-iteration-data,s", "Saves all camera information between iterations to iterCameraParam.txt, it also saves point locations for all iterations in iterPointsParam.txt. Warning: This is slow as pixel observations need to be calculated on each step.")
    ("report-level,r",po::value<int>(&report_level)->default_value(10),"Changes the detail of the Bundle Adjustment Report")
    ("run-match,m", "Run ipmatch to create .match files from overlapping images.")
    ("match-debug-images,d", "Create debug images when you run ipmatch.")
    ("help,h", "Display this help message");

  po::options_description hidden_options("");
  hidden_options.add_options()
    ("input-files", po::value<std::vector<std::string> >(&image_files));

  po::options_description options("Allowed Options");
  options.add(general_options).add(hidden_options);

  po::positional_options_description p;
  p.add("input-files", -1);

  po::variables_map vm;
  po::store( po::command_line_parser( argc, argv ).options(options).positional(p).run(), vm );
  po::notify( vm );

  std::ostringstream usage;
  usage << "Usage: " << argv[0] << " [options] <rmax image filenames>..." << std::endl << std::endl;
  usage << general_options << std::endl;

  if( vm.count("help") ) {
    std::cout << usage << std::endl;
    return 1;
  }

  if( vm.count("input-files") < 1 || image_files.size() < 2) {
    std::cout << "Error: Must specify at least two input files!" << std::endl << std::endl;
    std::cout << usage.str();
    return 1;
  }
  gcp_files = sort_out_gcps( image_files );

  // Read in the camera model and RMAX image info for the input
  // images.
  std::vector<std::string> camera_files(image_files.size());
  std::vector<ImageInfo> image_infos(image_files.size());
  std::vector<boost::shared_ptr<CameraModel> > camera_models(image_files.size());
  for (unsigned i = 0; i < image_files.size(); ++i) {
    read_image_info( image_files[i], image_infos[i] );
    CAHVORModel *cahvor = new CAHVORModel;
    *cahvor = rmax_image_camera_model(image_infos[i]);
    camera_models[i] = boost::shared_ptr<CameraModel>(cahvor);
  }

  if ( vm.count("cnet") ) {
    // Loading a Control Network

    vw_out() << "Loading control network from file: " << cnet_file << "\n";
    cnet->read_binary(cnet_file);

  } else {
    // Building a Control Network
    build_control_network( *cnet, camera_models,
                           image_files, min_matches );
    add_ground_control_points( *cnet, image_files,
                               gcp_files.begin(), gcp_files.end() );

    cnet->write_binary("rmax_adjust");
  }

  HelicopterBundleAdjustmentModel ba_model(image_infos, cnet);
  AdjustSparse<HelicopterBundleAdjustmentModel, L2Error> bundle_adjuster(ba_model, L2Error());

  if (vm.count("lambda"))
    bundle_adjuster.set_lambda(lambda);

  //Clearing the monitoring text files to be used for saving camera params
  if (vm.count("save-iteration-data")){
    std::ofstream ostr("iterCameraParam.txt",std::ios::out);
    ostr << "";
    ostr.close();
    ostr.open("iterPointsParam.txt",std::ios::out);
    ostr << "";
    ostr.close();

    //Now I'm going to save the initial starting position of the cameras
    ba_model.write_adjusted_cameras_append("iterCameraParam.txt");
    std::ofstream ostr_points("iterPointsParam.txt",std::ios::app);
    for (unsigned i = 0; i < ba_model.num_points(); ++i){
      Vector<double,3> current_point = ba_model.B_parameters(i);
      ostr_points << i << "\t" << current_point(0) << "\t" << current_point(1) << "\t" << current_point(2) << "\n";
    }
  }

  // Reporter
  BundleAdjustReport<AdjustSparse<HelicopterBundleAdjustmentModel, L2Error> >
    reporter( "RMAX Adjust", ba_model, bundle_adjuster, report_level );

  // Performing Bundle Adjustment
  double abs_tol = 1e10, rel_tol=1e10;
  while(bundle_adjuster.update(abs_tol, rel_tol)) {
    reporter.loop_tie_in();

    // Writing Current Camera Parameters to file for later reading in MATLAB
    if (vm.count("save-iteration-data")) {

      //Writing this iterations camera data
      ba_model.write_adjusted_cameras_append("iterCameraParam.txt");

      //Writing this iterations point data, also saving the pixel param data
      std::ofstream ostr_points("iterPointsParam.txt",std::ios::app);
      for (unsigned i = 0; i < ba_model.num_points(); ++i){

        Vector<double,3> current_point = ba_model.B_parameters(i);
        ostr_points << i << "\t" << current_point(0) << "\t" << current_point(1) << "\t" << current_point(2) << "\n";
      }
    }

    if (bundle_adjuster.iterations() > max_iterations || abs_tol < 0.01 || rel_tol < 1e-10)
      break;
  }
  reporter.end_tie_in();

  for (unsigned int i=0; i < ba_model.num_cameras(); ++i)
    ba_model.write_adjustment(i, prefix_from_filename(image_files[i])+".rmax_adjust");

  // Compute the post-adjustment residuals
  std::vector<boost::shared_ptr<CameraModel> > adjusted_cameras = ba_model.adjusted_cameras();
}