/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    ISAM2-inl.h
 * @brief   Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
 * @author  Michael Kaess, Richard Roberts
 */

#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/algorithm/string.hpp>

#include <gtsam/base/timing.h>
#include <gtsam/base/debug.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>

#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/nonlinear/nonlinearExceptions.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>

namespace gtsam {

using namespace std;

static const bool disableReordering = false;
static const double batchThreshold = 0.65;

/* ************************************************************************* */
std::string ISAM2DoglegParams::adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const {
  std::string s;
  switch (adaptationMode) {
  case DoglegOptimizerImpl::SEARCH_EACH_ITERATION:   s = "SEARCH_EACH_ITERATION";  break;
  case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION:  s = "ONE_STEP_PER_ITERATION"; break;
  default:                                           s = "UNDEFINED";              break;
  }
  return s;
}

/* ************************************************************************* */
DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator(const std::string& adaptationMode) const {
  std::string s = adaptationMode;  boost::algorithm::to_upper(s);
  if (s == "SEARCH_EACH_ITERATION")  return DoglegOptimizerImpl::SEARCH_EACH_ITERATION;
  if (s == "ONE_STEP_PER_ITERATION") return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION;

  /* default is SEARCH_EACH_ITERATION */
  return DoglegOptimizerImpl::SEARCH_EACH_ITERATION;
}

/* ************************************************************************* */
ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) const {
  std::string s = str;  boost::algorithm::to_upper(s);
  if (s == "QR") return ISAM2Params::QR;
  if (s == "CHOLESKY") return ISAM2Params::CHOLESKY;

  /* default is CHOLESKY */
  return ISAM2Params::CHOLESKY;
}

/* ************************************************************************* */
std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) const {
  std::string s;
  switch (value) {
  case ISAM2Params::QR:         s = "QR"; break;
  case ISAM2Params::CHOLESKY:   s = "CHOLESKY"; break;
  default:                      s = "UNDEFINED"; break;
  }
  return s;
}

/* ************************************************************************* */
ISAM2::ISAM2(const ISAM2Params& params):
    deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
  if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
    doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}

/* ************************************************************************* */
ISAM2::ISAM2():
    deltaDoglegUptodate_(true), deltaUptodate_(true) {
  if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
    doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}

/* ************************************************************************* */
ISAM2::ISAM2(const ISAM2& other) {
  *this = other;
}

/* ************************************************************************* */
ISAM2& ISAM2::operator=(const ISAM2& rhs) {
  // Copy BayesTree
  this->Base::operator=(rhs);

  // Copy our variables
  // When we have Permuted<...>, it is only necessary to copy this permuted
  // view and not the original, because copying the permuted view automatically
  // copies the original.
  theta_ = rhs.theta_;
  variableIndex_ = rhs.variableIndex_;
  delta_ = rhs.delta_;
  deltaNewton_ = rhs.deltaNewton_;
  RgProd_ = rhs.RgProd_;
  deltaDoglegUptodate_ = rhs.deltaDoglegUptodate_;
  deltaUptodate_ = rhs.deltaUptodate_;
  deltaReplacedMask_ = rhs.deltaReplacedMask_;
  nonlinearFactors_ = rhs.nonlinearFactors_;

  linearFactors_ = GaussianFactorGraph();
  linearFactors_.reserve(rhs.linearFactors_.size());
  BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) {
    linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactor::shared_ptr()); }

  ordering_ = rhs.ordering_;
  params_ = rhs.params_;
  doglegDelta_ = rhs.doglegDelta_;

  lastAffectedVariableCount = rhs.lastAffectedVariableCount;
  lastAffectedFactorCount = rhs.lastAffectedFactorCount;
  lastAffectedCliqueCount = rhs.lastAffectedCliqueCount;
  lastAffectedMarkedCount = rhs.lastAffectedMarkedCount;
  lastBacksubVariableCount = rhs.lastBacksubVariableCount;
  lastNnzTop = rhs.lastNnzTop;

  return *this;
}

/* ************************************************************************* */
FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
  static const bool debug = false;
  if(debug) cout << "Getting affected factors for ";
  if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
  if(debug) cout << endl;

  FactorGraph<NonlinearFactor > allAffected;
  FastList<size_t> indices;
  BOOST_FOREACH(const Index key, keys) {
//    const list<size_t> l = nonlinearFactors_.factors(key);
//    indices.insert(indices.begin(), l.begin(), l.end());
    const VariableIndex::Factors& factors(variableIndex_[key]);
    BOOST_FOREACH(size_t factor, factors) {
      if(debug) cout << "Variable " << key << " affects factor " << factor << endl;
      indices.push_back(factor);
    }
  }
  indices.sort();
  indices.unique();
  if(debug) cout << "Affected factors are: ";
  if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } }
  if(debug) cout << endl;
  return indices;
}

/* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
FactorGraph<GaussianFactor>::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const {

  gttic(getAffectedFactors);
  FastList<size_t> candidates = getAffectedFactors(affectedKeys);
  gttoc(getAffectedFactors);

  NonlinearFactorGraph nonlinearAffectedFactors;

  gttic(affectedKeysSet);
  // for fast lookup below
  FastSet<Index> affectedKeysSet;
  affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
  gttoc(affectedKeysSet);

  gttic(check_candidates_and_linearize);
  FactorGraph<GaussianFactor>::shared_ptr linearized = boost::make_shared<FactorGraph<GaussianFactor> >();
  BOOST_FOREACH(size_t idx, candidates) {
    bool inside = true;
    bool useCachedLinear = params_.cacheLinearizedFactors;
    BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
      Index var = ordering_[key];
      if(affectedKeysSet.find(var) == affectedKeysSet.end()) {
        inside = false;
        break;
      }
      if(useCachedLinear && relinKeys.find(var) != relinKeys.end())
        useCachedLinear = false;
    }
    if(inside) {
      if(useCachedLinear) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
        assert(linearFactors_[idx]);
        assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys());
#endif
        linearized->push_back(linearFactors_[idx]);
      } else {
        GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_);
        linearized->push_back(linearFactor);
        if(params_.cacheLinearizedFactors) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
          assert(linearFactors_[idx]->keys() == linearFactor->keys());
#endif
          linearFactors_[idx] = linearFactor;
        }
      }
    }
  }
  gttoc(check_candidates_and_linearize);

  return linearized;
}

/* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area
GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {

  static const bool debug = false;

  GaussianFactorGraph cachedBoundary;

  BOOST_FOREACH(sharedClique orphan, orphans) {
    // find the last variable that was eliminated
    Index key = (*orphan)->frontals().back();
    // retrieve the cached factor and add to boundary
    cachedBoundary.push_back(orphan->cachedFactor());
    if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); }
  }

  return cachedBoundary;
}

boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& markedKeys,
    const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices,
    const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {

  // TODO:  new factors are linearized twice, the newFactors passed in are not used.

  const bool debug = ISDEBUG("ISAM2 recalculate");

  // Input: BayesTree(this), newFactors

//#define PRINT_STATS // figures for paper, disable for timing
#ifdef PRINT_STATS
  static int counter = 0;
  int maxClique = 0;
  double avgClique = 0;
  int numCliques = 0;
  int nnzR = 0;
  if (counter>0) { // cannot call on empty tree
    GaussianISAM2_P::CliqueData cdata =  this->getCliqueData();
    GaussianISAM2_P::CliqueStats cstats = cdata.getStats();
    maxClique = cstats.maxCONDITIONALSize;
    avgClique = cstats.avgCONDITIONALSize;
    numCliques = cdata.conditionalSizes.size();
    nnzR = calculate_nnz(this->root());
  }
  counter++;
#endif

  if(debug) {
    cout << "markedKeys: ";
    BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; }
    cout << endl;
    cout << "observedKeys: ";
    BOOST_FOREACH(const Index key, observedKeys) { cout << key << " "; }
    cout << endl;
  }

  // 1. Remove top of Bayes tree and convert to a factor graph:
  // (a) For each affected variable, remove the corresponding clique and all parents up to the root.
  // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques.
  gttic(removetop);
  Cliques orphans;
  BayesNet<GaussianConditional> affectedBayesNet;
  this->removeTop(markedKeys, affectedBayesNet, orphans);
  gttoc(removetop);

  if(debug) affectedBayesNet.print("Removed top: ");
  if(debug) orphans.print("Orphans: ");

  //    FactorGraph<GaussianFactor> factors(affectedBayesNet);
  // bug was here: we cannot reuse the original factors, because then the cached factors get messed up
  // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries,
  //  so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't
  //  contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose
  //  not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected
  //  in the cached_ values which again will be wrong]
  // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary

  // BEGIN OF COPIED CODE

  // ordering provides all keys in conditionals, there cannot be others because path to root included
  gttic(affectedKeys);
  FastList<Index> affectedKeys = affectedBayesNet.ordering();
  gttoc(affectedKeys);

  boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>()); // Will return this result

  if(affectedKeys.size() >= theta_.size() * batchThreshold) {

    gttic(batch);

    gttic(add_keys);
    BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); }
    gttoc(add_keys);

    gttic(reorder);
    gttic(CCOLAMD);
    // Do a batch step - reorder and relinearize all variables
    vector<int> cmember(theta_.size(), 0);
    if(constrainKeys) {
      if(!constrainKeys->empty()) {
        typedef std::pair<const Index,int> Index_Group;
        if(theta_.size() > constrainKeys->size()) { // Only if some variables are unconstrained
          BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) {
            cmember[index_group.first] = index_group.second; }
        } else {
          int minGroup = *boost::range::min_element(boost::adaptors::values(*constrainKeys));
          BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) {
            cmember[index_group.first] = index_group.second - minGroup; }
        }
      }
    } else {
      if(theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained
        BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; }
      }
    }
    Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember));
    Permutation::shared_ptr colamdInverse(colamd->inverse());
    gttoc(CCOLAMD);

    // Reorder
    gttic(permute_global_variable_index);
    variableIndex_.permuteInPlace(*colamd);
    gttoc(permute_global_variable_index);
    gttic(permute_delta);
    delta_.permuteInPlace(*colamd);
    deltaNewton_.permuteInPlace(*colamd);
    RgProd_.permuteInPlace(*colamd);
    gttoc(permute_delta);
    gttic(permute_ordering);
    ordering_.permuteInPlace(*colamd);
    gttoc(permute_ordering);
    gttoc(reorder);

    gttic(linearize);
    GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_, ordering_);
    if(params_.cacheLinearizedFactors)
      linearFactors_ = linearized;
    gttoc(linearize);

    gttic(eliminate);
    JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearized, variableIndex_);
    sharedClique newRoot;
    if(params_.factorization == ISAM2Params::CHOLESKY)
      newRoot = jt.eliminate(EliminatePreferCholesky);
    else if(params_.factorization == ISAM2Params::QR)
      newRoot = jt.eliminate(EliminateQR);
    else assert(false);
    if(debug) newRoot->print("Eliminated: ");
    gttoc(eliminate);

    gttic(insert);
    this->clear();
    this->insert(newRoot);
    gttoc(insert);

    result.variablesReeliminated = affectedKeysSet->size();
    result.factorsRecalculated = nonlinearFactors_.size();

    lastAffectedMarkedCount = markedKeys.size();
    lastAffectedVariableCount = affectedKeysSet->size();
    lastAffectedFactorCount = linearized.size();

    // Reeliminated keys for detailed results
    if(params_.enableDetailedResults) {
      BOOST_FOREACH(Key key, theta_.keys()) {
        result.detail->variableStatus[key].isReeliminated = true;
      }
    }

    gttoc(batch);

  } else {

    gttic(incremental);

    // 2. Add the new factors \Factors' into the resulting factor graph
    FastList<Index> affectedAndNewKeys;
    affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end());
    affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end());
    gttic(relinearizeAffected);
    GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys));
    if(debug) factors.print("Relinearized factors: ");
    gttoc(relinearizeAffected);

    if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }

    // Reeliminated keys for detailed results
    if(params_.enableDetailedResults) {
      BOOST_FOREACH(Index index, affectedAndNewKeys) {
        result.detail->variableStatus[ordering_.key(index)].isReeliminated = true;
      }
    }

    result.variablesReeliminated = affectedAndNewKeys.size();
    result.factorsRecalculated = factors.size();
    lastAffectedMarkedCount = markedKeys.size();
    lastAffectedVariableCount = affectedKeys.size();
    lastAffectedFactorCount = factors.size();

#ifdef PRINT_STATS
    // output for generating figures
    cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size()
              << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique
              << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
#endif

    gttic(cached);
    // add the cached intermediate results from the boundary of the orphans ...
    GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans);
    if(debug) cachedBoundary.print("Boundary factors: ");
    factors.push_back(cachedBoundary);
    gttoc(cached);

    // END OF COPIED CODE

    // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree])

    gttic(reorder_and_eliminate);

    gttic(list_to_set);
    // create a partial reordering for the new and contaminated factors
    // markedKeys are passed in: those variables will be forced to the end in the ordering
    affectedKeysSet->insert(markedKeys.begin(), markedKeys.end());
    affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end());
    gttoc(list_to_set);

    gttic(PartialSolve);
    Impl::ReorderingMode reorderingMode;
    reorderingMode.nFullSystemVars = ordering_.size();
    reorderingMode.algorithm = Impl::ReorderingMode::COLAMD;
    reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST;
    if(constrainKeys) {
      reorderingMode.constrainedKeys = *constrainKeys;
    } else {
      reorderingMode.constrainedKeys = FastMap<Index,int>();
      BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
    }
    FastSet<Index> affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve
    BOOST_FOREACH(Index unused, unusedIndices) {
      affectedUsedKeys.erase(unused);
    }
    // Remove unaffected keys from the constraints
    FastMap<Index,int>::iterator iter = reorderingMode.constrainedKeys->begin();
    while(iter != reorderingMode.constrainedKeys->end()) {
      if(affectedUsedKeys.find(iter->first) == affectedUsedKeys.end()) {
        reorderingMode.constrainedKeys->erase(iter++);
      } else {
        ++iter;
      }
    }
    Impl::PartialSolveResult partialSolveResult =
        Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR));
    gttoc(PartialSolve);

    // We now need to permute everything according this partial reordering: the
    // delta vector, the global ordering, and the factors we're about to
    // re-eliminate.  The reordered variables are also mentioned in the
    // orphans and the leftover cached factors.
    gttic(permute_global_variable_index);
    variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
    gttoc(permute_global_variable_index);
    gttic(permute_delta);
    delta_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
    deltaNewton_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
    RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
    gttoc(permute_delta);
    gttic(permute_ordering);
    ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
    gttoc(permute_ordering);
    if(params_.cacheLinearizedFactors) {
      gttic(permute_cached_linear);
      //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
      FastList<size_t> permuteLinearIndices = getAffectedFactors(affectedAndNewKeys);
      BOOST_FOREACH(size_t idx, permuteLinearIndices) {
        linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse);
      }
      gttoc(permute_cached_linear);
    }

    gttoc(reorder_and_eliminate);

    gttic(reassemble);
    if(partialSolveResult.bayesTree) {
      assert(!this->root_);
      this->insert(partialSolveResult.bayesTree);
    }
    gttoc(reassemble);

    // 4. Insert the orphans back into the new Bayes tree.
    gttic(orphans);
    gttic(permute);
    BOOST_FOREACH(sharedClique orphan, orphans) {
      (void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse);
    }
    gttoc(permute);
    gttic(insert);
    // add orphans to the bottom of the new tree
    BOOST_FOREACH(sharedClique orphan, orphans) {
      // Because the affectedKeysSelector is sorted, the orphan separator keys
      // will be sorted correctly according to the new elimination order after
      // applying the permutation, so findParentClique, which looks for the
      // lowest-ordered parent, will still work.
      Index parentRepresentative = Base::findParentClique((*orphan)->parents());
      sharedClique parent = (*this)[parentRepresentative];
      parent->children_ += orphan;
      orphan->parent_ = parent; // set new parent!
    }
    gttoc(insert);
    gttoc(orphans);

    gttoc(incremental);
  }

  // Root clique variables for detailed results
  if(params_.enableDetailedResults) {
    BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) {
      result.detail->variableStatus[ordering_.key(index)].inRootClique = true;
    }
  }

  return affectedKeysSet;
}

/* ************************************************************************* */
ISAM2Result ISAM2::update(
    const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
    const boost::optional<FastMap<Key,int> >& constrainedKeys, const boost::optional<FastList<Key> >& noRelinKeys,
    const boost::optional<FastList<Key> >& extraReelimKeys, bool force_relinearize) {

  const bool debug = ISDEBUG("ISAM2 update");
  const bool verbose = ISDEBUG("ISAM2 update verbose");

  static int count = 0;
  count++;

  lastAffectedVariableCount = 0;
  lastAffectedFactorCount = 0;
  lastAffectedCliqueCount = 0;
  lastAffectedMarkedCount = 0;
  lastBacksubVariableCount = 0;
  lastNnzTop = 0;
  ISAM2Result result;
  if(params_.enableDetailedResults)
    result.detail = ISAM2Result::DetailedResults();
  const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0);

  if(verbose) {
    cout << "ISAM2::update\n";
    this->print("ISAM2: ");
  }

  // Update delta if we need it to check relinearization later
  if(relinearizeThisStep) {
    gttic(updateDelta);
    updateDelta(disableReordering);
    gttoc(updateDelta);
  }

  gttic(push_back_factors);
  // Add the new factor indices to the result struct
  result.newFactorsIndices.resize(newFactors.size());
  for(size_t i=0; i<newFactors.size(); ++i)
    result.newFactorsIndices[i] = i + nonlinearFactors_.size();

  // 1. Add any new factors \Factors:=\Factors\cup\Factors'.
  if(debug || verbose) newFactors.print("The new factors are: ");
  nonlinearFactors_.push_back(newFactors);

  // Remove the removed factors
  NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size());
  BOOST_FOREACH(size_t index, removeFactorIndices) {
    removeFactors.push_back(nonlinearFactors_[index]);
    nonlinearFactors_.remove(index);
    if(params_.cacheLinearizedFactors)
      linearFactors_.remove(index);
  }

  // Remove removed factors from the variable index so we do not attempt to relinearize them
  variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));

  // Compute unused keys and indices
  FastSet<Key> unusedKeys;
  FastSet<Index> unusedIndices;
  {
    // Get keys from removed factors and new factors, and compute unused keys,
    // i.e., keys that are empty now and do not appear in the new factors.
    FastSet<Key> removedAndEmpty;
    BOOST_FOREACH(Key key, removeFactors.keys()) {
      if(variableIndex_[ordering_[key]].empty())
        removedAndEmpty.insert(removedAndEmpty.end(), key);
    }
    FastSet<Key> newFactorSymbKeys = newFactors.keys();
    std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
      newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));

    // Get indices for unused keys
    BOOST_FOREACH(Key key, unusedKeys) {
      unusedIndices.insert(unusedIndices.end(), ordering_[key]);
    }
  }
  gttoc(push_back_factors);

  gttic(add_new_variables);
  // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
  Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_);
  // New keys for detailed results
  if(params_.enableDetailedResults) {
    BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } }
  gttoc(add_new_variables);

  gttic(evaluate_error_before);
  if(params_.evaluateNonlinearError)
    result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate()));
  gttoc(evaluate_error_before);

  gttic(gather_involved_keys);
  // 3. Mark linear update
  FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
  // Also mark keys involved in removed factors
  {
    FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors
    markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
  }
  // Also mark any provided extra re-eliminate keys
  if(extraReelimKeys) {
    BOOST_FOREACH(Key key, *extraReelimKeys) {
      markedKeys.insert(ordering_.at(key));
    }
  }

  // Observed keys for detailed results
  if(params_.enableDetailedResults) {
    BOOST_FOREACH(Index index, markedKeys) {
      result.detail->variableStatus[ordering_.key(index)].isObserved = true;
    }
  }
  // NOTE: we use assign instead of the iterator constructor here because this
  // is a vector of size_t, so the constructor unintentionally resolves to
  // vector(size_t count, Index value) instead of the iterator constructor.
  FastVector<Index> observedKeys;  observedKeys.reserve(markedKeys.size());
  BOOST_FOREACH(Index index, markedKeys) {
    if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused
      observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them
  }
  gttoc(gather_involved_keys);

  // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
  FastSet<Index> relinKeys;
  if (relinearizeThisStep) {
    gttic(gather_relinearize_keys);
    // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
    if(params_.enablePartialRelinearizationCheck)
      relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold);
    else
      relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold);
    if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging

    // Remove from relinKeys any keys whose linearization points are fixed
    BOOST_FOREACH(Key key, fixedVariables_) {
      relinKeys.erase(ordering_[key]);
    }
    if(noRelinKeys) {
      BOOST_FOREACH(Key key, *noRelinKeys) {
        relinKeys.erase(ordering_[key]);
      }
    }

    // Above relin threshold keys for detailed results
    if(params_.enableDetailedResults) {
      BOOST_FOREACH(Index index, relinKeys) {
        result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold = true;
        result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }

    // Add the variables being relinearized to the marked keys
    vector<bool> markedRelinMask(ordering_.size(), false);
    BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
    markedKeys.insert(relinKeys.begin(), relinKeys.end());
    gttoc(gather_relinearize_keys);

    gttic(fluid_find_all);
    // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
    if (!relinKeys.empty() && this->root()) {
      // add other cliques that have the marked ones in the separator
      Impl::FindAll(this->root(), markedKeys, markedRelinMask);

      // Relin involved keys for detailed results
      if(params_.enableDetailedResults) {
        FastSet<Index> involvedRelinKeys;
        Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask);
        BOOST_FOREACH(Index index, involvedRelinKeys) {
          if(!result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold) {
            result.detail->variableStatus[ordering_.key(index)].isRelinearizeInvolved = true;
            result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
      }
    }
    gttoc(fluid_find_all);

    gttic(expmap);
    // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
    if (!relinKeys.empty())
      Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_);
    gttoc(expmap);

    result.variablesRelinearized = markedKeys.size();
  } else {
    result.variablesRelinearized = 0;
  }

  gttic(linearize_new);
  // 7. Linearize new factors
  if(params_.cacheLinearizedFactors) {
    gttic(linearize);
    FactorGraph<GaussianFactor>::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_);
    linearFactors_.push_back(*linearFactors);
    assert(nonlinearFactors_.size() == linearFactors_.size());
    gttoc(linearize);

    gttic(augment_VI);
    // Augment the variable index with the new factors
    variableIndex_.augment(*linearFactors);
    gttoc(augment_VI);
  } else {
    variableIndex_.augment(*newFactors.symbolic(ordering_));
  }
  gttoc(linearize_new);

  gttic(recalculate);
  // 8. Redo top of Bayes tree
  // Convert constrained symbols to indices
  boost::optional<FastMap<Index,int> > constrainedIndices;
  if(constrainedKeys) {
    constrainedIndices = FastMap<Index,int>();
    typedef pair<const Key, int> Key_Group;
    BOOST_FOREACH(Key_Group key_group, *constrainedKeys) {
      constrainedIndices->insert(make_pair(ordering_[key_group.first], key_group.second));
    }
  }
  boost::shared_ptr<FastSet<Index> > replacedKeys;
  if(!markedKeys.empty() || !observedKeys.empty())
    replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedIndices, result);

  // Update replaced keys mask (accumulates until back-substitution takes place)
  if(replacedKeys) {
    BOOST_FOREACH(const Index var, *replacedKeys) {
      deltaReplacedMask_[var] = true; } }
  gttoc(recalculate);

  // After the top of the tree has been redone and may have index gaps from
  // unused keys, condense the indices to remove gaps by rearranging indices
  // in all data structures.
  if(!unusedKeys.empty()) {
    gttic(remove_variables);
    Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
        deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_, fixedVariables_);
    gttoc(remove_variables);
  }
  result.cliques = this->nodes().size();
  deltaDoglegUptodate_ = false;
  deltaUptodate_ = false;

  gttic(evaluate_error_after);
  if(params_.evaluateNonlinearError)
    result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate()));
  gttoc(evaluate_error_after);

  return result;
}

