/**
 * @file SQPOptimizer.h
 * @brief Interface for a generic SQP-based nonlinear optimization engine
 * @author Alex Cunningham
 */

#pragma once

#include "VectorConfig.h"

namespace gtsam {

/**
 * This class is an engine for performing SQP-based optimization
 * It stores a graph, a config, and needs a specific ordering, and
 * then will perform optimization iterations in a functional way.
 */
template<class FactorGraph, class Config>
class SQPOptimizer {

public:
	// useful for storing configurations
	typedef boost::shared_ptr<const Config> shared_config;
	typedef boost::shared_ptr<const VectorConfig> shared_vconfig;

private:
	// keep const references to the graph and the original ordering
	const FactorGraph* graph_;
	const Ordering* ordering_;

	// keep configurations
	shared_config config_;
	shared_vconfig lagrange_config_;
	double error_;

public:
	/**
	 * Standard external constructor
	 * @param graph is the nonlinear graph to optimize
	 * @param ordering is the elimination ordering to use
	 * @param config is the initial configuration for the real variables
	 */
	SQPOptimizer(const FactorGraph& graph, const Ordering& ordering,
			shared_config config);

	/**
	 * Constructor that includes a lagrange initialization.  Primarily
	 * for internal iterations, but if the user has an idea of what a good
	 * set of lagrange multipliers is, they can specify them, assuming that
	 * the naming convention is the same as the internal system.
	 * @param graph is the nonlinear graph to optimize
	 * @param ordering is the elimination ordering to use
	 * @param config is the initial configuration for the real variables
	 * @param lagrange is the configuration of lagrange multipliers
	 */
	SQPOptimizer(const FactorGraph& graph, const Ordering& ordering,
			shared_config config, shared_vconfig lagrange);

	/// Access functions
	const FactorGraph* graph() const { return graph_; }
	const Ordering* ordering() const { return ordering_; }
	shared_config config() const { return config_; }

};

}