/*
 * @file TransformConstraint.h
 * @brief A constraint for combining graphs by common landmarks and a transform node
 * @author Alex Cunningham
 */

#pragma once

#include <gtsam/nonlinear/NonlinearConstraint.h>

namespace gtsam {

/**
 * A constraint between two landmarks in separate maps
 * Templated on:
 *   Values    : The overall config
 *	 PKey      : Key of landmark being constrained
 *	 Point     : Type of landmark
 *	 TKey      : Key of transform used
 *	 Transform : Transform variable class
 *
 * The Point and Transform concepts must be Lie types, and the transform
 * relationship "Point = transform_from(Transform, Point)" must exist.
 *
 * This base class should be specialized to implement the cost function for
 * specific classes of landmarks
 */
template<class Values, class PKey, class TKey>
class TransformConstraint : public NonlinearEqualityConstraint3<Values, PKey, TKey, PKey> {

public:
	typedef typename PKey::Value_t Point;
	typedef typename TKey::Value_t Transform;

	typedef NonlinearEqualityConstraint3<Values, PKey, TKey, PKey> Base;
	typedef TransformConstraint<Values, PKey, TKey> This;

	/**
	 * General constructor with all of the keys to variables in the map
	 * Extracts everything necessary from the key types
	 */
	TransformConstraint(const PKey& foreignKey, const TKey& transKey, const PKey& localKey, double mu = 1000.0)
	: Base(foreignKey, transKey, localKey, Point().dim(), mu) {}

	virtual ~TransformConstraint(){}

	/** Combined cost and derivative function using boost::optional */
	virtual Vector evaluateError(const Point& foreign, const Transform& trans, const Point& local,
				boost::optional<Matrix&> Dforeign = boost::none,
				boost::optional<Matrix&> Dtrans = boost::none,
				boost::optional<Matrix&> Dlocal = boost::none) const  {
		Point newlocal = trans.transform_from(foreign, Dtrans, Dforeign);
		if (Dlocal) {
			Point dummy;
			*Dlocal = -1* eye(dummy.dim());
		}
		return local.logmap(newlocal);
	}
};
} // \namespace gtsam