/* ************************************************************************* */
void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeys, boost::optional<std::vector<size_t>&> marginalFactorsIndices,
    boost::optional<std::vector<size_t>&> deletedFactorsIndices)
{
  // Convert set of keys into a set of indices
  FastSet<Index> indices;
  BOOST_FOREACH(Key key, leafKeys) {
    indices.insert(ordering_[key]);
  }

  // Keep track of marginal factors - map from clique to the marginal factors
  // that should be incorporated into it, passed up from it's children.
  multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors;

  // Remove each variable and its subtrees
  BOOST_REVERSE_FOREACH(Index j, indices) {
    if(nodes_[j]) { // If the index was not already removed by removing another subtree
      sharedClique clique = nodes_[j];

      // See if we should remove the whole clique
      bool marginalizeEntireClique = true;
      BOOST_FOREACH(Index frontal, clique->conditional()->frontals()) {
        if(indices.find(frontal) == indices.end()) {
          marginalizeEntireClique = false;
          break; } }

      // Remove either the whole clique or part of it
      if(marginalizeEntireClique) {
        // Remove the whole clique and its subtree, and keep the marginal factor.
        GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor();
        // We do not need the marginal factors associated with this clique
        // because their information is already incorporated in the new
        // marginal factor.  So, now associate this marginal factor with the
        // parent of this clique.
        marginalFactors.insert(make_pair(clique->parent(), marginalFactor));
        // Now remove this clique and its subtree - all of its marginal
        // information has been stored in marginalFactors.
        const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques
        BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
          marginalFactors.erase(removedClique);
          BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) {
            if(indices.find(indexInClique) == indices.end())
              throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); }
        }
      }
      else {
        // Reeliminate the current clique and the marginals from its children,
        // then keep only the marginal on the non-marginalized variables.  We
        // get the childrens' marginals from any existing children, plus
        // the marginals from the marginalFactors multimap, which come from any
        // subtrees already marginalized out.

        // Add child marginals and remove marginalized subtrees
        GaussianFactorGraph graph;
        FastSet<size_t> factorsInSubtreeRoot;
        Cliques subtreesToRemove;
        BOOST_FOREACH(const sharedClique& child, clique->children()) {
          // Remove subtree if child depends on any marginalized keys
          BOOST_FOREACH(Index parentIndex, child->conditional()->parents()) {
            if(indices.find(parentIndex) != indices.end()) {
              subtreesToRemove.push_back(child);
              graph.push_back(child->cachedFactor()); // Add child marginal
              break;
            }
          }
        }
        Cliques childrenRemoved;
        BOOST_FOREACH(const sharedClique& childToRemove, subtreesToRemove) {
          const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques
          childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end());
          BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
            marginalFactors.erase(removedClique);
            BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) {
              if(indices.find(indexInClique) == indices.end())
                throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); }
          }
        }

        // Gather remaining children after we removed marginalized subtrees
        vector<sharedClique> orphans(clique->children().begin(), clique->children().end());

        // Add the factors that are pulled into the current clique by the marginalized variables.
        // These are the factors that involve *marginalized* frontal variables in this clique
        // but do not involve frontal variables of any of its children.
        FastSet<size_t> factorsFromMarginalizedInClique;
        BOOST_FOREACH(Index indexInClique, clique->conditional()->frontals()) {
          if(indices.find(indexInClique) != indices.end())
            factorsFromMarginalizedInClique.insert(variableIndex_[indexInClique].begin(), variableIndex_[indexInClique].end()); }
        BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) {
          BOOST_FOREACH(Index indexInClique, removedChild->conditional()->frontals()) {
            BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) {
              factorsFromMarginalizedInClique.erase(factorInvolving); } } }
        BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique) {
          graph.push_back(nonlinearFactors_[i]->linearize(theta_, ordering_)); }

        // Remove the current clique
        sharedClique parent = clique->parent();
        this->removeClique(clique);

        // Reeliminate the linear graph to get the marginal and discard the conditional
        const FastSet<Index> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
        FastSet<Index> cliqueFrontalsToEliminate;
        std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), indices.begin(), indices.end(),
          std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end()));
        vector<Index> cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end());
        pair<GaussianConditional::shared_ptr, GaussianFactorGraph> eliminationResult1 =
          graph.eliminate(cliqueFrontalsToEliminateV,
          params_.factorization==ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky);

        // Add the resulting marginal
        BOOST_FOREACH(const GaussianFactor::shared_ptr& marginal, eliminationResult1.second) {
          if(marginal)
            marginalFactors.insert(make_pair(clique, marginal)); }

        // Recover the conditional on the remaining subset of frontal variables
        // of this clique being martially marginalized.
        size_t nToEliminate = std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) - clique->conditional()->begin() + 1;
        GaussianFactorGraph graph2;
        graph2.push_back(clique->conditional()->toFactor());
        GaussianFactorGraph::EliminationResult eliminationResult2 =
          params_.factorization == ISAM2Params::QR ?
          EliminateQR(graph2, nToEliminate) :
          EliminatePreferCholesky(graph2, nToEliminate);
        GaussianFactorGraph graph3;
        graph3.push_back(eliminationResult2.second);
        GaussianFactorGraph::EliminationResult eliminationResult3 =
          params_.factorization == ISAM2Params::QR ?
          EliminateQR(graph3, clique->conditional()->nrFrontals() - nToEliminate) :
          EliminatePreferCholesky(graph3, clique->conditional()->nrFrontals() - nToEliminate);
        sharedClique newClique = boost::make_shared<Clique>(make_pair(eliminationResult3.first, clique->cachedFactor()));

        // Add the marginalized clique to the BayesTree
        this->addClique(newClique, parent);

        // Add the orphans
        BOOST_FOREACH(const sharedClique& orphan, orphans) {
          this->addClique(orphan, newClique); }
      }
    }
  }

  // At this point we have updated the BayesTree, now update the remaining iSAM2 data structures

  // Gather factors to add - the new marginal factors
  GaussianFactorGraph factorsToAdd;
  typedef pair<sharedClique, GaussianFactor::shared_ptr> Clique_Factor;
  BOOST_FOREACH(const Clique_Factor& clique_factor, marginalFactors) {
    if(clique_factor.second) {
      factorsToAdd.push_back(clique_factor.second);
      if(marginalFactorsIndices) marginalFactorsIndices->push_back(nonlinearFactors_.size());
      nonlinearFactors_.push_back(boost::make_shared<LinearContainerFactor>(
        clique_factor.second, ordering_));
      if(params_.cacheLinearizedFactors) {
        linearFactors_.push_back(clique_factor.second);
      }
      BOOST_FOREACH(Index factorIndex, *clique_factor.second) {
        fixedVariables_.insert(ordering_.key(factorIndex));
      }
    }
  }
  variableIndex_.augment(factorsToAdd); // Augment the variable index

  // Remove the factors to remove that have been summarized in the newly-added marginal factors
  FastSet<size_t> factorIndicesToRemove;
  BOOST_FOREACH(Index j, indices) {
    factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); }
  vector<size_t> removedFactorsIndices;
  SymbolicFactorGraph removedFactors;
  BOOST_FOREACH(size_t i, factorIndicesToRemove) {
    if(deletedFactorsIndices) deletedFactorsIndices->push_back(i);
    removedFactorsIndices.push_back(i);
    removedFactors.push_back(nonlinearFactors_[i]->symbolic(ordering_));
    nonlinearFactors_.remove(i);
    if(params_.cacheLinearizedFactors)
      linearFactors_.remove(i);
  }
  variableIndex_.remove(removedFactorsIndices, removedFactors);

  // Remove the marginalized variables
  Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
    deltaReplacedMask_, ordering_, nodes_, linearFactors_, fixedVariables_);
}

