#include <cstdlib>
#include <sstream>
#include <iostream>
#include <vector>
#include <limits>
#include <cmath>

#if HAVE_NEW_CXX
# include <unordered_map>
# include <unordered_set>
#else
# include <tr1/unordered_map>
# include <tr1/unordered_set>
namespace std { using std::tr1::unordered_map; using std::tr1::unordered_set; }
#endif

#include <signal.h>

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

#include "json_feature_map_lexer.h"
#include "fdict.h"
#include "feature_map.h"
#include "prob.h"
#include "filelib.h"
#include "liblbfgs/lbfgs++.h"

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

volatile bool* requested_stop = NULL;

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("linear,n", "Linear regression (default is multiclass logistic regression)")
        ("ordinal,o", "Ordinal regression (proportional odds)")
        ("training_features,x", po::value<vector<string> >(), "Files containing training instance features")
        ("training_responses,y", po::value<string>(), "File containing training instance responses (if unspecified, do prediction only)")
        ("tx", po::value<vector<string> >(), "File containing test instance features")
        ("ty", po::value<string>(), "File containing test instance responses (optional)")
        ("z", po::value<string>(), "Write learned weights to this file (optional)")
        ("write_test_predictions,p", "Write model prediction for each test instance")
        ("write_test_distribution,D", "Write posterior distribution of outputs for each test instance (categorical models only)")
        ("dont_write_weights,W", "Supress writing learned weights")
        ("multiclass_test_probability_threshold,P", po::value<double>()->default_value(0.0), "When evaluating a multiclass model, only compute the accuracy on instances where the predicted class posterior probability is > P")
        ("l1",po::value<double>()->default_value(0.0), "l_1 regularization strength")
        ("l2",po::value<double>()->default_value(1e-10), "l_2 regularization strength")
        ("temperature,T",po::value<double>()->default_value(0), "Temperature for entropy regularization (> 0 flattens, < 0 sharpens; = 0 no effect)")
        ("weights,w", po::value<string>(), "Initial weights file")
        ("epsilon,e", po::value<double>()->default_value(1e-4), "Epsilon for convergence test. Terminates when ||g|| < epsilon * max(1, ||w||)")
        ("delta,d", po::value<double>()->default_value(0), "Delta for convergence test. Terminates when (f' - f) / f < delta")
        ("memory_buffers,m",po::value<unsigned>()->default_value(40), "Number of memory buffers for LBFGS")
        ("test_features,t", po::value<string>(), "[deprecated option, use --tx] File containing test instance features")
        ("test_responses,s", po::value<string>(), "[deprecated option, use --ty] File containing test response features (ARKRegression format)")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  bool prediction_only = !conf->count("training_responses");
  if (conf->count("help") || (prediction_only && (!conf->count("weights") || !conf->count("tx")))) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

enum RegressionType { kLINEAR, kLOGISTIC, kORDINAL };

struct TrainingInstance {
  FrozenFeatureMap x;
  union {
    unsigned label;  // for categorical & ordinal predictions
    float value;     // for continuous predictions
  } y;
};

struct ReaderHelper {
  explicit ReaderHelper(vector<TrainingInstance>* xyp,
                        bool h,
                        vector<string>* iids) : xy_pairs(xyp), lc(), flag(), has_labels(h), ids(iids), merge() {}
  unordered_map<string, unsigned> id2ind;
  FeatureMapStorage* fms;
  vector<TrainingInstance>* xy_pairs;
  int lc;
  bool flag;
  bool has_labels;
  vector<string>* ids;
  vector<bool> merged;
  bool merge;
};

void ReaderCB(const string& id,
              const std::pair<int,float>* begin,
              const std::pair<int,float>* end,
              void* extra) {
  ReaderHelper& rh = *reinterpret_cast<ReaderHelper*>(extra);
  ++rh.lc;
  if (rh.lc % 1000  == 0) { cerr << '.'; rh.flag = true; }
  if (rh.lc % 50000 == 0) { cerr << " [" << rh.lc << "]\n"; rh.flag = false; }
  if (rh.ids && !rh.merge) rh.ids->push_back(id);
  if (rh.has_labels) {
    const unordered_map<string, unsigned>::iterator it = rh.id2ind.find(id);
    if (it == rh.id2ind.end()) {
      cerr << "\nUnlabeled example in line " << rh.lc << " (key=" << id << ')' << endl;
      abort();
    } else {
      if (rh.merge) {
        rh.merged[it->second - 1] = true;
        const FrozenFeatureMap& prev = (*rh.xy_pairs)[it->second - 1].x;
        (*rh.xy_pairs)[it->second - 1].x = rh.fms->AddFeatureMap(begin,end,prev);
      } else {
        (*rh.xy_pairs)[it->second - 1].x = rh.fms->AddFeatureMap(begin,end);
      }
    }
  } else {
    TrainingInstance x_no_y;
    if (rh.merge) {
      if (!rh.ids) {
        cerr << "Missing IDs\n";
        abort();
      }
      unsigned& ind = rh.id2ind[id];
      rh.merged[ind - 1] = true;
      const FrozenFeatureMap& prev = (*rh.xy_pairs)[ind - 1].x;
      (*rh.xy_pairs)[ind - 1].x = rh.fms->AddFeatureMap(begin,end,prev);
    } else {
      x_no_y.x = rh.fms->AddFeatureMap(begin,end);
      unsigned& ind = rh.id2ind[id];
      assert(ind == 0);
      rh.xy_pairs->push_back(x_no_y);
      ind = rh.xy_pairs->size();
    }
  }
}

void ReadWeightsMulticlass(const string& fname,
                           vector<string>* plabels,
                           vector<double>* pw) {
  map<string, unsigned> lm;
  vector<string>& labels = *plabels;
  vector<double>& weights = *pw;
// 3	***CATEGORICAL***	Iris-versicolor	Iris-setosa	Iris-virginica
  ReadFile rf(fname);
  istream& in = *rf.stream();
  unsigned rk; // read K
  string cat;
  in >> rk >> cat;
  if (cat != "***CATEGORICAL***") {
    cerr << "Unexpected weights type: " << cat << endl;
    abort();
  }
  bool has_labels = labels.size() > 0;
  for (unsigned i = 0; i < rk; ++i) {
    string f;
    in >> f;
    if (has_labels) {
      if (labels[i] != f) { cerr << "Bad label order: " << labels[i] << " != " << f << endl; abort(); }
    } else {
      labels.push_back(f);
    }
  }
  for (unsigned i = 0; i < labels.size(); ++i)
    lm[labels[i]] = i;
  const unsigned K = labels.size();
// Iris-versicolor	***BIAS***	17.619343957411
// Iris-setosa	***BIAS***	28.1532494500727
//  Iris-versicolor	petal-length	-2.69876613900064
  string l,f;
  double w;
  weights.resize((1 + FD::NumFeats()) * (labels.size() - 1), 0.0);
  for (unsigned i = 1; i < K; ++i) {
    in >> l >> f >> w;
    if (f != "***BIAS***") { cerr << "Bad format!\n"; abort(); }
    weights[lm[l]] = w;
  }
  unsigned p = FD::NumFeats();
  FD::Freeze();
  string fl;
  unsigned total = 0;
  unsigned skipped = 0;
  getline(in, fl); // extra newline after >> reading
  while(getline(in, fl)) {
    ++total;
    size_t first_field_end = fl.find('\t');
    size_t second_field_end = fl.rfind('\t');
    if (first_field_end == string::npos || second_field_end == string::npos || first_field_end == second_field_end) {
      cerr << "Badly formatted weight: " << fl << endl;
      abort();
    }
    unsigned y = lm[fl.substr(0, first_field_end)];
    unsigned fid = FD::Convert(fl.substr(first_field_end+1,second_field_end - first_field_end - 1));
    double w = strtod(&fl[second_field_end+1], NULL);
    if (!fid) {
      // cerr << "Skipping feature " << FD::Convert(fid) << endl;
      ++skipped;
    } else {
      weights[(K - 1) + y * p + fid] = w;
    }
  }
  if (skipped) {
    cerr << "Skipped " << skipped << " unneeded features of " << total << " total features\n";
  }
}

