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

 * 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    smallExample.h
 * @brief   Create small example with two poses and one landmark
 * @brief   smallExample
 * @author  Carlos Nieto
 */

// \callgraph


#pragma once

#include <tests/simulated2D.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <boost/tuple/tuple.hpp>

namespace gtsam {
	namespace example {

		typedef NonlinearFactorGraph Graph;

		/**
		 * Create small example for non-linear factor graph
		 */
		boost::shared_ptr<const Graph> sharedNonlinearFactorGraph();
		Graph createNonlinearFactorGraph();

		/**
		 * Create values structure to go with it
		 * The ground truth values structure for the example above
		 */
		Values createValues();

		/** Vector Values equivalent */
		VectorValues createVectorValues();

		/**
		 * create a noisy values structure for a nonlinear factor graph
		 */
		boost::shared_ptr<const Values> sharedNoisyValues();
		Values createNoisyValues();

		/**
		 * Zero delta config
		 */
		VectorValues createZeroDelta(const Ordering& ordering);

		/**
		 * Delta config that, when added to noisyValues, returns the ground truth
		 */
		VectorValues createCorrectDelta(const Ordering& ordering);

		/**
		 * create a linear factor graph
		 * The non-linear graph above evaluated at NoisyValues
		 */
		JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering);

		/**
		 * create small Chordal Bayes Net x <- y
		 */
		GaussianBayesNet createSmallGaussianBayesNet();

		/**
		 * Create really non-linear factor graph (cos/sin)
		 */
		boost::shared_ptr<const Graph>
		sharedReallyNonlinearFactorGraph();
		Graph createReallyNonlinearFactorGraph();

		/**
		 * Create a full nonlinear smoother
		 * @param T number of time-steps
		 */
		std::pair<Graph, Values> createNonlinearSmoother(int T);

		/**
		 * Create a Kalman smoother by linearizing a non-linear factor graph
		 * @param T number of time-steps
		 */
		std::pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none);

		/* ******************************************************* */
		// Linear Constrained Examples
		/* ******************************************************* */

		/**
		 * Creates a simple constrained graph with one linear factor and
		 * one binary equality constraint that sets x = y
		 */
		JacobianFactorGraph createSimpleConstraintGraph();
		VectorValues createSimpleConstraintValues();

		/**
		 * Creates a simple constrained graph with one linear factor and
		 * one binary constraint.
		 */
		JacobianFactorGraph createSingleConstraintGraph();
		VectorValues createSingleConstraintValues();

		/**
		 * Creates a constrained graph with a linear factor and two
		 * binary constraints that share a node
		 */
		JacobianFactorGraph createMultiConstraintGraph();
		VectorValues createMultiConstraintValues();

		/* ******************************************************* */
		// Planar graph with easy subtree for SubgraphPreconditioner
		/* ******************************************************* */

		/*
		 * Create factor graph with N^2 nodes, for example for N=3
		 *  x13-x23-x33
		 *   |   |   |
		 *  x12-x22-x32
		 *   |   |   |
		 * -x11-x21-x31
		 * with x11 clamped at (1,1), and others related by 2D odometry.
		 */
		boost::tuple<JacobianFactorGraph, VectorValues> planarGraph(size_t N);

		/*
		 * Create canonical ordering for planar graph that also works for tree
		 * With x11 the root, e.g. for N=3
		 * x33 x23 x13 x32 x22 x12 x31 x21 x11
		 */
		Ordering planarOrdering(size_t N);

		/*
		 * Split graph into tree and loop closing constraints, e.g., with N=3
		 *  x13-x23-x33
		 *   |
		 *  x12-x22-x32
		 *   |
		 * -x11-x21-x31
		 */
		std::pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(
				size_t N, const JacobianFactorGraph& original);

	} // example
} // gtsam