/* ************************************************************************* */
void ISAM2::updateDelta(bool forceFullSolve) const {

  if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
    // If using Gauss-Newton, update with wildfireThreshold
    const ISAM2GaussNewtonParams& gaussNewtonParams =
        boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
    const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold;
    gttic(Wildfire_update);
    lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold);
    gttoc(Wildfire_update);

  } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) {
    // If using Dogleg, do a Dogleg step
    const ISAM2DoglegParams& doglegParams =
        boost::get<ISAM2DoglegParams>(params_.optimizationParams);

    // Do one Dogleg iteration
    gttic(Dogleg_Iterate);
    DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate(
        *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose));
    gttoc(Dogleg_Iterate);

    gttic(Copy_dx_d);
    // Update Delta and linear step
    doglegDelta_ = doglegResult.Delta;
    delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
    gttoc(Copy_dx_d);
  }

  deltaUptodate_ = true;
}

/* ************************************************************************* */
Values ISAM2::calculateEstimate() const {
  // We use ExpmapMasked here instead of regular expmap because the former
  // handles Permuted<VectorValues>
  gttic(Copy_Values);
  Values ret(theta_);
  gttoc(Copy_Values);
  gttic(getDelta);
  const VectorValues& delta(getDelta());
  gttoc(getDelta);
  gttic(Expmap);
  vector<bool> mask(ordering_.size(), true);
  Impl::ExpmapMasked(ret, delta, ordering_, mask);
  gttoc(Expmap);
  return ret;
}

/* ************************************************************************* */
const Value& ISAM2::calculateEstimate(Key key) const {
  const Index index = getOrdering()[key];
  const Vector& delta = getDelta()[index];
  return *theta_.at(key).retract_(delta);
}

/* ************************************************************************* */
Values ISAM2::calculateBestEstimate() const {
  VectorValues delta(theta_.dims(ordering_));
  internal::optimizeInPlace<Base>(this->root(), delta);
  return theta_.retract(delta, ordering_);
}

/* ************************************************************************* */
Matrix ISAM2::marginalCovariance(Index key) const {
  return marginalFactor(ordering_[key],
    params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky)
    ->information().inverse();
}

/* ************************************************************************* */
const VectorValues& ISAM2::getDelta() const {
  if(!deltaUptodate_)
    updateDelta();
  return delta_;
}

/* ************************************************************************* */
VectorValues optimize(const ISAM2& isam) {
  gttic(allocateVectorValues);
  VectorValues delta = *allocateVectorValues(isam);
  gttoc(allocateVectorValues);
  optimizeInPlace(isam, delta);
  return delta;
}

/* ************************************************************************* */
void optimizeInPlace(const ISAM2& isam, VectorValues& delta) {
  // We may need to update the solution calculations
  if(!isam.deltaDoglegUptodate_) {
    gttic(UpdateDoglegDeltas);
    double wildfireThreshold = 0.0;
    if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
      wildfireThreshold = boost::get<ISAM2GaussNewtonParams>(isam.params().optimizationParams).wildfireThreshold;
    else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams))
      wildfireThreshold = boost::get<ISAM2DoglegParams>(isam.params().optimizationParams).wildfireThreshold;
    else
      assert(false);
    ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
    isam.deltaDoglegUptodate_ = true;
    gttoc(UpdateDoglegDeltas);
  }

  gttic(copy_delta);
  delta = isam.deltaNewton_;
  gttoc(copy_delta);
}

/* ************************************************************************* */
VectorValues optimizeGradientSearch(const ISAM2& isam) {
  gttic(Allocate_VectorValues);
  VectorValues grad = *allocateVectorValues(isam);
  gttoc(Allocate_VectorValues);

  optimizeGradientSearchInPlace(isam, grad);

  return grad;
}

/* ************************************************************************* */
void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
  // We may need to update the solution calcaulations
  if(!isam.deltaDoglegUptodate_) {
    gttic(UpdateDoglegDeltas);
    double wildfireThreshold = 0.0;
    if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
      wildfireThreshold = boost::get<ISAM2GaussNewtonParams>(isam.params().optimizationParams).wildfireThreshold;
    else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams))
      wildfireThreshold = boost::get<ISAM2DoglegParams>(isam.params().optimizationParams).wildfireThreshold;
    else
      assert(false);
    ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
    isam.deltaDoglegUptodate_ = true;
    gttoc(UpdateDoglegDeltas);
  }

  gttic(Compute_Gradient);
  // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
  gradientAtZero(isam, grad);
  double gradientSqNorm = grad.dot(grad);
  gttoc(Compute_Gradient);

  gttic(Compute_minimizing_step_size);
  // Compute minimizing step size
  double RgNormSq = isam.RgProd_.asVector().squaredNorm();
  double step = -gradientSqNorm / RgNormSq;
  gttoc(Compute_minimizing_step_size);

  gttic(Compute_point);
  // Compute steepest descent point
  scal(step, grad);
  gttoc(Compute_point);
}