void ReadWeightsLinear(const string& fname,
                       vector<double>* pw) {
  map<string, unsigned> lm;
  vector<double>& weights = *pw;
  // 6	***CONTINUOUS***

  ReadFile rf(fname);
  istream& in = *rf.stream();
  unsigned rk; // read K
  string cat;
  in >> rk >> cat;
  if (cat != "***CONTINUOUS***") {
    cerr << "Unexpected weights type: " << cat << endl;
    abort();
  }
  weights.resize(1 + FD::NumFeats(), 0.0);
  string f;
  double w;
  in >> f >> w;
  if (f != "***BIAS***") { cerr << "Bad format!\n"; abort(); }
  weights[0] = w;
  // ***BIAS***	44.1115567293068
  // horsepower	-0.0366264048792159
  // acceleration	-0.0140216308150685

  FD::Freeze();
  string fl;
  unsigned total = 0;
  unsigned skipped = 0;
  getline(in, fl); // extra newline after >> reading
  while(getline(in, fl)) {
    ++total;
    size_t first_field_end = fl.find('\t');
    if (first_field_end == string::npos) {
      cerr << "Badly formatted weight: " << fl << endl;
      abort();
    }
    unsigned fid = FD::Convert(fl.substr(0, first_field_end));
    double w = strtod(&fl[first_field_end+1], NULL);
    if (!fid) {
      // cerr << "Skipping feature " << FD::Convert(fid) << endl;
      ++skipped;
    } else {
      weights[fid + 1] = w;
    }
  }
  if (skipped) {
    cerr << "Skipped " << skipped << " unneeded features of " << total << " total features\n";
  }
}

void ReadLabeledInstances(const vector<string>& ffeats,
                          const string& fresp,
                          const RegressionType resptype,
                          FeatureMapStorage* fms,
                          vector<TrainingInstance>* xy_pairs,
                          vector<string>* labels,
                          vector<string>* instance_ids = NULL) {
  bool flag = false;
  xy_pairs->clear();
  int lc = 0;
  ReaderHelper rh(xy_pairs, fresp.size() > 0, instance_ids);
  rh.merge = false;
  unordered_map<string, unsigned> label2id;
  if (fresp.size() == 0) {
    cerr << "No gold standard responses provided to learn from!" << endl;
  } else {
    cerr << "Reading responses from " << fresp << " ..." << endl;
    ReadFile fr(fresp);
    for (unsigned i = 0; i < labels->size(); ++i)
      label2id[(*labels)[i]] = i;
    istream& in = *fr.stream();
    string line;
    while(getline(in, line)) {
      ++lc;
      if (lc % 1000 == 0) { cerr << '.'; flag = true; }
      if (lc % 40000 == 0) { cerr << " [" << lc << "]\n"; flag = false; }
      if (line.size() == 0) continue;
      if (line[0] == '#') {
        if (line.size() > 1 && line[1] == '#') {
          if (lc != 1) {
            cerr << "[WARNING] Line " << lc << " appears to be label declaration ... ignoring\n";
          } else {
            istringstream is(line);
            string label;
            is >> label;
            while(is >> label) {
              unordered_map<string, unsigned>::iterator it = label2id.find(label);
              if (it == label2id.end()) {
                it = label2id.insert(make_pair(label, labels->size())).first;
                labels->push_back(label);
              }
            }
          }
        }
        continue;
      }
      unsigned p = 0;
      while (p < line.size() && line[p] != ' ' && line[p] != '\t') { ++p; }
      unsigned& ind = rh.id2ind[line.substr(0, p)];
      if (ind != 0) { cerr << "ID " << line.substr(0, p) << " duplicated in line " << lc << endl; abort(); }
      while (p < line.size() && (line[p] == ' ' || line[p] == '\t')) { ++p; }
      assert(p < line.size());
      xy_pairs->push_back(TrainingInstance());
      ind = xy_pairs->size();
      switch (resptype) {
        case kLINEAR:
          xy_pairs->back().y.value = strtof(&line[p], 0);
          break;
        case kLOGISTIC:
          {
            unordered_map<string, unsigned>::iterator it = label2id.find(line.substr(p));
            if (it == label2id.end()) {
              const string label = line.substr(p);
              it = label2id.insert(make_pair(label, labels->size())).first;
              labels->push_back(label);
            }
            xy_pairs->back().y.label = it->second;  // label id
          }
          break;
        case kORDINAL:
          {
            // TODO allow labels not to be consecutive and start from 0
            // requires label re-indexing
            const unsigned label = strtol(&line[p], 0, 10);
            xy_pairs->back().y.label = label;
            if (label >= labels->size()) {
              labels->resize(label + 1);
            }
            (*labels)[label] = line.substr(p);
          }
          break;
      }
    }
    if (flag) cerr << endl;
    if (resptype == kLOGISTIC || resptype == kORDINAL) {
      cerr << "LABELS:";
      for (unsigned j = 0; j < labels->size(); ++j)
        cerr << " " << (*labels)[j];
      cerr << endl;
    }
  }
  FeatureMapStorage* pfms = NULL;
  FeatureMapStorage* tfms = NULL;
  for (unsigned i = 0; i < ffeats.size(); ++i) {
    if (i == ffeats.size() - 1) {
      pfms = tfms;
      rh.fms = fms;
    } else {
      delete pfms;
      pfms = tfms;
      tfms = new FeatureMapStorage;
      rh.fms = tfms;
    }
    if (pfms) {
      rh.merge = true;
      rh.merged.clear();
      rh.merged.resize(rh.xy_pairs->size(), false);
    }
    const string& ffeat = ffeats[i];
    cerr << "Reading features from " << ffeat << " ..." << endl;
    ReadFile ff(ffeat);
    JSONFeatureMapLexer::ReadRules(ff.stream(), ReaderCB, &rh);
    if (pfms) {
      for (unsigned j = 0; j < rh.xy_pairs->size(); ++j)
        if (!rh.merged[j])
          (*rh.xy_pairs)[j].x = rh.fms->AddFeatureMap(NULL, NULL, (*rh.xy_pairs)[j].x);
    }
    if (rh.flag) cerr << endl;
  }
  delete pfms;
}

// helper base class (not polymorphic- just a container and some helper functions) for loss functions
// real loss functions should implement double operator()(const vector<double>& x, double* g),
// which should evaluate f(x) and g = f'(x)
struct BaseLoss {
  // dimp1 = number of categorial outputs possible for logistic regression
  // for linear regression, it should be 1 more than the dimension of the response variable
  BaseLoss(
      const vector<TrainingInstance>& tr,
      unsigned dimp1,
      unsigned numfeats,
      unsigned ll2) : training(tr), K(dimp1), p(numfeats), l2(ll2) {}

  // w.x (bias excluded)
  template <class FeatureMapType>
  double DotProduct(const FeatureMapType& fx,
                    const vector<double>& w) const {
    const unsigned km1 = K - 1;
    double dotproduct = 0;
    for (typename FeatureMapType::const_iterator it = fx.begin(); it != fx.end(); ++it)
      dotproduct += w[it->first + km1] * it->second;
    return dotproduct;
  }

  double ApplyRegularizationTerms(const vector<double>& weights,
                                  double* g) const {
    double reg = 0;
    for (size_t i = K - 1; i < weights.size(); ++i) {
      const double& w_i = weights[i];
      reg += l2 * w_i * w_i;
      g[i] += 2 * l2 * w_i;
    }
    return reg;
  }

  template <class FeatureMapType>
  void GradAdd(const FeatureMapType& fx,
               const unsigned y,
               const double scale,
               double* acc) const {
    acc[y] += scale; // class bias
    for (typename FeatureMapType::const_iterator it = fx.begin();
         it != fx.end(); ++it)
      acc[it->first + y * p + K - 1] += it->second * scale;
  }

  const vector<TrainingInstance>& training;
  const unsigned K, p;
  const double l2;
};

struct UnivariateSquaredLoss : public BaseLoss {

  // weight vector layout for p features
  //   w[0] = bias weight
  //   w[1 : p] = feature weights

  UnivariateSquaredLoss(
          const vector<TrainingInstance>& tr,
          unsigned numfeats,
          const double l2) : BaseLoss(tr, 2, numfeats, l2) {}

  template <class FeatureMapType>
  double Predict(const FeatureMapType& fx, const vector<double>& w) const {
    return DotProduct(fx, w) + w[0];
  }

