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

 * 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   inference-inl.h
 * @brief  inference definitions
 * @author Frank Dellaert, Richard Roberts
 */

#include <gtsam/inference/inference-inl.h>

#include <gtsam/inference/SymbolicFactorGraph.h>

namespace gtsam {


/* ************************************************************************* */
//Conditional::shared_ptr
//Inference::EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var) {
//
//  tic("EliminateOne");
//
//  // Get the factors involving the eliminated variable
//  VariableIndex<>::mapped_type& varIndexEntry(variableIndex[var]);
//  typedef VariableIndex<>::mapped_factor_type mapped_factor_type;
//
//  if(!varIndexEntry.empty()) {
//
//    vector<size_t> removedFactors(varIndexEntry.size());
//    transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(),
//        boost::lambda::bind(&VariableIndex<>::mapped_factor_type::factorIndex, boost::lambda::_1));
//
//    // The new joint factor will be the last one in the factor graph
//    size_t jointFactorIndex = factorGraph.size();
//
//    static const bool debug = false;
//
//    if(debug) {
//      cout << "Eliminating " << var;
//      factorGraph.print(" from graph: ");
//      cout << removedFactors.size() << " factors to remove" << endl;
//    }
//
//    // Compute the involved keys, uses the variableIndex to mark whether each
//    // key has been added yet, but the positions stored in the variableIndex are
//    // from the unsorted positions and will be fixed later.
//    tic("EliminateOne: Find involved vars");
//    typedef set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > InvolvedKeys;
//    InvolvedKeys involvedKeys;
//    BOOST_FOREACH(size_t removedFactorI, removedFactors) {
//      if(debug) cout << removedFactorI << " is involved" << endl;
//      // If the factor has not previously been removed
//      if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) {
//        // Loop over the variables involved in the removed factor to update the
//        // variable index and joint factor positions of each variable.
//        BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) {
//          if(debug) cout << "  pulls in variable " << involvedVariable << endl;
//          // Mark the new joint factor as involving each variable in the removed factor.
//          assert(!variableIndex[involvedVariable].empty());
//          involvedKeys.insert(involvedVariable);
//        }
//      }
//
//      // Remove the original factor
//      factorGraph.remove(removedFactorI);
//    }
//
//    // We need only mark the next variable to be eliminated as involved with the joint factor
//    if(involvedKeys.size() > 1) {
//      InvolvedKeys::const_iterator next = involvedKeys.begin(); ++ next;
//      variableIndex[*next].push_back(mapped_factor_type(jointFactorIndex,0));
//    }
//    toc("EliminateOne: Find involved vars");
//    if(debug) cout << removedFactors.size() << " factors to remove" << endl;
//
//    // Join the factors and eliminate the variable from the joint factor
//    tic("EliminateOne: Combine");
//    Conditional::shared_ptr conditional = Conditional::FromRange(involvedKeys.begin(), involvedKeys.end(), 1);
//    Factor::shared_ptr eliminated(new Factor(conditional->beginParents(), conditional->endParents()));
//    toc("EliminateOne: Combine");
//
//    tic("EliminateOne: store eliminated");
//    factorGraph.push_back(eliminated);  // Put the eliminated factor into the factor graph
//    toc("EliminateOne: store eliminated");
//
//    toc("EliminateOne");
//
//    return conditional;
//
//  } else { // varIndexEntry.empty()
//    toc("EliminateOne");
//    return Conditional::shared_ptr();
//  }
//}

}