/* ************************************************************************* */
VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) {
  return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
}

/* ************************************************************************* */
static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root, VectorValues& g) {
  // Loop through variables in each clique, adding contributions
  int variablePosition = 0;
  for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) {
    const int dim = root->conditional()->dim(jit);
    g[*jit] += root->gradientContribution().segment(variablePosition, dim);
    variablePosition += dim;
  }

  // Recursively add contributions from children
  typedef boost::shared_ptr<ISAM2Clique> sharedClique;
  BOOST_FOREACH(const sharedClique& child, root->children()) {
    gradientAtZeroTreeAdder(child, g);
  }
}

/* ************************************************************************* */
void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) {
  // Zero-out gradient
  g.setZero();

  // Sum up contributions for each clique
  if(bayesTree.root())
    gradientAtZeroTreeAdder(bayesTree.root(), g);
}

}
/// namespace gtsam


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <pthread.h>
#include <unistd.h>
#include <boost/unordered_map.hpp>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/trim.hpp>

#include "ini.c"
#include "INIReader.cpp"
#include "AdUtil.h"
//#include "Util.h"

using namespace boost;

static bool isLong(const std::string& str, long* l) {
  try {
    string str1(str);
    boost::trim(str1);
    *l = boost::lexical_cast<long>(str1);
  }
  catch (...) {
    // DEBUG(LOGNAME,"not long string: str=%s", str.c_str());
    return false;
  }
  return true;
}
static bool isFloat(const std::string& str, float* f) {
  try {
    string str1(str);
    boost::trim(str1);
    *f = boost::lexical_cast<float>(str1);
  }
  catch (...) {
    // DEBUG(LOGNAME,"not float string: str=%s", str.c_str());
    return false;
  }
  return true;
}

typedef struct {
  long node_id;
  long left, right;
  float impurity;
  long feature;
  float thresh;
  long feature_type;
  std::vector<std::string> feature_list;
  float value;  // only leaf node has value
} node_t;

typedef struct {
  boost::unordered_map<int, float> mTreeWeights;
  boost::unordered_map<int, std::vector<node_t> > mTrees;
  boost::unordered_map<int, boost::unordered_map<int, int> > mLeafCodes;
  float mConstWeight;
  boost::unordered_map<uint64_t, std::vector<long> > mFeatureMappingKey;
  boost::unordered_map<uint64_t, boost::unordered_map<long, long> >
      mFeatureMappingValue;
} gbdt_coeff_t;

bool parseNodeFromString(const std::string& node_str, int node_id,
                         node_t& node) {
  // left_id, right_id, impurity(infomation gain), feature_id/feature_name,
  // thresh, feature_type(0 value/1 category), value_list(category list),
  // value(only leaf valid)
  // 1,2,0.010769,2,575.500000,0,,0.000000
  if (node_str == "") return false;

  vector<string> tmp;
  AdUtil::Split3(tmp, node_str, ',');
  long left, right, feature, feature_type;
  float impurity, thresh, value;
  if (!isLong(tmp[0], &left) || !isLong(tmp[1], &right) ||
      !isFloat(tmp[2], &impurity) || !isLong(tmp[3], &feature) ||
      !isFloat(tmp[4], &thresh) || !isLong(tmp[5], &feature_type) ||
      !isFloat(tmp[7], &value)) {
    return false;
  }
  node.left = left;
  node.right = right;
  node.impurity = impurity;
  node.feature = feature;
  node.thresh = thresh;
  node.feature_type = feature_type;
  node.value = value;
  string value_list = tmp[6];
  tmp.clear();
  AdUtil::Split3(tmp, value_list, ';');
  for (int i = 0; i < tmp.size(); i++) {
    node.feature_list.push_back(tmp[i]);
  }

  return true;
}

bool loadModel(gbdt_coeff_t& model, const char* model_file) {
  ifstream ifs(model_file);
  if (!ifs) {
    return false;
  }

  unordered_map<int, std::vector<node_t> >().swap(model.mTrees);  //释放内存
  INIReader* gbdt_define = new INIReader(model_file);
  if (gbdt_define == NULL) {
    return false;
  }

  int num_trees = gbdt_define->GetInteger("declare", "num", 0);
  string weights = gbdt_define->Get("declare", "weights", "");
  vector<string> tmp;
  AdUtil::Split3(tmp, weights, ',');
  for (int i = 0; i < tmp.size(); i++) {
    float v;
    if (!isFloat(tmp[i], &v)) {
      return false;
    }
    model.mTreeWeights.insert(make_pair(i, v));
  }

  char tree_name[256], node_name[256];
  int max_leaf_index = 0;
  int leaf_node_code = 0;  // 叶子节点编号，用于做特征变换
  for (int i = 0; i < num_trees; i++) {
    sprintf(tree_name, "tree_%d", i);
    string tree_name_str(tree_name);

    int node_id = 0, max_leaf_index = -1;
    vector<node_t> tree;
    boost::unordered_map<int, int> leaf_code;
    while (true) {
      sprintf(node_name, "node_%d", node_id);
      string node_name_str(node_name);

      string node_str = gbdt_define->Get(tree_name_str, node_name_str, "");
      if (node_str == "" && max_leaf_index > node_id) {
        node_t node;
        tree.push_back(node);
        node_id += 1;
        continue;
      }
      if (node_str == "" && max_leaf_index <= node_id) {
        break;
      }

      // 解析树节点
      node_t node;
      node.left = -1;
      node.right = -1;
      if (!parseNodeFromString(node_str, node_id, node)) return false;
      if (max_leaf_index < node.left) max_leaf_index = node.left;
      if (max_leaf_index < node.right) max_leaf_index = node.right;
      tree.push_back(node);
      if (node.left == -1 && node.right == -1) {
        leaf_code.insert(make_pair(node.node_id, leaf_node_code));
        leaf_node_code += 1;
      }

      node_id += 1;
    }
    model.mTrees.insert(make_pair(i, tree));
    model.mLeafCodes.insert(make_pair(i, leaf_code));
  }
  delete gbdt_define;

  return true;
}

bool transform(unordered_map<uint32_t, uint32_t>& features, gbdt_coeff_t& model,
               unordered_map<uint32_t, uint32_t>& output) {
  int num_trees = model.mTreeWeights.size();

  for (int i = 0; i < num_trees; i++) {
    std::vector<node_t>& tree = model.mTrees[i];
    boost::unordered_map<int, int>& leaf_code = model.mLeafCodes[i];

    int leaf_index = 0;
    while (tree[leaf_index].left != -1 && tree[leaf_index].right != -1) {
      int feature_index = tree[leaf_index].feature;

      int feature_value = 0;
      if (features.find(feature_index) != features.end())
        feature_value = features[feature_index];

      if (feature_value <= tree[leaf_index].thresh)
        leaf_index = tree[leaf_index].left;
      else
        leaf_index = tree[leaf_index].right;
    }
    long feature_idx =
        leaf_code[leaf_index];  // 经过gbdt叶子节点编号后的叶节点id

    output.insert(make_pair(feature_idx, 1));
  }

  return true;
}