  // evaluate squared loss and gradient
  double operator()(const vector<double>& x, double* g) const {
    fill(g, g + x.size(), 0.0);
    double cll = 0;
    for (unsigned i = 0; i < training.size(); ++i) {
      const FrozenFeatureMap& fmapx = training[i].x;
      const double refy = training[i].y.value;
      const double predy = Predict(fmapx, x);
      const double diff = predy - refy;
      cll += diff * diff;
      GradAdd(fmapx, 0, 2 * diff, g);
    }
    const double reg = ApplyRegularizationTerms(x, g);
    return cll + reg;
  }

  // return RMSE
  double Evaluate(const vector<TrainingInstance>& test,
                  const vector<double>& w,
                  vector<float>* preds = NULL) const {
    vector<double> dotprods(1);  // K-1 degrees of freedom
    double mse = 0;
    if (preds) preds->resize(test.size());
    for (unsigned i = 0; i < test.size(); ++i) {
      const double predy = Predict(test[i].x, w);
      if (preds) (*preds)[i] = predy;
      const double refy = test[i].y.value;
      const double diff = predy - refy;
      //cerr << "line=" << (i+1) << " true=" << refy << " pred=" << predy << endl;
      mse += diff * diff;
    }
    mse /= test.size();
    return sqrt(mse);
  }
};

// predictions made by multiclass logistic regression for some
// input x
//   y_hat is the 1-best prediction
//   posterior[y] is p(y_hat | x)
struct MulticlassPrediction {
  unsigned y_hat;
  vector<double> posterior;
};

struct MulticlassLogLoss : public BaseLoss {

  // weight vector layout for K classes, with p features
  //   w[0 : K-2] = bias weights
  //   w[y*p + K-1 : y*p + K + p - 2] = feature weights for y^th class

  MulticlassLogLoss(
          const vector<TrainingInstance>& tr,
          unsigned k,
          unsigned numfeats,
          const double l2,
          const double t = 0.0) : BaseLoss(tr, k, numfeats, l2), T(t) {}

  //   g = E[ F(x,y) * log p(y|x) ] + H(y | x) * E[ F(x,y) ]
  //   note: g will be scaled by T
  double Entropy(const prob_t& z,
                 const vector<double>& dotprods,
                 const FrozenFeatureMap& fmapx,
                 double* g) const {
    const double log_z = log(z);
    double entropy = log_z * exp(-log_z);   // class K dotprod = 0
    map<unsigned, double> ef;
    for (unsigned j = 0; j < dotprods.size(); ++j) {
      const double log_prob = dotprods[j] - log_z;
      const double prob = exp(log_prob);
      const double e_logprob = prob * log_prob;
      entropy -= e_logprob;
      GradAdd(fmapx, j, T * e_logprob, g);
    }
    for (unsigned j = 0; j < dotprods.size(); ++j) {
      const double log_prob = dotprods[j] - log_z;
      const double prob = exp(log_prob);
      GradAdd(fmapx, j, T * prob * entropy, g);
    }

    return entropy;
  }

  // evaluate log loss and gradient
  double operator()(const vector<double>& x, double* g) const {
    fill(g, g + x.size(), 0.0);
    vector<double> dotprods(K - 1);  // K-1 degrees of freedom
    vector<prob_t> probs(K);
    double cll = 0;
    double entropy = 0;  // only computed if T != 0
    for (unsigned i = 0; i < training.size(); ++i) {
      const FrozenFeatureMap& fmapx = training[i].x;
      const unsigned refy = training[i].y.label;
      ComputeDotProducts(fmapx, x, &dotprods);
      prob_t z;
      for (unsigned j = 0; j < dotprods.size(); ++j)
        z += (probs[j] = prob_t(dotprods[j], init_lnx()));
      z += (probs.back() = prob_t::One());
      for (unsigned y = 0; y < probs.size(); ++y) {
        probs[y] /= z;
        //cerr << "  p(y=" << y << ")=" << probs[y].as_float() << "\tz=" << z << endl;
      }
      cll -= log(probs[refy]);  // log p(y | x)

      for (unsigned y = 0; y < dotprods.size(); ++y) {
        double scale = probs[y].as_float();
        if (y == refy) { scale -= 1.0; }
        GradAdd(fmapx, y, scale, g);
      }
      if (T) entropy += Entropy(z, dotprods, fmapx, g);
    }
    double reg = ApplyRegularizationTerms(x, g);
    return cll + reg - T * entropy;
  }

  template <class FeatureMapType>
  pair<unsigned, double> Predict(const FeatureMapType& fx,
                                 const vector<double>& w,
                                 MulticlassPrediction* pred = NULL) const {
    vector<double> dotprods(K - 1);  // K-1 degrees of freedom
    if (pred) pred->posterior.resize(K);
    ComputeDotProducts(fx, w, &dotprods);
    prob_t z = prob_t::One();  // exp(0) for k^th class
    for (unsigned j = 0; j < dotprods.size(); ++j)
      z += prob_t(dotprods[j], init_lnx());
    const double log_z = log(z);
    double best = 0;
    unsigned besty = dotprods.size();
    for (unsigned y = 0; y < dotprods.size(); ++y) {
      if (dotprods[y] > best) { best = dotprods[y]; besty = y; }
      if (pred) pred->posterior[y] = exp(dotprods[y] - log_z);
    }
    if (pred) {
      pred->posterior.back() = exp(-log_z);
      pred->y_hat = besty;
    }
    return make_pair(besty, exp(best - log_z));
  }

  double Evaluate(const vector<TrainingInstance>& test,
                  const vector<double>& w,
                  double thresh_p,
		  vector<MulticlassPrediction>* preds = NULL) const {
    double correct = 0;
    unsigned examples = 0;
    if (preds) preds->resize(test.size());
    for (unsigned i = 0; i < test.size(); ++i) {
      MulticlassPrediction* ppred = NULL;
      if (preds) ppred = &(*preds)[i];
      const pair<unsigned, double> pred = Predict(test[i].x, w, ppred);
      const unsigned predy = pred.first;
      const unsigned refy = test[i].y.label;
      // cerr << "line=" << (i+1) << " true=" << refy << " pred=" << predy << "  p(y|x) = " << pred.second << endl;
      if (pred.second >= thresh_p) {
        ++examples;
        if (refy == predy) ++correct;
      }
    }
    return correct / examples;
  }

  template <class FeatureMapType>
  void ComputeDotProducts(const FeatureMapType& fx,  // feature vector of x
                          const vector<double>& w,         // full weight vector
                          vector<double>* pdotprods) const {
    vector<double>& dotprods = *pdotprods;
    const unsigned km1 = K - 1;
    dotprods.resize(km1);
    for (unsigned y = 0; y < km1; ++y)
      dotprods[y] = w[y];  // bias terms
    for (typename FeatureMapType::const_iterator it = fx.begin(); it != fx.end(); ++it) {
      const float fval = it->second;
      const unsigned fid = it->first;
      for (unsigned y = 0; y < km1; ++y)
        dotprods[y] += w[fid + y * p + km1] * fval;
    }
  }

  const double T; // temperature for entropy regularization
};

struct OrdinalLogLoss : public BaseLoss {

  // weight vector layout for K levels, with p features
  //   w[0 : K-2] = level biases
  //   w[K-1 : K + p - 2] = feature weights

  OrdinalLogLoss(
          const vector<TrainingInstance>& tr,
          unsigned k,
          unsigned numfeats,
          const double l2) : BaseLoss(tr, k, numfeats, l2) {}

  // evaluate log loss and gradient
  double operator()(const vector<double>& x, double* g) const {
    const unsigned km1 = K - 1;
    double cll = 0;
    fill(g, g + x.size(), 0.0);
    vector<double> u(K);
    u[0] = u[km1] = 0;
    for (unsigned k = 1; k < km1; k++)
      u[k] = 1 / (1 - exp(x[k] - x[k - 1]));
    for (unsigned i = 0; i < training.size(); ++i) {
      const FrozenFeatureMap& fmapx = training[i].x;
      const unsigned level = training[i].y.label;
      const double dotprod = DotProduct(fmapx, x);
      const double pj = LevelProb(dotprod, x, level);
      const double pjp1 = LevelProb(dotprod, x, level + 1);
      cll -= LogDeltaProb(dotprod, x, level);
      if (level > 0)
        g[level - 1] -= u[level] - 1 + pj;
      if (level < km1)
        g[level] -= - u[level] + pjp1;
      double scale = (1 - pj - pjp1);
      for (FrozenFeatureMap::const_iterator it = fmapx.begin();
          it != fmapx.end(); ++it)
        g[km1 + it->first] -= it->second * scale;
    }
    double reg = ApplyRegularizationTerms(x, g);
    return cll + reg;
  }

