// main
int main(int argc, char** argv) {

  // load Plaza1 data
  list<TimedOdometry> odometry = readOdometry();
//  size_t M = odometry.size();

  vector<RangeTriple> triples = readTriples();
  size_t K = triples.size();

  // parameters
  size_t start = 220, end=3000;
  size_t minK = 100; // first batch of smart factors
  size_t incK = 50; // minimum number of range measurements to process after
  bool robust = true;
  bool smart = true;

  // Set Noise parameters
  Vector priorSigmas = Vector3(1, 1, M_PI);
  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
  double sigmaR = 100; // range standard deviation
  const NM::Base::shared_ptr // all same type
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
  rangeNoise = robust ? tukey : gaussian;

  // Initialize iSAM
  ISAM2 isam;

  // Add prior on first pose
  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
      M_PI - 2.02108900000000);
  NonlinearFactorGraph newFactors;
  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));

  ofstream os2(
      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
  ofstream os3(
      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");

  //  initialize points (Gaussian)
  Values initial;
  if (!smart) {
    initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
    initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
    initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
    initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
  }
  Values landmarkEstimates = initial; // copy landmarks
  initial.insert(0, pose0);

  //  initialize smart range factors
  size_t ids[] = { 1, 6, 0, 5 };
  typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
  map<size_t, SmartPtr> smartFactors;
  if (smart) {
    for(size_t jj: ids) {
      smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
      newFactors.push_back(smartFactors[jj]);
    }
  }

  // set some loop variables
  size_t i = 1; // step counter
  size_t k = 0; // range measurement counter
  Pose2 lastPose = pose0;
  size_t countK = 0, totalCount=0;

  // Loop over odometry
  gttic_(iSAM);
  for(const TimedOdometry& timedOdometry: odometry) {
    //--------------------------------- odometry loop -----------------------------------------
    double t;
    Pose2 odometry;
    boost::tie(t, odometry) = timedOdometry;

    // add odometry factor
    newFactors.push_back(
        BetweenFactor<Pose2>(i - 1, i, odometry,
            NM::Diagonal::Sigmas(odoSigmas)));

    // predict pose and add as initial estimate
    Pose2 predictedPose = lastPose.compose(odometry);
    lastPose = predictedPose;
    initial.insert(i, predictedPose);
    landmarkEstimates.insert(i, predictedPose);

    // Check if there are range factors to be added
    while (k < K && t >= boost::get<0>(triples[k])) {
      size_t j = boost::get<1>(triples[k]);
      double range = boost::get<2>(triples[k]);
      if (i > start) {
        if (smart && totalCount < minK) {
          smartFactors[j]->addRange(i, range);
          printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
        }
        else {
          RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
              rangeNoise);
          // Throw out obvious outliers based on current landmark estimates
          Vector error = factor.unwhitenedError(landmarkEstimates);
          if (k <= 200 || fabs(error[0]) < 5)
            newFactors.push_back(factor);
        }
        totalCount += 1;
      }
      k = k + 1;
      countK = countK + 1;
    }

    // Check whether to update iSAM 2
    if (k >= minK && countK >= incK) {
      gttic_(update);
      isam.update(newFactors, initial);
      gttoc_(update);
      gttic_(calculateEstimate);
      Values result = isam.calculateEstimate();
      gttoc_(calculateEstimate);
      lastPose = result.at<Pose2>(i);
      bool hasLandmarks = result.exists(symbol('L', ids[0]));
      if (hasLandmarks) {
        // update landmark estimates
        landmarkEstimates = Values();
        for(size_t jj: ids)
          landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj)));
      }
      newFactors = NonlinearFactorGraph();
      initial = Values();
      if (smart && !hasLandmarks) {
        cout << "initialize from smart landmarks" << endl;
        for(size_t jj: ids) {
          Point2 landmark = smartFactors[jj]->triangulate(result);
          initial.insert(symbol('L', jj), landmark);
          landmarkEstimates.insert(symbol('L', jj), landmark);
        }
      }
      countK = 0;
      for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
        os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
            << endl;
      if (smart) {
        for(size_t jj: ids) {
          Point2 landmark = smartFactors[jj]->triangulate(result);
          os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
              << endl;
        }
      }
    }
    i += 1;
    if (i>end) break;
    //--------------------------------- odometry loop -----------------------------------------
  } // end for
  gttoc_(iSAM);

  // Print timings
  tictoc_print_();

  // Write result to file
  Values result = isam.calculateEstimate();
  ofstream os(
      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
  for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
    os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
        << it.value.theta() << endl;
  exit(0);
}
// main
int main (int argc, char** argv) {

  // load Plaza2 data
  list<TimedOdometry> odometry = readOdometry();
//  size_t M = odometry.size();

  vector<RangeTriple> triples = readTriples();
  size_t K = triples.size();

  // parameters
  size_t minK = 150; // minimum number of range measurements to process initially
  size_t incK = 25; // minimum number of range measurements to process after
  bool groundTruth = false;
  bool robust = true;

  // Set Noise parameters
  Vector priorSigmas = Vector3(1,1,M_PI);
  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
  double sigmaR = 100; // range standard deviation
  const NM::Base::shared_ptr // all same type
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
  rangeNoise = robust ? tukey : gaussian;

  // Initialize iSAM
  ISAM2 isam;

  // Add prior on first pose
  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
      M_PI - 2.02108900000000);
  NonlinearFactorGraph newFactors;
  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
  Values initial;
  initial.insert(0, pose0);

  //  initialize points
  if (groundTruth) { // from TL file
    initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
    initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
    initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
    initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
  } else { // drawn from sigma=1 Gaussian in matlab version
    initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
    initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
    initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
    initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
  }

  // set some loop variables
  size_t i = 1; // step counter
  size_t k = 0; // range measurement counter
  bool initialized = false;
  Pose2 lastPose = pose0;
  size_t countK = 0;

  // Loop over odometry
  gttic_(iSAM);
  BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
    //--------------------------------- odometry loop -----------------------------------------
    double t;
    Pose2 odometry;
    boost::tie(t, odometry) = timedOdometry;

    // add odometry factor
    newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));

    // predict pose and add as initial estimate
    Pose2 predictedPose = lastPose.compose(odometry);
    lastPose = predictedPose;
    initial.insert(i, predictedPose);

    // Check if there are range factors to be added
    while (k < K && t >= boost::get<0>(triples[k])) {
      size_t j = boost::get<1>(triples[k]);
      double range = boost::get<2>(triples[k]);
      newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
      k = k + 1;
      countK = countK + 1;
    }

    // Check whether to update iSAM 2
    if ((k > minK) && (countK > incK)) {
      if (!initialized) { // Do a full optimize for first minK ranges
        gttic_(batchInitialization);
        LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
        initial = batchOptimizer.optimize();
        gttoc_(batchInitialization);
        initialized = true;
      }
      gttic_(update);
      isam.update(newFactors, initial);
      gttoc_(update);
      gttic_(calculateEstimate);
      Values result = isam.calculateEstimate();
      gttoc_(calculateEstimate);
      lastPose = result.at<Pose2>(i);
      newFactors = NonlinearFactorGraph();
      initial = Values();
      countK = 0;
    }
    i += 1;
    //--------------------------------- odometry loop -----------------------------------------
  } // BOOST_FOREACH