int main(int argc, char** argv) {
  if (argc != 2) {
    cerr << "Usage: " << argv[0] << " <model-file>" << endl;
    return -1;
  }
  const char* model_file = argv[1];
  gbdt_coeff_t model;

  if (!loadModel(model, model_file)) {
    cerr << "loadModel error" << endl;
    return -1;
  }

  string line;
  while (getline(cin, line)) {
    //-1 1 |f 609:1 3097:1 3769:1 1728:1 4573:1 1293:1 2441:1 4213:1 4201:1
    unordered_map<uint32_t, uint32_t> input;
    unordered_map<uint32_t, uint32_t> output;

    vector<string> tmp;
    AdUtil::Split3(tmp, line, ' ');
    for (int k = 3; k < tmp.size(); k++) {
      input.insert(make_pair(k - 3, atoi(tmp[k].c_str())));
    }

    if (!transform(input, model, output)) {
      cerr << "transform error" << endl;
      continue;
    }

    cout << tmp[0] << "\t" << tmp[1] << "\t" << tmp[2] << "\t";
    for (unordered_map<uint32_t, uint32_t>::iterator iter = output.begin();
         iter != output.end(); iter++) {
      cout << iter->first << ":1"
           << "\t";
    }
    cout << endl;
  }

  return 0;
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef __BOYER_MYRVOLD_PLANAR_TEST_HPP__
#define __BOYER_MYRVOLD_PLANAR_TEST_HPP__

#include <boost/graph/planar_detail/boyer_myrvold_impl.hpp>
#include <boost/parameter.hpp>
#include <boost/type_traits.hpp>
#include <boost/mpl/bool.hpp>

namespace boost
{

  struct no_kuratowski_subgraph_isolation {};
  struct no_planar_embedding {};

  namespace boyer_myrvold_params
  {

    BOOST_PARAMETER_KEYWORD(tag, graph)
    BOOST_PARAMETER_KEYWORD(tag, embedding)
    BOOST_PARAMETER_KEYWORD(tag, kuratowski_subgraph)
    BOOST_PARAMETER_KEYWORD(tag, vertex_index_map)
    BOOST_PARAMETER_KEYWORD(tag, edge_index_map)

    typedef parameter::parameters< parameter::required<tag::graph>,
                                   tag::embedding,
                                   tag::kuratowski_subgraph,
                                   tag::vertex_index_map,
                                   tag::edge_index_map
                                   > boyer_myrvold_params_t;

    namespace core
    {

      template <typename ArgumentPack>
      bool dispatched_boyer_myrvold(ArgumentPack const& args,
                                    mpl::true_,
                                    mpl::true_
                                    )
      {
        //Dispatch for no planar embedding, no kuratowski subgraph isolation

        typedef typename remove_const
                <
                    typename remove_reference
                    < typename parameter::binding
                        < ArgumentPack, tag::graph>::type
                    >::type
                >::type graph_t;

        typedef typename parameter::binding
          < ArgumentPack,
            tag::vertex_index_map,
            typename property_map
              < typename remove_reference<graph_t>::type,
                vertex_index_t>::const_type
          >::type vertex_index_map_t;

        boyer_myrvold_impl
          <graph_t,
           vertex_index_map_t,
           graph::detail::no_old_handles,
           graph::detail::no_embedding
          >
          planarity_tester(args[graph],
                           args[vertex_index_map |
                                get(vertex_index, args[graph])
                                ]
                           );

        return planarity_tester.is_planar() ? true : false;
      }

      template <typename ArgumentPack>
      bool dispatched_boyer_myrvold(ArgumentPack const& args,
                                    mpl::true_,
                                    mpl::false_
                                    )
      {
        //Dispatch for no planar embedding, kuratowski subgraph isolation
        typedef typename remove_const
                <
                    typename remove_reference
                    < typename parameter::binding
                        < ArgumentPack, tag::graph>::type
                    >::type
                >::type graph_t;

        typedef typename parameter::binding
          < ArgumentPack,
            tag::vertex_index_map,
            typename property_map<graph_t, vertex_index_t>::type
          >::type vertex_index_map_t;

        boyer_myrvold_impl
          <graph_t,
           vertex_index_map_t,
           graph::detail::store_old_handles,
           graph::detail::no_embedding
          >
          planarity_tester(args[graph],
                           args[vertex_index_map |
                                get(vertex_index, args[graph])
                                ]
                           );

        if (planarity_tester.is_planar())
          return true;
        else
          {
            planarity_tester.extract_kuratowski_subgraph
              (args[kuratowski_subgraph],
               args[edge_index_map|get(edge_index, args[graph])]
               );
            return false;
          }
      }

      template <typename ArgumentPack>
      bool dispatched_boyer_myrvold(ArgumentPack const& args,
                                    mpl::false_,
                                    mpl::true_
                                    )
      {
        //Dispatch for planar embedding, no kuratowski subgraph isolation
        typedef typename remove_const
                <
                    typename remove_reference
                    < typename parameter::binding
                        < ArgumentPack, tag::graph>::type
                    >::type
                >::type graph_t;

        typedef typename parameter::binding
          < ArgumentPack,
          tag::vertex_index_map,
          typename property_map<graph_t, vertex_index_t>::type
          >::type  vertex_index_map_t;

        boyer_myrvold_impl
          <graph_t,
           vertex_index_map_t,
           graph::detail::no_old_handles,
#ifdef BOOST_GRAPH_PREFER_STD_LIB
           graph::detail::std_list
#else
           graph::detail::recursive_lazy_list
#endif
          >
          planarity_tester(args[graph],
                           args[vertex_index_map |
                                get(vertex_index, args[graph])
                                ]
                           );

        if (planarity_tester.is_planar())
          {
            planarity_tester.make_edge_permutation(args[embedding]);
            return true;
          }
        else
          return false;
      }

      template <typename ArgumentPack>
      bool dispatched_boyer_myrvold(ArgumentPack const& args,
                                    mpl::false_,
                                    mpl::false_
                                    )
      {
        //Dispatch for planar embedding, kuratowski subgraph isolation
        typedef typename remove_const
                <
                    typename remove_reference
                    < typename parameter::binding
                        < ArgumentPack, tag::graph>::type
                    >::type
                >::type graph_t;

        typedef typename parameter::binding
          < ArgumentPack,
          tag::vertex_index_map,
          typename property_map<graph_t, vertex_index_t>::type
          >::type vertex_index_map_t;

        boyer_myrvold_impl
          <graph_t,
          vertex_index_map_t,
          graph::detail::store_old_handles,
#ifdef BOOST_BGL_PREFER_STD_LIB
           graph::detail::std_list
#else
           graph::detail::recursive_lazy_list
#endif
          >
          planarity_tester(args[graph],
                           args[vertex_index_map |
                                get(vertex_index, args[graph])
                                ]
                           );

        if (planarity_tester.is_planar())
          {
            planarity_tester.make_edge_permutation(args[embedding]);
            return true;
          }
        else
          {
            planarity_tester.extract_kuratowski_subgraph
              (args[kuratowski_subgraph],
               args[edge_index_map | get(edge_index, args[graph])]
               );
            return false;
          }
      }

      template <typename ArgumentPack>
      bool boyer_myrvold_planarity_test(ArgumentPack const& args)
      {

        typedef typename parameter::binding
          < ArgumentPack,
            tag::kuratowski_subgraph,
            const no_kuratowski_subgraph_isolation&
          >::type
          kuratowski_arg_t;

        typedef typename parameter::binding
          < ArgumentPack,
            tag::embedding,
            const no_planar_embedding&
          >::type
          embedding_arg_t;

         return dispatched_boyer_myrvold
           (args,
            boost::is_same
              <embedding_arg_t, const no_planar_embedding&>(),
            boost::is_same
              <kuratowski_arg_t, const no_kuratowski_subgraph_isolation&>()
            );
      }

    } //namespace core

  } //namespace boyer_myrvold_params

  template <typename A0>
  bool boyer_myrvold_planarity_test(A0 const& arg0)
  {
    return boyer_myrvold_params::core::boyer_myrvold_planarity_test
      (boyer_myrvold_params::boyer_myrvold_params_t()(arg0));
  }

  template <typename A0, typename A1>
  //  bool boyer_myrvold_planarity_test(A0 const& arg0, A1 const& arg1)
  bool boyer_myrvold_planarity_test(A0 const& arg0, A1 const& arg1)
  {
    return boyer_myrvold_params::core::boyer_myrvold_planarity_test
      (boyer_myrvold_params::boyer_myrvold_params_t()(arg0,arg1));
  }

  template <typename A0, typename A1, typename A2>
  bool boyer_myrvold_planarity_test(A0 const& arg0,
                                    A1 const& arg1,
                                    A2 const& arg2
                                    )
  {
    return boyer_myrvold_params::core::boyer_myrvold_planarity_test
      (boyer_myrvold_params::boyer_myrvold_params_t()(arg0,arg1,arg2));
  }

  template <typename A0, typename A1, typename A2, typename A3>
  bool boyer_myrvold_planarity_test(A0 const& arg0,
                                    A1 const& arg1,
                                    A2 const& arg2,
                                    A3 const& arg3
                                    )
  {
    return boyer_myrvold_params::core::boyer_myrvold_planarity_test
      (boyer_myrvold_params::boyer_myrvold_params_t()(arg0,arg1,arg2,arg3));
  }

  template <typename A0, typename A1, typename A2, typename A3, typename A4>
  bool boyer_myrvold_planarity_test(A0 const& arg0,
                                    A1 const& arg1,
                                    A2 const& arg2,
                                    A3 const& arg3,
                                    A4 const& arg4
                                    )
  {
    return boyer_myrvold_params::core::boyer_myrvold_planarity_test
      (boyer_myrvold_params::boyer_myrvold_params_t()
       (arg0,arg1,arg2,arg3,arg4)
       );
  }

}

#endif //__BOYER_MYRVOLD_PLANAR_TEST_HPP__


//=======================================================================
// Copyright 2015 Tsinghua University.
// Authors: Fuan Pu (Pu.Fuan@gmail.com)
//
// Dung's abstract argumentation framework
//=======================================================================

#ifndef ARGUMATRIX_PARSER_HPP
#define ARGUMATRIX_PARSER_HPP

#include <iostream>
#include <string>
#include <utility>
#include <algorithm>
// #include <regex>
#include <boost/regex.hpp>

#include "config/config.hpp"
#include "dung_theory/ArgumentProperty.hpp"
#include "dung_theory/AttackProperty.hpp"
#include "dung_theory/DungAF.hpp"

namespace argumatrix{

using namespace std;

class parser
{
public:
	/**
	 * @brief Read Aspartix format from a file to DungAF. We have implemented
	 * three approaches  to read aspartix format. stl::regex, boost::regex and
	 * the approach from ArgSemSAT.1.0rc3. By testing, we choose boost::regex
	 * since it is more stable than stl::regex, and slightly faster than ArgSemSAT.
	 * @param std::string filePath -- the file path of the Aspartix format file.
	 * @param DungAF& daf -- the output abstract argumentation framework
	 * @return bool. If the translation is successful return true, else return false.
	 */
	static bool Aspartix2DungAF(const std::string& filePath, DungAF& daf);

	/**
	 * Read Trivial Graph Format from a file to DungAF
	 * @param std::string filePath -- the file path of the Aspartix format file.
	 * @param DungAF& daf -- the output abstract argumentation framework
	 * @return bool. If the translation is successful return true, else return false.
	 */
	static bool TrivialGraph2DungAF(const std::string& filePath, DungAF& daf);
};

//bool parser::Aspartix2DungAF(const std::string& fileName, DungAF& daf)
//{
//	bool _is_successful = true;
//
//	// The argument and attack regex expressions of Aspartix format
//	// arg(a1).
//	// att(a1, a2).
//
//	// Before parsing, we need to clear the daf.
//	daf.clear();
//
//	std::ifstream _infile;
//	_infile.open(fileName, ios_base::in);
//
//	//check the file
//	if(!_infile)
//	{
//		cerr << "Can not open the file: " << fileName << endl;
//		exit(1);
//	}
//
//	size_type start;
//	size_type _line_num = 0;
//	std::string _line_str;
//
//	while ( getline(_infile, _line_str) )
//	{
//		// add attacks
//		if ( (start =_line_str.find("att(") != string::npos) )
//		{
//			start += 3;
//
//			size_type comma = _line_str.find(",", start);
//			string arg_src = trim(_line_str.substr(start, comma - start));
//
//			++comma;
//			size_type end_ = _line_str.find(")", comma);
//			string arg_tag = trim(_line_str.substr(comma, end_ - comma));
//
//			daf.addAttack(arg_src, arg_tag);
//			//cout << arg_src << "->" << arg_tag << endl;
//		}
//		else if ((start =_line_str.find("arg(") != string::npos))
//		{
//			start += 3;
//			size_type end_ = _line_str.find(")", start);
//			string arg1 = trim(_line_str.substr(start, end_ - start));
//
//			daf.addArgument(arg1);
//			//cout << arg1 << endl;
//		}
//	}
//
//	_infile.close();
//
//	return _is_successful;
//} // Aspartix2DungAF(const std::string& fileName, DungAF& daf)

//// The std::regex may result regex_error on linux systems. Thus, we using
//// boost::regex to substitute.
bool parser::Aspartix2DungAF(const std::string& fileName, DungAF& daf)
{
	bool _is_successful = true;

	// The argument and attack regex expressions of Aspartix format
	// arg(a1).
	// att(a1, a2).
	const boost::regex rgx_arg("arg\\(\\s*([a-zA-Z0-9_]+)\\s*\\)");
	const boost::regex rgx_att("att\\(\\s*([a-zA-Z0-9_]+)\\s*,\\s*([a-zA-Z0-9_]+)\\s*\\)");

	// Before parsing, we need to clear the daf.
	daf.clear();

	std::ifstream _infile;
	_infile.open(fileName, ios_base::in);

	//check the file
	if(!_infile)
	{
		cerr << "Can not open the file: " << fileName << endl;
		exit(1);
	}

	std::string _line_str;
	size_type   _line_num = 0;
	boost::cmatch _result;

	while ( getline(_infile, _line_str) )
	{
		//_infile.getline()
		//_infile >> _line_str;
		++_line_num;

		if(_line_str.length()<=4)
			continue;

		if (boost::regex_search(_line_str.c_str(), _result, rgx_att))  // Find Attack
		{
			daf.addAttack(_result[1], _result[2]);
			//cout << "Adding Attack: (" << _result[1] << "," << _result[2] << ")" << endl;
		}
		else if (boost::regex_search(_line_str.c_str(), _result, rgx_arg)) // Find Argument
		{
			daf.addArgument( _result[1] );
			//cout << "Adding Argument: " << _result[1] << endl;
		}
		else
		{
			std::cerr << "Ignorance: " << fileName << endl
				<< "     Line number(" << _line_num << "): " << _line_str << endl;
			_is_successful = false;
		}
	}

	_infile.close();

	return _is_successful;
} // Aspartix2DungAF(const std::string& fileName, DungAF& daf)

/////////////////////
// Implemented by stl::regex
/////////////////////
//bool parser::Aspartix2DungAF(const std::string& fileName, DungAF& daf)
//{
//	bool _is_successful = true;
//
//	// The argument and attack regex expressions of Aspartix format
//	// arg(a1).
//	// att(a1, a2).
//	const std::regex rgx_arg("arg\\(\\s*([a-zA-Z0-9]+)\\s*\\)");
//	const std::regex rgx_att("att\\(\\s*([a-zA-Z0-9]+)\\s*,\\s*([a-zA-Z0-9]+)\\s*\\)");
//
//	// Before parsing, we need to clear the daf.
//	daf.clear();
//
//	std::ifstream _infile;
//	_infile.open(fileName);
//
//	//check the file
//	if(!_infile)
//	{
//		cerr << "Can not open the file: " << fileName << endl;
//		exit(1);
//	}
//
//	std::string _line_str;
//	size_type   _line_num = 0;
//	std::match_results<std::string::const_iterator> _result;
//
//	while ( getline(_infile, _line_str) )
//	{
//		//_infile.getline()
//		//_infile >> _line_str;
//		++_line_num;
//
//		if(_line_str.length()<=4)
//			continue;
//
//		if (std::regex_search(_line_str, _result, rgx_att))  // Find Attack
//		{
//			daf.addAttack(_result[1], _result[2]);
//			//cout << "Adding Attack: (" << _result[1] << "," << _result[2] << ")" << endl;
//		}
//		else if (std::regex_search(_line_str, _result, rgx_arg)) // Find Argument
//		{
//			daf.addArgument( _result[1] );
//			//cout << "Adding Argument: " << _result[1] << endl;
//		}
//		else
//		{
//			std::cout << "Ignorance: " << fileName << endl
//				      << "     Line number(" << _line_num << "): " << _line_str << endl;
//			_is_successful = false;
//		}
//	}
//
//	_infile.close();
//
//	return _is_successful;
//} // Aspartix2DungAF(const std::string& fileName, DungAF& daf)

bool parser::TrivialGraph2DungAF(const std::string& fileName, DungAF& daf)
{
	bool _is_successful=true;
	string arg1, arg2;

	// Before parsing, we need to clear the daf.
	daf.clear();

	std::ifstream _infile;
	_infile.open(fileName, ios_base::in);

	//check the file
	if(!_infile)
	{
		cerr << "Can not open the file: " << fileName << endl;
		exit(1);
	}

	// read arguments
	while(_infile >> arg1) {
		if(arg1=="#") {
			break;
		} else {
			daf.addArgument(arg1);
			// cout << arg1 << endl;
		}
	}

	// read attackes
	while(_infile >> arg1 >> arg2) {
		daf.addAttack(arg1, arg2);
		// cout << arg1 << " -> " << arg2 << endl;
	}

	_infile.close();

	return _is_successful;
} // TrivialGraph2DungAF(const std::string& fileName, DungAF& daf)

} // namespace argumatrix

#endif  //ARGUMATRIX_PARSER_HPP


/*
 * CubeBoundaryDistribution.cpp
 *
 *  Created on: 22.02.2018
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/utilities.h>

#include <measurements/CubeBoundaryDistribution.h>
#include <measurements/GridDistribution.h>
#include <measurements/SensorDistribution.h>

#include <cstdio>
#include <fstream>
#include <iostream>

namespace wavepi {
namespace measurements {
using namespace dealii;

template <>
void CubeBoundaryDistribution<1>::update_grid(const std::vector<double> &times,
                                              const std::vector<std::vector<double>> &points_per_dim) {
  Assert(points_per_dim.size() == 1, ExcInternalError());

  this->points_per_dim = points_per_dim;

  size_t nX = points_per_dim[0].size();

  std::vector<Point<1>> points_each_time;
  points_each_time.reserve(2);

  points_each_time.emplace_back(points_per_dim[0][0]);
  points_each_time.emplace_back(points_per_dim[0][nX - 1]);

  std::vector<std::vector<Point<1>>> points_per_time(times.size());

  for (size_t i = 0; i < times.size(); i++)
    points_per_time[i] = points_each_time;

  update_points(times, points_per_time);

  // update times_per_point and points now (cleared by update_points!)
  points = points_each_time;
  times_per_point.reserve(points_each_time.size());

  for (size_t ix = 0; ix < points_each_time.size(); ix++)
    times_per_point.push_back(times);
}

template <>
void CubeBoundaryDistribution<2>::update_grid(const std::vector<double> &times,
                                              const std::vector<std::vector<double>> &points_per_dim) {
  Assert(points_per_dim.size() == 2, ExcInternalError());

  this->points_per_dim = points_per_dim;

  size_t nX = points_per_dim[0].size();
  size_t nY = points_per_dim[1].size();

  std::vector<Point<2>> points_each_time;
  points_each_time.reserve(2 * (nX + nY));

  for (size_t ix = 0; ix < nX; ix++)
    for (size_t iy = 0; iy < nY; iy++)
      if (ix == 0 || ix == nX - 1 || iy == 0 || iy == nY - 1)
        points_each_time.emplace_back(points_per_dim[0][ix], points_per_dim[1][iy]);

  std::vector<std::vector<Point<2>>> points_per_time(times.size());

  for (size_t i = 0; i < times.size(); i++)
    points_per_time[i] = points_each_time;

  update_points(times, points_per_time);

  // update times_per_point and points now (cleared by update_points!)
  points = points_each_time;
  times_per_point.reserve(points_each_time.size());

  for (size_t ix = 0; ix < points_each_time.size(); ix++)
    times_per_point.push_back(times);
}

template <>
void CubeBoundaryDistribution<3>::update_grid(const std::vector<double> &times,
                                              const std::vector<std::vector<double>> &points_per_dim) {
  Assert(points_per_dim.size() == 3, ExcInternalError());

  this->points_per_dim = points_per_dim;

  size_t nX = points_per_dim[0].size();
  size_t nY = points_per_dim[1].size();
  size_t nZ = points_per_dim[2].size();

  std::vector<Point<3>> points_each_time;
  points_each_time.reserve(2 * (nX * nY + nX * nZ + nY * nZ));

  for (size_t ix = 0; ix < nX; ix++)
    for (size_t iy = 0; iy < nY; iy++)
      for (size_t iz = 0; iz < nZ; iz++)
        if (ix == 0 || ix == nX - 1 || iy == 0 || iy == nY - 1 || iz == 0 || iz == nZ - 1)
          points_each_time.emplace_back(points_per_dim[0][ix], points_per_dim[1][iy], points_per_dim[2][iz]);

  std::vector<std::vector<Point<3>>> points_per_time(times.size());

  for (size_t i = 0; i < times.size(); i++)
    points_per_time[i] = points_each_time;

  update_points(times, points_per_time);

  // update times_per_point and points now (cleared by update_points!)
  points = points_each_time;
  times_per_point.reserve(points_each_time.size());

  for (size_t ix = 0; ix < points_each_time.size(); ix++)
    times_per_point.push_back(times);
}

template <int dim>
CubeBoundaryDistribution<dim>::CubeBoundaryDistribution(const std::vector<double> &times,
                                                        const std::vector<std::vector<double>> &points_per_dim) {
  update_grid(times, points_per_dim);
}

template <int dim>
void CubeBoundaryDistribution<dim>::declare_parameters(ParameterHandler &prm) {
  prm.enter_subsection("CubeBoundaryDistribution");
  {
    prm.declare_entry("points x", "-0.9:10:0.9", Patterns::Anything(),
                      "points for the grid in x-direction. Format: '[lb]:[n_points]:[ub]'. Lower bound and upper bound "
                      "are inclusive, so they define the margin to the boundary of the cube.");
    prm.declare_entry("points y", "-0.9:10:0.9", Patterns::Anything(),
                      "points for the grid in y-direction. Format: '[lb]:[n_points]:[ub]'. Lower bound and upper bound "
                      "are inclusive, so they define the margin to the boundary of the cube.");
    prm.declare_entry("points z", "-0.9:10:0.9", Patterns::Anything(),
                      "points for the grid in z-direction. Format: '[lb]:[n_points]:[ub]'. Lower bound and upper bound "
                      "are inclusive, so they define the margin to the boundary of the cube.");
    prm.declare_entry("points t", "0:10:6", Patterns::Anything(),
                      "points for the grid in time. Format: '[lower bound]:[n_points]:[ub]'. Upper bound is inclusive, "
                      "lower bound is exclusive iff it equals 0.0.");
  }
  prm.leave_subsection();
}

template <int dim>
void CubeBoundaryDistribution<dim>::get_parameters(ParameterHandler &prm) {
  prm.enter_subsection("CubeBoundaryDistribution");
  {
    AssertThrow(0 <= dim && dim <= 3, ExcInternalError());

    std::vector<std::vector<double>> spatial_points;
    spatial_points.emplace_back(parse_description(prm.get("points x")));
    if (dim > 1) spatial_points.emplace_back(parse_description(prm.get("points y")));
    if (dim > 2) spatial_points.emplace_back(parse_description(prm.get("points z")));

    auto temporal_points = parse_description(prm.get("points t"), true);
    update_grid(temporal_points, spatial_points);
  }
  prm.leave_subsection();
}

template <int dim>
std::vector<double> CubeBoundaryDistribution<dim>::parse_description(const std::string description, bool is_time) {
  double lb, ub;
  size_t nb;

  AssertThrow(std::sscanf(description.c_str(), " %lf : %zu : %lf ", &lb, &nb, &ub) == 3,
              ExcMessage("Could not parse points"));
  AssertThrow((is_time && nb > 1 && lb < ub && lb >= 0.0) || (!is_time && nb >= 1 && lb <= ub),
              ExcMessage("Illegal interval spec: " + description));

  std::vector<double> points(nb);

  if (lb == 0.0 && is_time)  // lb excl, ub incl
    for (size_t i = 0; i < nb; i++)
      points[i] = lb + (i + 1) * (ub - lb) / nb;
  else
    // lb excl, ub excl
    for (size_t i = 0; i < nb; i++)
      points[i] = lb + (i + 1) * (ub - lb) / (nb + 1);

  return points;
}

template <int dim>
size_t CubeBoundaryDistribution<dim>::index_times_per_point(size_t point_index, size_t time_index) {
  return time_index * this->points_per_time[0].size() + point_index;
}

template <int dim>
void CubeBoundaryDistribution<dim>::write_pvd(const std::vector<double> &values, std::string path, std::string filename,
                                              std::string name) {
  GridDistribution<dim> grid(this->times, points_per_dim);

  auto grid_values = spread_to_grid(values);
  grid.write_pvd(grid_values, path, filename, name);
}

template <>
std::vector<double> CubeBoundaryDistribution<1>::spread_to_grid(const std::vector<double> &values) {
  size_t nX = points_per_dim[0].size();

  std::vector<double> grid_values;
  grid_values.reserve(nX);
  size_t idx = 0;

  for (size_t it = 0; it < times.size(); it++)
    for (size_t ix = 0; ix < nX; ix++)
      if (ix == 0 || ix == nX - 1)
        grid_values.push_back(values[idx++]);
      else
        grid_values.push_back(0.0);

  return grid_values;
}

template <>
std::vector<double> CubeBoundaryDistribution<2>::spread_to_grid(const std::vector<double> &values) {
  size_t nX = points_per_dim[0].size();
  size_t nY = points_per_dim[1].size();

  std::vector<double> grid_values;
  grid_values.reserve(nX * nY);
  size_t idx = 0;

  for (size_t it = 0; it < times.size(); it++)
    for (size_t ix = 0; ix < nX; ix++)
      for (size_t iy = 0; iy < nY; iy++)
        if (ix == 0 || ix == nX - 1 || iy == 0 || iy == nY - 1)
          grid_values.push_back(values[idx++]);
        else
          grid_values.push_back(0.0);

  return grid_values;
}

template <>
std::vector<double> CubeBoundaryDistribution<3>::spread_to_grid(const std::vector<double> &values) {
  size_t nX = points_per_dim[0].size();
  size_t nY = points_per_dim[1].size();
  size_t nZ = points_per_dim[2].size();

  std::vector<double> grid_values;
  grid_values.reserve(nX * nY * nZ);
  size_t idx = 0;

  for (size_t it = 0; it < times.size(); it++)
    for (size_t ix = 0; ix < nX; ix++)
      for (size_t iy = 0; iy < nY; iy++)
        for (size_t iz = 0; iz < nZ; iz++)
          if (ix == 0 || ix == nX - 1 || iy == 0 || iy == nY - 1 || iz == 0 || iz == nZ - 1)
            grid_values.push_back(values[idx++]);
          else
            grid_values.push_back(0.0);

  return grid_values;
}

template class CubeBoundaryDistribution<1>;
template class CubeBoundaryDistribution<2>;
template class CubeBoundaryDistribution<3>;

} /* namespace measurements */
} /* namespace wavepi */


/*
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The Original Code is Copyright (C) 2018 Blender Foundation.
 * All rights reserved.
 */

#ifndef __EIGEN3_MATRIX_C_API_CC__
#define __EIGEN3_MATRIX_C_API_CC__

/* Eigen gives annoying huge amount of warnings here, silence them! */
#if defined(__GNUC__) && !defined(__clang__)
#  pragma GCC diagnostic ignored "-Wlogical-op"
#endif

#ifdef __EIGEN3_MATRIX_C_API_CC__ /* quiet warning */
#endif

#include <Eigen/Core>
#include <Eigen/Dense>

#include "matrix.h"

using Eigen::Map;
using Eigen::Matrix4f;

bool EIG_invert_m4_m4(float inverse[4][4], const float matrix[4][4])
{
  Map<Matrix4f> M = Map<Matrix4f>((float *)matrix);
  Matrix4f R;
  bool invertible = true;
  M.computeInverseWithCheck(R, invertible, 0.0f);
  if (!invertible) {
    R = R.Zero();
  }
  memcpy(inverse, R.data(), sizeof(float) * 4 * 4);
  return invertible;
}

#endif /* __EIGEN3_MATRIX_C_API_CC__ */