  template <class FeatureMapType>
  unsigned Predict(const FeatureMapType& fx, const vector<double>& w) const {
    const double dotprod = DotProduct(fx, w);
    for (unsigned k = 0; k < K; k++)
      if (dotprod < w[k]) return k;
    return K-1;
  }

  template <class FeatureMapType>
  unsigned Predict(const FeatureMapType& fx, const vector<double>& w, MulticlassPrediction* pred) const {
    const double dotprod = DotProduct(fx, w);
    pred->posterior.resize(K);
    double bestp = -1;
    unsigned besty = 0;
    for (unsigned k = 0; k < K; ++k) {
      double p = exp(LogDeltaProb(dotprod, w, k));
      pred->posterior[k] = p;
      if (p > bestp) { bestp = p; besty = k; }
    }
    pred->y_hat = besty;
    return besty;
  }

  double Evaluate(const vector<TrainingInstance>& test,
                  const vector<double>& w,
                  vector<MulticlassPrediction>* predictions = NULL) const {
    double correct = 0;
    if (predictions) predictions->resize(test.size());
    MulticlassPrediction dummy;
    for (unsigned i = 0; i < test.size(); ++i) {
      MulticlassPrediction* pred = predictions ? &(*predictions)[i] : &dummy;
      const unsigned predy = Predict(test[i].x, w, pred);
      const unsigned refy = test[i].y.label;
      if (refy == predy) correct++;
    }
    return correct / test.size();
  }

  double LevelProb(double dotprod, const vector<double>& w,
                   unsigned level) const { // p(y >= level+1)
    if (level == K) return 0; // p(y > K) = 0
    if (level == 0) return 1; // p(y >= 1) = 1
    return 1 / (1 + exp(w[level - 1] - dotprod));
  }

  double LogDeltaProb(double dotprod, const vector<double>& w,
                      unsigned level) const { // log p(y == level + 1)
        if (level == K-1) {
          prob_t zj = prob_t(dotprod, init_lnx()) + prob_t(w[K-2], init_lnx());
          return dotprod - log(zj);
        }
        if (level == 0) {
          prob_t zjp1 = prob_t(dotprod, init_lnx()) + prob_t(w[0], init_lnx());
          return w[0] - log(zjp1);
        }
        if (w[level] <= w[level - 1]) return -1e3;
        prob_t zj = prob_t(dotprod, init_lnx()) + prob_t(w[level - 1], init_lnx());
        prob_t zjp1 = prob_t(dotprod, init_lnx()) + prob_t(w[level], init_lnx());
        prob_t dalpha = prob_t(w[level], init_lnx()) - prob_t(w[level - 1], init_lnx());
        return (dotprod + log(dalpha) - log(zj) - log(zjp1));
      }

};

