/**
 * @file DummyFactor.cpp
 *
 * @date Sep 10, 2012
 * @author Alex Cunningham
 */

#include <gtsam_unstable/slam/DummyFactor.h>

#include <boost/foreach.hpp>

namespace gtsam {

/* ************************************************************************* */
DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2)
: NonlinearFactor(key1, key2)
{
	dims_.push_back(dim1);
	dims_.push_back(dim2);
	if (dim1 > dim2)
		rowDim_ = dim1;
	else
		rowDim_ = dim2;
}

/* ************************************************************************* */
void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
	std::cout << s << "  DummyFactor dim = " << rowDim_ << ", keys = { ";
	BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
	std::cout << "}" << std::endl;
}

/* ************************************************************************* */
bool DummyFactor::equals(const NonlinearFactor& f, double tol) const {
  const DummyFactor* e = dynamic_cast<const DummyFactor*>(&f);
  return e && Base::equals(f, tol) && dims_ == e->dims_ && rowDim_ == e->rowDim_;
}

/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
DummyFactor::linearize(const Values& c, const Ordering& ordering) const {
	// Only linearize if the factor is active
	if (!this->active(c))
		return boost::shared_ptr<JacobianFactor>();

	 // Fill in terms with zero matrices
  std::vector<std::pair<Index, Matrix> > terms(this->size());
  for(size_t j=0; j<this->size(); ++j) {
    terms[j].first = ordering[this->keys()[j]];
    terms[j].second = zeros(rowDim_, dims_[j]);
  }

  noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_);
  return GaussianFactor::shared_ptr(
  		new JacobianFactor(terms, zero(rowDim_), model));
}

/* ************************************************************************* */

} // \namespace gtsam