template <class LossFunction>
double LearnParameters(LossFunction& loss,
                       const double l1,
                       const unsigned l1_start,
                       const unsigned memory_buffers,
                       const double epsilon,
                       const double delta,
                       vector<double>* px) {
  LBFGS<LossFunction> lbfgs(px, loss, memory_buffers, l1, l1_start, epsilon, delta);
  requested_stop = lbfgs.GetCancelFlag();
  lbfgs.MinimizeFunction();
  return 0;
}

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

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  string line;
  double l1 = conf["l1"].as<double>();
  double l2 = conf["l2"].as<double>();
  double temp = conf["temperature"].as<double>();
  const unsigned memory_buffers = conf["memory_buffers"].as<unsigned>();
  const double epsilon = conf["epsilon"].as<double>();
  const double delta = conf["delta"].as<double>();
  const double p_thresh = conf["multiclass_test_probability_threshold"].as<double>();
  if (l1 < 0.0) {
    cerr << "L1 strength must be >= 0\n";
    return 1;
  }
  if (l2 < 0.0) {
    cerr << "L2 strength must be >= 0\n";
    return 2;
  }
  if (p_thresh < 0.0 || p_thresh > 1.0) {
    cerr << "--multiclass_test_probability_threshold must be between 0 and 1\n";
    return 3;
  }

  RegressionType resptype = kLOGISTIC;
  if (conf.count("linear")) {
    if (conf.count("ordinal")) {
      cerr << "--ordinal and --linear are mutually exclusive\n";
      return 1;
    }
    resptype = kLINEAR;
  } else if (conf.count("ordinal")) {
    resptype = kORDINAL;
  }
  bool do_training = conf.count("training_features") && conf.count("training_responses");
  if (!do_training && (conf.count("training_features") && conf.count("training_responses"))) {
    cerr << "You must specify both training_features (-x) and training_responses (-y)!\n";
    return 1;
  }
  vector<string> labels; // only populated for non-continuous models
  vector<TrainingInstance> training, test;
  FeatureMapStorage fms;
  if (do_training) {
    vector<string> xfile = conf["training_features"].as<vector<string> >();
    string yfile = conf["training_responses"].as<string>();
    ReadLabeledInstances(xfile, yfile, resptype, &fms, &training, &labels);
  }

  if (conf.count("test_features")) {
    std::cerr << "THE OPTION --test_features IS DEPRECATED USE --tx AND --ty INSTEAD\n";
    const vector<string> txfile = conf["test_features"].as<vector<string> >();
    const string tyfile = conf["test_responses"].as<string>();
    ReadLabeledInstances(txfile, tyfile, resptype, &fms, &test, &labels);
  }

  bool test_labels = false;
  vector<string> test_ids;
  if (conf.count("tx")) {
    const vector<string> txfile = conf["tx"].as<vector<string> >();
    string tyfile;
    if (conf.count("ty")) tyfile = conf["ty"].as<string>();
    test_labels = tyfile.size();
    ReadLabeledInstances(txfile, tyfile, resptype, &fms, &test, &labels, &test_ids);
  }
  assert(test_ids.size() == test.size());

  string weights_file;
  if (conf.count("weights")) {
    weights_file = conf["weights"].as<string>();
    cerr << "               WEIGHTS FILE: " << weights_file << endl;
  }
  vector<double> weights;
  if (weights_file.size() > 0) {
    if (resptype == kLOGISTIC)
      ReadWeightsMulticlass(weights_file, &labels, &weights);
    else if (resptype == kLINEAR)
      ReadWeightsLinear(weights_file, &weights);
    else {
      cerr << "Don't know how to read weights file--please implement\n";
      abort();
    }
  }

  cerr << "         Number of features: " << FD::NumFeats() << endl;
  cerr << "Number of training examples: " << training.size() << endl;
  const unsigned p = FD::NumFeats();
  cout.precision(15);
  ostream* out = &cout;
  if (conf.count("dont_write_weights")) out = NULL;
  if (conf.count("z") == 1) {
    if (!out) {
      cerr << "You specified an output weights file (--z) but also used -W.\n";
      return 1;
    }
    out = new ofstream(conf["z"].as<string>().c_str());
  }
  bool write_dist = conf.count("write_test_distribution");
  bool write_pps = conf.count("write_test_predictions");

  // set up signal handler to catch SIGINT
  signal(SIGINT, signal_callback_handler);

  if (conf.count("linear")) {  // linear regression
    weights.resize(1 + p, 0.0);
    cerr << "       Number of parameters: " << weights.size() << endl;
    UnivariateSquaredLoss loss(training, p, l2);
    LearnParameters(loss, l1, 1, memory_buffers, epsilon, delta, &weights);

    if (test.size()) {
      vector<float> preds;
      double rmse = loss.Evaluate(test, weights, &preds);
      if (test_labels) {
        cerr << "Held-out RMSE: " << rmse << endl;
      }
      if (!test_labels || write_pps) {
        for (unsigned i = 0; i < test.size(); ++i)
          cout << test_ids[i] << "\t" << preds[i] << endl;
      }
    }

    if (out) {
      *out << p << "\t***CONTINUOUS***" << endl;
      *out << "***BIAS***\t" << weights[0] << endl;
      for (unsigned f = 0; f < p; ++f) {
        const double w = weights[1 + f];
        if (w)
          *out << FD::Convert(f) << "\t" << w << endl;
      }
    }
  } else if (conf.count("ordinal")) {
    const unsigned K = labels.size();
    const unsigned km1 = K - 1;
    weights.resize(p + km1, 0.0);
    for (unsigned k = 0; k < km1; k++)
      weights[k] = log(k+1) - log(K);
    if (weights_file.size() > 0) {
      cerr << "Please implement.\n";
      abort();
    }
    OrdinalLogLoss loss(training, K, p, l2);
    LearnParameters(loss, l1, km1, memory_buffers, epsilon, delta, &weights);

    if (test.size()) {
      vector<MulticlassPrediction> predictions;
      double acc = loss.Evaluate(test, weights, &predictions);
      if (test_labels) {
        cerr << "Held-out accuracy: " << acc << endl;
      }
      if (!test_labels || write_dist || write_pps) {
        for (unsigned i = 0; i < test.size(); ++i) {
          cout << test_ids[i] << '\t' << predictions[i].y_hat;
          if (write_dist) {
            cout << "\t{";
            for (unsigned y = 0; y < K; ++y)
              cout << (y ? ", " : "") << '"' << labels[y] << "\": " << predictions[i].posterior[y];
            cout << '}';
          }
          cout << endl;
        }
      }
    }

    if (out) {
      *out << p << "\t***ORDINAL***";
      for (unsigned y = 0; y < K; ++y)
        *out << '\t' << labels[y];
      *out << endl;
      for (unsigned y = 0; y < km1; ++y)
        *out << "y>=" << labels[y+1] << "\t" << weights[y] << endl;
      for (unsigned f = 0; f < p; ++f) {
        const double w = weights[km1 + f];
        if (w) *out << FD::Convert(f) << "\t" << w << endl;
      }
    }
  } else {                     // logistic regression
    weights.resize((1 + FD::NumFeats()) * (labels.size() - 1), 0.0);
    cerr << "       Number of parameters: " << weights.size() << endl;
    cerr << "           Number of labels: " << labels.size() << endl;
    const unsigned K = labels.size();
    const unsigned km1 = K - 1;
    MulticlassLogLoss loss(training, K, p, l2, temp);
    if (do_training) {
      LearnParameters(loss, l1, km1, memory_buffers, epsilon, delta, &weights);
    }

    if (test.size()) {
      vector<MulticlassPrediction> predictions;
      double acc = loss.Evaluate(test, weights, p_thresh, &predictions);
      if (test_labels) {
        cerr << "Held-out accuracy: " << acc << endl;
      }
      if (!test_labels || write_dist || write_pps) {
        for (unsigned i = 0; i < test.size(); ++i) {
          cout << test_ids[i] << '\t' << labels[predictions[i].y_hat];
          if (write_dist) {
            cout << "\t{";
            for (unsigned y = 0; y < K; ++y)
              cout << (y ? ", " : "") << '"' << labels[y] << "\": " << predictions[i].posterior[y];
            cout << '}';
          }
          cout << endl;
        }
      }
    }

    if (out) {
      *out << K << "\t***CATEGORICAL***";
      for (unsigned y = 0; y < K; ++y)
        *out << '\t' << labels[y];
      *out << endl;
      for (unsigned y = 0; y < km1; ++y)
        *out << labels[y] << "\t***BIAS***\t" << weights[y] << endl;
      for (unsigned y = 0; y < km1; ++y) {
        for (unsigned f = 0; f < p; ++f) {
          const double w = weights[km1 + y * p + f];
          if (w)
            *out << labels[y] << "\t" << FD::Convert(f) << "\t" << w << endl;
        }
      }
    }
  }
  if (out && out != &cout)
    delete out;

  return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110221    J. Leloux         Startup of TLE header file and class.
 *      110301    J. Leloux         Adjusting header file to parent classes and Tudat rules.
 *      110803    J. Leloux         First setup for codecheck.
 *      110805    K. Kumar          Layout and comment corrections; added
 *                                  get-function for vector container of TLE data.
 *      110810    J. Leloux         Tested new setup and changed descriptions.
 *      110826    J. Leloux         Added functionality for 2-line and 3-line data.
 *      111027    K. Kumar          Modified 2-line and 3-line options using enum.
 *
 *    References
 *      Leloux, J. Filtering Techniques for Orbital Debris Conjunction Analysis
 *          - applied to SSN TLE catalog data and including astrodynamics and
 *          collision probability theory, MSc Literature Research, Delft
 *          University of Technology, 2010.
 *      Celestrak (a). Space Track TLE Retriever Help,
 *          http://celestrak.com/SpaceTrack/TLERetrieverHelp.asp, 2011. Last
 *          accessed: 5 August, 2011.
 *      Space Track. TLE Format, http://www.space-track.org/tle_format.html,
 *          2004. Last accessed: 5 August, 2011.
 *      Celestrak (b). FAQs: Two-Line Element Set Format,
 *          http://celestrak.com/columns/v04n03/, 2006. Last accessed:
 *          5 August, 2011.
 *      Celestrak (c). NORAD Two-Line Element Set Format,
 *          http://celestrak.com/NORAD/documentation/tle-fmt.asp, 2004. Last
 *          accessed: 5 August, 2011.
 *
 *    Notes
 *      Raw TLE data can be obtained from (Celestrak (a), 2011). Explanations of the TLE data
 *      format can be viewed in (Space Track, 2004), (Celestrak (b), 2006), and
 *      (Celestrak (c), 2004).
 *
 */

#include <cmath>
#include <iostream>
#include <map>
#include <string>
#include <utility>

#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>
#include <boost/exception/all.hpp>
#include <boost/throw_exception.hpp>
#include <boost/algorithm/string/trim.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/InputOutput/twoLineElementsTextFileReader.h"

namespace tudat
{
namespace input_output
{

// Using declarations.
using mathematical_constants::PI;
using std::string;
using std::stringstream;
using std::endl;
using std::cerr;
using std::vector;
using std::multimap;
using std::pair;

//! Open data file.
void TwoLineElementsTextFileReader::openFile( )
{
    if ( absoluteDirectoryPath_.compare( "" ) == 0 )
    {
        absoluteFilePath_ = getTudatRootPath( ) + relativeDirectoryPath_ + fileName_;
    }

    else
    {
        absoluteFilePath_ = absoluteDirectoryPath_ + fileName_;
    }

    // Open data file.
    dataFile_.open( absoluteFilePath_.c_str( ), std::ios::binary );

    // Check if file could be opened. Throw exception with error message if file could not be
    // opened.
    if ( !dataFile_ )
    {
        boost::throw_exception(
                    boost::enable_error_info(
                        std::runtime_error(
                            boost::str( boost::format( "Data file '%s' could not be opened." )
                                 % absoluteFilePath_.c_str( ) ) ) )
            << boost::errinfo_file_name( absoluteFilePath_.c_str( ) )
            << boost::errinfo_file_open_mode( "std::ios::binary" )
            << boost::errinfo_api_function( "std::ifstream::open" ) );
    }
}

//! Skip lines.
void TwoLineElementsTextFileReader::skipLines( unsigned int numberOfLines )
{
    // Call getline( ) function for set number of lines to be skipped.
    for ( unsigned int i = 0; i < numberOfLines; i++ )
    {
        // Get next line of data from file and don't do anything.
        std::getline( dataFile_, stringOfData_ );

        // Increment line counter.
        lineCounter_++;
    }
}

//! Read and store data.
void TwoLineElementsTextFileReader::readAndStoreData( )
{
    // Reset the datafile.
    containerOfDataFromFile_.clear( );

    // Whilst the end of the data file has not been reached, continue reading
    // from lines from data file.
    while( !dataFile_.eof( ) )
    {
        // Get next line of data from data file and store in a string.
        getline( dataFile_, stringOfData_ );

        // Check if line of data is header line.
        if ( lineCounter_ <= numberOfHeaderLines_ )
        {
            // Store header line data.
            containerOfHeaderDataFromFile_[ lineCounter_ ] = stringOfData_;
        }

        // Else process non-header data line.
        else
        {
            // Check if string doesn't start with set starting character, if string
            // is not empty, and if the skip keyword is not in the string.
            if ( ( ( !startingCharacter_.empty( ) && stringOfData_.substr( 0, 1 )
                     .compare( startingCharacter_ ) != 0 )
                   || ( !skipKeyword_.empty( ) && stringOfData_.find( skipKeyword_ )
                        == string::npos )
                   || ( startingCharacter_.empty( ) && skipKeyword_.empty( ) ) )
                 && !stringOfData_.empty( ) )
            {
                // Store string in container.
                containerOfDataFromFile_[ lineCounter_ ] = stringOfData_;
            }
        }

        // Increment line counter.
        lineCounter_++;
    }
}

//! Read and store data.
void TwoLineElementsTextFileReader::readAndStoreData( unsigned int numberOfLines )
{
    // Loop over number of lines of data to read and stored from data file.
    for ( unsigned int i = 0; i < numberOfLines; i++ )
    {
        // Get next line of data from data file and store in a string.
        getline( dataFile_, stringOfData_ );

        // Check string is not empty.
        if ( !stringOfData_.empty( ) )
        {
            // Store string in container.
            containerOfDataFromFile_[ lineCounter_ ] = stringOfData_;
        }

        // Increment line counter.
        lineCounter_++;
    }
}

//! Strip End-Of-Line characters.
void TwoLineElementsTextFileReader::stripEndOfLineCharacters(
        LineBasedStringDataMap& containerOfLinesOfData )
{
    // Declare local variables.
    // Declare string iterator.
    string::iterator iteratorString_;

    // Loop through all the strings stored in the container.
    for ( LineBasedStringDataMap::iterator iteratorContainerOfDataFromFile_
          = containerOfLinesOfData.begin( );
          iteratorContainerOfDataFromFile_ != containerOfLinesOfData.end( );
          iteratorContainerOfDataFromFile_++ )
    {
        // Loop through all the characters in the string.
        for ( iteratorString_ = iteratorContainerOfDataFromFile_->second.begin( );
              iteratorString_ != iteratorContainerOfDataFromFile_->second.end( );
              iteratorString_++ )
        {
            // Check if end-of-line characters are present in string.
            // The end-of-line characters are checked for their integer
            // equivalents. See: http://www.asciitable.com/.
            if ( static_cast< int >( *iteratorString_ ) == 10
                 || static_cast< int >( *iteratorString_ ) == 13 )
            {
                // Strip end-of-line character from string.
                iteratorContainerOfDataFromFile_->second.erase( iteratorString_ );

                // Decrement string iterator since character was erased from string.
                iteratorString_--;
            }
        }
    }
}

//! Convert and store TLE data.
void TwoLineElementsTextFileReader::storeTwoLineElementData( )
{
    using boost::algorithm::trim_copy;

    // Strip End-Of-Line characters from data container.
    stripEndOfLineCharacters( containerOfDataFromFile_ );

    // Calculate number of objects in catalog file.
    numberOfObjects_ = static_cast< int >( lineCounter_ / numberOfLinesPerTwoLineElementDatum_ );

    // Set TLE data vector size.
    twoLineElementData_.resize( numberOfObjects_ );

    // Create vector of the three lines of a single object's TLE data as strings.
    vector< string > twoLineElementString_( 3 );

    // Create the object counter.
    unsigned int objectNumberCounter_ = 0;

    // Declare Keplerian elements variables.
    double inclination_;
    double rightAscensionOfAscendingNode_;
    double eccentricity_;
    double argumentOfPerigee_;
    double meanMotion_;

    // Declare approximate number of revolutions, remainder, and lost part.
    int approximateNumberOfRevolutions_;
    int approximateNumberOfRevolutionsRemainder_;
    int lostNumberOfRevolutions_;

    // Reference: Table 2 in (Vallado, D.A., et al., 2006).
    const double earthWithWorldGeodeticSystem72GravitationalParameter = 398600.8e9;

    // For every 3 lines, read the data from 3 consecutive strings of TLE data
    // and convert them to the TLE data variables
    for ( unsigned int i = 1; i < lineCounter_; i += numberOfLinesPerTwoLineElementDatum_ )
    {
        // General setup for variable storing.
        //---------------------------------------------------------------------

        // Fill the vector with the line strings from the data container.
        if ( numberOfLinesPerTwoLineElementDatum_ == 3 )
        {
            twoLineElementString_.at( 0 ) = containerOfDataFromFile_.at( i );
            twoLineElementString_.at( 1 ) = containerOfDataFromFile_.at( i + 1 );
            twoLineElementString_.at( 2 ) = containerOfDataFromFile_.at( i + 2 );
        }

        else if ( numberOfLinesPerTwoLineElementDatum_ == 2 )
        {
            twoLineElementString_.at( 1 ) = containerOfDataFromFile_.at( i );
            twoLineElementString_.at( 2 ) = containerOfDataFromFile_.at( i + 1 );
        }

        // Push the TLE strings to the TLE data container.
        twoLineElementData_[ objectNumberCounter_ ].twoLineElementStrings = twoLineElementString_;

        // Push the line numbers to the TLE data container.
        twoLineElementData_[ objectNumberCounter_ ].lineNumbers.push_back( i );
        twoLineElementData_[ objectNumberCounter_ ].lineNumbers.push_back( i + 1 );
        if ( numberOfLinesPerTwoLineElementDatum_ == 3 )
        {
            twoLineElementData_[ objectNumberCounter_ ].lineNumbers.push_back( i + 2 );
        }

        // Initiate a stringstream for each line.
        stringstream line0StringStream_( stringstream::in | stringstream::out );
        stringstream line1StringStream_( stringstream::in | stringstream::out );
        stringstream line2StringStream_( stringstream::in | stringstream::out );

        // Insert the strings into the stringstreams
        if ( numberOfLinesPerTwoLineElementDatum_ == 3 )
        {
            line0StringStream_ << twoLineElementString_.at( 0 );
        }
        line1StringStream_ << twoLineElementString_.at( 1 );
        line2StringStream_ << twoLineElementString_.at( 2 );

        // Line-0 variable storing.
        //---------------------------------------------------------------------
        if ( numberOfLinesPerTwoLineElementDatum_ == 3 )
        {
            // Declare string containing part of name of object.
            string namePart_;

            // Loop through the stringstream and read words that constitute name of
            // object. Store name parts in objectName storage container.
            while ( line0StringStream_ >> namePart_ )
            {
                twoLineElementData_[ objectNumberCounter_ ].objectName.push_back( namePart_ );
            }

            // Insert the entire line-0 string in the object name string.
            twoLineElementData_[ objectNumberCounter_ ].objectNameString
                    = twoLineElementString_.at( 0 );
        }

        // Line-1 variable storing.
        //---------------------------------------------------------------------
        // Fill all line-1 variables of the object structure with substrings
        // of the line-1 string using the template function to transform the
        // string to another type.
        // See reference for which columns are assigned to which variable.

        // Get line number integer of line-1 from string.
        twoLineElementData_[ objectNumberCounter_ ].lineNumberLine1  =
                boost::lexical_cast<unsigned int>(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 0, 1 ) ) );

        // Get object indentification number integer of line-1 from string
        twoLineElementData_[ objectNumberCounter_ ].objectIdentificationNumber =
                boost::lexical_cast<unsigned int>(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 2, 5 ) ) );

        // Get classification character from string.
        twoLineElementData_[ objectNumberCounter_ ].tleClassification =
                twoLineElementString_.at( 1 ) [ 7 ];

        // Get launch year integer from string.
        twoLineElementData_[ objectNumberCounter_ ].launchYear =
                boost::lexical_cast< unsigned int >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 9, 2 ) ) );

        // Calculate four-digit launch year from the above.
        if ( twoLineElementData_[ objectNumberCounter_ ].launchYear > 56 )
        {
            twoLineElementData_[ objectNumberCounter_ ].fourDigitlaunchYear =
                    twoLineElementData_[ objectNumberCounter_ ].launchYear + 1900;
        }

        else
        {
            twoLineElementData_[ objectNumberCounter_ ].fourDigitlaunchYear =
                    twoLineElementData_[ objectNumberCounter_ ].launchYear + 2000;
        }

        // Get launch number integer from string.
        twoLineElementData_[ objectNumberCounter_ ].launchNumber =
                boost::lexical_cast< unsigned int >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 11, 3 ) ) );

        // Get launch part string from string.
        twoLineElementData_[ objectNumberCounter_ ].launchPart =
                twoLineElementString_.at( 1 ).substr( 14, 3 );

        // Get epoch year integer from string.
        twoLineElementData_[ objectNumberCounter_ ].epochYear =
                boost::lexical_cast< unsigned int >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 18, 2 ) ) );

        // Calculate four-digit epoch year from the above.
        if ( twoLineElementData_[ objectNumberCounter_ ].epochYear > 56 )
        {
            twoLineElementData_[ objectNumberCounter_ ].fourDigitEpochYear =
                    twoLineElementData_[ objectNumberCounter_ ].epochYear + 1900;
        }

        else
        {
            twoLineElementData_[ objectNumberCounter_ ].fourDigitEpochYear =
                    twoLineElementData_[ objectNumberCounter_ ].epochYear + 2000;
        }

        // Get epoch day double from string.
        twoLineElementData_[ objectNumberCounter_ ].epochDay =
                boost::lexical_cast< double >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 20, 12 ) ) );

        // Get "first-derivative of mean motion divided by two" double from string.
        twoLineElementData_[ objectNumberCounter_].firstDerivativeOfMeanMotionDividedByTwo =
                boost::lexical_cast< double >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 33, 10 ) ) );

        // Get coefficient of scientific notation of "second-derivative of mean motion divided
        // by six" double from string,
        // Apply implied leading decimal point.
        twoLineElementData_[ objectNumberCounter_ ]
                .coefficientOfSecondDerivativeOfMeanMotionDividedBySix =
                boost::lexical_cast< double >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 44, 6 ) ) ) / 100000.0;

        // Get exponent of scientific notation of "second-derivative of mean motion divided
        // by six" integer from string.
        twoLineElementData_[ objectNumberCounter_]
                .exponentOfSecondDerivativeOfMeanMotionDividedBySix =
                boost::lexical_cast< double >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 50, 2 ) ) );

        // Calculate "second-derivative of mean motion divided by six" double from the above two.
        twoLineElementData_[ objectNumberCounter_ ].secondDerivativeOfMeanMotionDividedBySix =
                twoLineElementData_[ objectNumberCounter_]
                .coefficientOfSecondDerivativeOfMeanMotionDividedBySix
                * pow( 10, twoLineElementData_[ objectNumberCounter_ ]
                       .exponentOfSecondDerivativeOfMeanMotionDividedBySix );

        // Get coefficient of scientific notation of "B* divided by six" double
        // from string; apply implied leading decimal point.
        twoLineElementData_[ objectNumberCounter_ ].coefficientOfBStar =
                boost::lexical_cast< double >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 53, 6 ) ) ) /
                100000.0;

        // Get exponent of scientific notation of B* integer from string
        twoLineElementData_[ objectNumberCounter_ ].exponentOfBStar =
                boost::lexical_cast< int >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 59, 2 ) ) );

        // Calculate B* double from the above two.
        twoLineElementData_[ objectNumberCounter_ ].bStar =
                twoLineElementData_[ objectNumberCounter_ ].coefficientOfBStar
                * pow( 10.0, twoLineElementData_[ objectNumberCounter_ ].exponentOfBStar );

        // Get orbital model integer from string.
        twoLineElementData_[ objectNumberCounter_ ].orbitalModel =
                boost::lexical_cast< unsigned int >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 62, 1 ) ) );

        // Get TLE number integer from string.
        twoLineElementData_[ objectNumberCounter_ ].tleNumber =
                boost::lexical_cast< unsigned int >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 64, 4 ) ) );

        // Get modulo-10 checksum integer from string.
        twoLineElementData_[ objectNumberCounter_ ].modulo10CheckSumLine1 =
                boost::lexical_cast< unsigned int >(
                    trim_copy( twoLineElementString_.at( 1 ).substr( 68, 1 ) ) );

        // Line-2 variable storing.
        //---------------------------------------------------------------------
        // Fill all line-2 variables of the object structure partly with the
        // stringstream and partly with with substrings of the line-2 string
        // using the template function to transform the string to another type.
        // See reference for which columns are assigned to which variable.

        // Get line number integer of line-2 from stringstream.
        line2StringStream_ >> twoLineElementData_[ objectNumberCounter_ ].lineNumberLine2;

        // Get object identification number integer of line-2 from stringstream.
        line2StringStream_ >> twoLineElementData_[ objectNumberCounter_ ]
                              .objectIdentificationNumberLine2;

        // Get inclination double from stringstream.
        line2StringStream_ >> inclination_;
        twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                    orbital_element_conversions::inclinationIndex ) = inclination_;

        // Get right ascension of ascending node double from stringstream.
        line2StringStream_ >> rightAscensionOfAscendingNode_;
        twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                    orbital_element_conversions::longitudeOfAscendingNodeIndex )
                = rightAscensionOfAscendingNode_;

        // Get eccetricity double from stringstream.
        line2StringStream_ >> eccentricity_;
        eccentricity_ /= 10000000;
        twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                    orbital_element_conversions::eccentricityIndex ) = eccentricity_;

        // Get argument of perigee double from stringstream
        line2StringStream_ >> argumentOfPerigee_;
        twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                    orbital_element_conversions::argumentOfPeriapsisIndex ) = argumentOfPerigee_;

        // Get mean anomaly double from stringstream.
        line2StringStream_ >> twoLineElementData_[ objectNumberCounter_ ].meanAnomaly;

        // Get mean motion double from line-2 string.
        twoLineElementData_[ objectNumberCounter_ ].meanMotionInRevolutionsPerDay =
                boost::lexical_cast< double >(
                    trim_copy( twoLineElementString_[ 2 ].substr( 52, 11 ) ) );

        // Get revolution number integer from line-2 string.
        twoLineElementData_[ objectNumberCounter_ ].revolutionNumber =
                boost::lexical_cast< int >(
                    trim_copy( twoLineElementString_[ 2 ].substr( 63, 5 ) ) );

        // Get modulo-10 checksum integer of line-2 from line-2 string.
        twoLineElementData_[ objectNumberCounter_ ].modulo10CheckSumLine2 =
                boost::lexical_cast< unsigned int >(
                    trim_copy( twoLineElementString_[ 2 ].substr( 68, 1 ) ) );

        // Calculate the approximate total number of revolutions, as the counter resets to 0 after
        // passing by 99999, insert the current year in the following equation.
        approximateNumberOfRevolutions_ = twoLineElementData_[ objectNumberCounter_ ]
                .meanMotionInRevolutionsPerDay
                * ( currentYear_
                    - twoLineElementData_[ objectNumberCounter_ ].fourDigitlaunchYear )
                * physical_constants::JULIAN_YEAR_IN_DAYS;
        approximateNumberOfRevolutionsRemainder_ = approximateNumberOfRevolutions_ % 100000;
        lostNumberOfRevolutions_ = approximateNumberOfRevolutions_
                - approximateNumberOfRevolutionsRemainder_;

        // Check if the counter has been reset after passing 99999.
        if ( ( twoLineElementData_[ objectNumberCounter_ ].revolutionNumber -
               approximateNumberOfRevolutionsRemainder_ ) <=
             ( approximateNumberOfRevolutionsRemainder_ + 100000 -
               twoLineElementData_[ objectNumberCounter_ ].revolutionNumber ) )
        {
            twoLineElementData_[ objectNumberCounter_ ].totalRevolutionNumber =
                    lostNumberOfRevolutions_ + twoLineElementData_[ objectNumberCounter_ ]
                    .revolutionNumber;
        }

        else
        {
            twoLineElementData_[ objectNumberCounter_ ].totalRevolutionNumber =
                    lostNumberOfRevolutions_ - 100000
                    + twoLineElementData_[ objectNumberCounter_ ].revolutionNumber;
        }

        // Check if total number of revolutions is negative.
        if ( twoLineElementData_[ objectNumberCounter_ ].totalRevolutionNumber < 0 )
        {
            twoLineElementData_[ objectNumberCounter_ ].totalRevolutionNumber =
                    twoLineElementData_[ objectNumberCounter_ ].revolutionNumber;
        }

        // Semi-major axis of the object is calculated from the other TLE variables.
        meanMotion_ = twoLineElementData_[ objectNumberCounter_ ].meanMotionInRevolutionsPerDay
                * 2.0 * PI / physical_constants::JULIAN_DAY;
        twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                    orbital_element_conversions::semiMajorAxisIndex )
                = orbital_element_conversions::
                convertEllipticalMeanMotionToSemiMajorAxis(
                    meanMotion_, earthWithWorldGeodeticSystem72GravitationalParameter );

        // Perigee of the object is calculated from the other TLE variables.
        twoLineElementData_[ objectNumberCounter_ ].perigee =
                twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                    orbital_element_conversions::semiMajorAxisIndex )
                * ( 1.0 - twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                        orbital_element_conversions::eccentricityIndex ) );

        // Apogee of the object is calculated from the other TLE variables.
        twoLineElementData_[ objectNumberCounter_ ].apogee =
                twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                    orbital_element_conversions::semiMajorAxisIndex )
                * ( 1.0 + twoLineElementData_[ objectNumberCounter_ ].TLEKeplerianElements(
                        orbital_element_conversions::eccentricityIndex ) );

        // Increment object number counter by one.
        objectNumberCounter_++;
    }
}

//! Checks the integrity of the TLE input file.
multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileIntegrity( )
{
    // Create vector of the three lines of a TLE as strings
    vector< string > twoLineElementString_( 3 );

    // Boolean which turns to true if one of the tests is not passed.
    bool isObjectErroneous = false;

    // Vector of object numbers of objects with corrupted TLE data.
    vector< int > corruptedTwoLineElementDataPositions_;

    // Multimap of corrupted TLE errors.
    multimap< int, string > corruptedTwoLineElementDataErrors_;

    // Loop is started over the entire structure of objects,
    // multiple checks are performed per TLE, if a check is not passed boolean
    // is set to true for that object, and amount of corrupted TLEs integer is
    // incremented.
    for ( unsigned int i = 0; i < numberOfObjects_; i++ )
    {
        // Fill the vector with the line strings from the data container.
        if ( numberOfLinesPerTwoLineElementDatum_ == 3 )
        {
            twoLineElementString_.at( 1 ) = containerOfDataFromFile_[ i + 1 + ( i * 2 ) + 1 ];
            twoLineElementString_.at( 2 ) = containerOfDataFromFile_[ i + 1 + ( i * 2 ) + 2 ];
        }

        else if ( numberOfLinesPerTwoLineElementDatum_ == 2 )
        {
            twoLineElementString_.at( 1 ) = containerOfDataFromFile_[ 2 * i + 1 ];
            twoLineElementString_.at( 2 ) = containerOfDataFromFile_[ 2 * i + 1 + 1 ];
        }

        // Set boolean to false.
        isObjectErroneous = false;

        // If a line-1 line number is not 1, error message is given and boolean is set to true.
        if ( twoLineElementData_.at( i ).lineNumberLine1 != 1 )
        {
            corruptedTwoLineElementDataErrors_.insert(
                        pair< int, string >( i, "Incorrect line-1 leading integer." ) );

            isObjectErroneous = true;
        }

        // If a line-2 line number is not 2, error message is given and boolean is set to true.
        if ( twoLineElementData_.at( i ).lineNumberLine2 != 2 )
        {
            corruptedTwoLineElementDataErrors_.insert(
                        pair< int, string >( i, "Incorrect line-2 leading integer." ) );

            isObjectErroneous = true;
        }

        if ( twoLineElementData_.at( i ).tleClassification != 'U'
                  && twoLineElementData_.at( i ).tleClassification != 'C' )
        {
            corruptedTwoLineElementDataErrors_.insert(
                        pair< int, string >( i, "Invalid TLE classification." ) );

            isObjectErroneous = true;
        }

        // Check if orbital model is 0, else error message is given and boolean is set to true.
        if ( twoLineElementData_.at( i ).orbitalModel != 0 )
        {
            corruptedTwoLineElementDataErrors_.insert(
                        pair< int, string >( i, "Incorrect orbital model." ) );

            isObjectErroneous = true;
        }

        // Line-1 modulo-10 checker, add all integers of line 1 together,
        // and minus signs count as 1, rest is not counted.

        // Create calculated modulo-10 checksum of line 1 and temporary integer for addition.
        unsigned int line1Modulo10Sum_ = 0;

        // Run loop over the 69-character long, line-2 string, but not the last character,
        // since this is the modulo-10 checksum itself and is not added.
        for ( unsigned int j = 0; j < 68; j++ )
        {
            // Check if the char is not a space or a dot, or a plus or a minus sign or any of the
            // alphabetical positions, because these can not be added.
            if ( twoLineElementString_.at( 1 )[ j ] != ' ' &&
                 twoLineElementString_.at( 1 )[ j ] != '.' &&
                 twoLineElementString_.at( 1 )[ j ] != '+' &&
                 twoLineElementString_.at( 1 )[ j ] != '-' &&
                 j != 7 && j != 14 && j != 15 && j != 16 )
            {
                // Add int to checksum.
                line1Modulo10Sum_ += boost::lexical_cast<int>(
                            twoLineElementString_.at( 1 ).substr( j , 1 ) );
            }

            // Check is the char is a minus sign
            else if ( twoLineElementString_.at( 1 )[ j ] == '-' )
            {
                // Add 1 to checksum.
                line1Modulo10Sum_++;
            }
        }

        // Take modulo 10 of checksum.
        line1Modulo10Sum_ %= 10;

        // Comparison of calculated modulo-10 checksum with modulo-10 checksum from TLE for line 1,
        // if they do not compare, error message is given and boolean is set to true.
        if ( line1Modulo10Sum_ != twoLineElementData_.at( i ).modulo10CheckSumLine1 )
        {
            corruptedTwoLineElementDataErrors_.insert(
                        pair< int, string >( i, "Incorrect line-1 modulo-10 checksum." ) );

            isObjectErroneous = true;
        }

        // Line-2 modulo-10 checker, add all integers of line 2 together.
        unsigned int line2Modulo10Sum_ = 0;

        // Run loop over the 69-character long, line-2 string, but not the last character,
        // since this is the modulo-10 checksum itself and is not added.
        for ( unsigned int k = 0; k < 68; k++ )
        {
            // Check if the char is not a space or a dot, because these can not be added.
            if ( twoLineElementString_.at( 2 )[ k ] != ' ' &&
                 twoLineElementString_.at( 2 )[ k ] != '.' )
            {
                // Add int to checksum.
                line2Modulo10Sum_ += boost::lexical_cast<int>(
                            twoLineElementString_[ 2 ].substr( k , 1 ) );
            }
        }

        // Take modulo 10 of checksum.
        line2Modulo10Sum_ %= 10;

        // Comparison of calculated modulo-10 checksum with modulo-10 checksum from TLE for line 2,
        // if they do not compare error message is given and boolean is set to true.
        if ( line2Modulo10Sum_ != twoLineElementData_.at( i ).modulo10CheckSumLine2 )
        {
            corruptedTwoLineElementDataErrors_.insert(
                        pair< int, string >( i, "Incorrect line-2 modulo-10 checksum." ) );

            isObjectErroneous = true;
        }

        // When the object's line-1 and line-2 identification numbers do not match an
        // error message is given and boolean is set to true.
        if ( twoLineElementData_.at( i ).objectIdentificationNumber !=
             twoLineElementData_.at( i ).objectIdentificationNumberLine2 )
        {
            corruptedTwoLineElementDataErrors_.insert(
                        pair< int, string >(
                            i, "Line-1 and line-2 object idenfitication number mismatch." ) );

            isObjectErroneous = true;
        }

        // If object is erroneous, add to list.
        if ( isObjectErroneous )
        {
            corruptedTwoLineElementDataPositions_.push_back( i );
        }
    }

    // Erase all the corrupted TLEs from the TLE data vector.

    for ( unsigned int q = 0; q < corruptedTwoLineElementDataPositions_.size( ); q++ )
    {
        twoLineElementData_.erase( twoLineElementData_.begin( ) + (
                                   corruptedTwoLineElementDataPositions_.at( q ) - q ) );
        numberOfObjects_--;
    }

    // Return the amount of corrupted TLEs
    return corruptedTwoLineElementDataErrors_;
}

} // namespace input_output
} // namespace tudat