/**
 * @file TupleConfig.h
 * @author Richard Roberts
 * @author Manohar Paluri
 * @author Alex Cunningham
 */

#include "LieConfig.h"
#include "VectorConfig.h"

#pragma once

namespace gtsam {

 /**
   *  TupleConfigs are a structure to manage heterogenous LieConfigs, so as to
   *  enable different types of variables/keys to be used simultaneously.  The
   *  interface is designed to mimic that of a single LieConfig.
   *
   *  This uses a recursive structure of config pairs to form a lisp-like
   *  list, with a special case (TupleConfigEnd) that contains only one config
   *  at the end.  Because this recursion is done at compile time, there is no
   *  runtime performance hit to using this structure.
   *
   *  For an easier to use approach, there are TupleConfigN classes, which wrap
   *  the recursive TupleConfigs together as a single class, so you can have
   *  mixed-type classes from 2-6 types.  Note that a TupleConfig2 is equivalent
   *  to the previously used PairConfig.
   *
   *  Design and extension note:
   *  To implement a recursively templated data structure, note that most operations
   *  have two versions: one with templates and one without.  The templated one allows
   *  for the arguments to be passed to the next config, while the specialized one
   *  operates on the "first" config. TupleConfigEnd contains only the specialized version.
   */
  template<class Config1, class Config2>
  class TupleConfig : public Testable<TupleConfig<Config1, Config2> > {

  protected:
	  // Data for internal configs
	  Config1 first_;	/// Arbitrary config
	  Config2 second_;	/// A TupleConfig or TupleConfigEnd, which wraps an arbitrary config

  public:
	  // typedefs for config subtypes
	  typedef class Config1::Key Key1;
	  typedef class Config1::Value Value1;

	  /** default constructor */
	  TupleConfig() {}

	  /** Copy constructor */
	  TupleConfig(const TupleConfig<Config1, Config2>& config) :
		  first_(config.first_), second_(config.second_) {}

	  /** Construct from configs */
	  TupleConfig(const Config1& cfg1, const Config2& cfg2) :
		  first_(cfg1), second_(cfg2) {}

	  virtual ~TupleConfig() {}

	  /** Print */
	  void print(const std::string& s = "") const {
			first_.print(s);
			second_.print();
	  }

	  /** Equality with tolerance for keys and values */
	  bool equals(const TupleConfig<Config1, Config2>& c, double tol=1e-9) const {
		  return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
	  }

	  /**
	   * Insert a key/value pair to the config.
	   * Note: if the key is already in the config, the config will not be changed.
	   * Use update() to allow for changing existing values.
	   * @param key is the key - can be an int (second version) if the can can be initialized from an int
	   * @param value is the value to insert
	   */
	  template<class Key, class Value>
	  void insert(const Key& key, const Value& value) {second_.insert(key, value);}
	  void insert(const int& key, const Value1& value) {first_.insert(Key1(key), value);}
	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value);}

	  /**
	   * Insert a complete config at a time.
	   * Note: if the key is already in the config, the config will not be changed.
	   * Use update() to allow for changing existing values.
	   * @param config is a full config to add
	   */
	  template<class Cfg1, class Cfg2>
	  void insert(const TupleConfig<Cfg1, Cfg2>& config) { second_.insert(config); }
	  void insert(const TupleConfig<Config1, Config2>& config) {
		  first_.insert(config.first_);
		  second_.insert(config.second_);
	  }

	  /**
	   * Update function for whole configs - this will change existing values
	   * @param config is a config to add
	   */
	  template<class Cfg1, class Cfg2>
	  void update(const TupleConfig<Cfg1, Cfg2>& config) { second_.update(config); }
	  void update(const TupleConfig<Config1, Config2>& config) {
	  	first_.update(config.first_);
	  	second_.update(config.second_);
	  }

	  /**
	   * Update function for single key/value pairs - will change existing values
	   * @param key is the variable identifier
	   * @param value is the variable value to update
	   */
	  template<class Key, class Value>
	  void update(const Key& key, const Value& value) { second_.update(key, value); }
	  void update(const Key1& key, const Value1& value) { first_.update(key, value); }

	  /**
	   * Insert a subconfig
	   * @param config is the config to insert
	   */
	  template<class Cfg>
	  void insertSub(const Cfg& config) { second_.insertSub(config); }
	  void insertSub(const Config1& config) { first_.insert(config);  }

	  /** erase an element by key */
	  template<class Key>
	  void erase(const Key& j)  { second_.erase(j); }
	  void erase(const Key1& j)  { first_.erase(j); }

	  /** clears the config */
	  void clear() { first_.clear(); second_.clear(); }

	  /** determine whether an element exists */
	  template<class Key>
	  bool exists(const Key& j) const { return second_.exists(j); }
	  bool exists(const Key1& j) const { return first_.exists(j); }

	  /** a variant of exists */
	  template<class Key>
	  boost::optional<typename Key::Value_t> exists_(const Key& j)  const { return second_.exists_(j); }
	  boost::optional<Value1>                exists_(const Key1& j) const { return first_.exists_(j); }

	  /** access operator */
	  template<class Key>
	  const typename Key::Value_t & operator[](const Key& j) const { return second_[j]; }
	  const Value1& operator[](const Key1& j) const { return first_[j]; }

	  /** at access function */
	  template<class Key>
	  const typename Key::Value_t & at(const Key& j) const { return second_.at(j); }
	  const Value1& at(const Key1& j) const { return first_.at(j); }

	  /** direct config access */
	  const Config1& config() const { return first_; }
	  const Config2& rest() const { return second_; }

	  /** zero: create VectorConfig of appropriate structure */
	  VectorConfig zero() const {
		  VectorConfig z1 = first_.zero(), z2 = second_.zero();
		  z2.insert(z1);
		  return z2;
	  }

	  /** @return number of key/value pairs stored */
	  size_t size() const { return first_.size() + second_.size(); }

	  /** @return true if config is empty */
	  bool empty() const { return first_.empty() && second_.empty(); }

	  /** @return The dimensionality of the tangent space */
	  size_t dim() const { return first_.dim() + second_.dim(); }

	  /** Expmap */
	  TupleConfig<Config1, Config2> expmap(const VectorConfig& delta) const {
	        return TupleConfig(gtsam::expmap(first_, delta), second_.expmap(delta));
	  }

	  /** logmap each element */
	  VectorConfig logmap(const TupleConfig<Config1, Config2>& cp) const {
		  VectorConfig ret(gtsam::logmap(first_, cp.first_));
		  ret.insert(second_.logmap(cp.second_));
		  return ret;
	  }

  private:
	  /** Serialization function */
	  friend class boost::serialization::access;
	  template<class Archive>
	  void serialize(Archive & ar, const unsigned int version) {
		  ar & BOOST_SERIALIZATION_NVP(first_);
		  ar & BOOST_SERIALIZATION_NVP(second_);
	  }

  };

  /**
   * End of a recursive TupleConfig - contains only one config
   *
   * Do not use this class directly - it should only be used as a part
   * of a recursive structure
   */
  template<class Config>
  class TupleConfigEnd : public Testable<TupleConfigEnd<Config> > {

  protected:
	  // Data for internal configs
	  Config first_;

  public:
	  // typedefs
	  typedef class Config::Key Key1;
	  typedef class Config::Value Value1;

	  TupleConfigEnd() {}

	  TupleConfigEnd(const TupleConfigEnd<Config>& config) :
		  first_(config.first_) {}

	  TupleConfigEnd(const Config& cfg) :
		  first_(cfg) {}

	  virtual ~TupleConfigEnd() {}

	  void print(const std::string& s = "") const {
			first_.print();
	  }

	  bool equals(const TupleConfigEnd<Config>& c, double tol=1e-9) const {
		  return first_.equals(c.first_, tol);
	  }

	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
	  void insert(const int& key, const Value1& value) {first_.insert(Key1(key), value);}

	  void insert(const TupleConfigEnd<Config>& config) {first_.insert(config.first_); }

	  void update(const TupleConfigEnd<Config>& config) {first_.update(config.first_); }

	  void update(const Key1& key, const Value1& value) { first_.update(key, value); }

	  void insertSub(const Config& config) {first_.insert(config); }

	  const Value1& operator[](const Key1& j) const { return first_[j]; }

	  const Config& config() const { return first_; }

	  void erase(const Key1& j) { first_.erase(j); }

	  void clear() { first_.clear(); }

	  bool empty() const { return first_.empty(); }

	  bool exists(const Key1& j) const { return first_.exists(j); }

	  boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); }

	  const Value1& at(const Key1& j) const { return first_.at(j); }

	  VectorConfig zero() const {
		  VectorConfig z = first_.zero();
		  return z;
	  }

	  size_t size() const { return first_.size(); }

	  size_t dim() const { return first_.dim(); }

	  TupleConfigEnd<Config> expmap(const VectorConfig& delta) const {
	        return TupleConfigEnd(gtsam::expmap(first_, delta));
	  }

	  VectorConfig logmap(const TupleConfigEnd<Config>& cp) const {
		  VectorConfig ret(gtsam::logmap(first_, cp.first_));
		  return ret;
	  }

  private:
	  friend class boost::serialization::access;
	  template<class Archive>
	  void serialize(Archive & ar, const unsigned int version) {
		  ar & BOOST_SERIALIZATION_NVP(first_);
	  }
  };

  /** Exmap static functions */
  template<class Config1, class Config2>
  inline TupleConfig<Config1, Config2> expmap(const TupleConfig<Config1, Config2>& c, const VectorConfig& delta) {
	  return c.expmap(delta);
  }

  /** logmap static functions */
  template<class Config1, class Config2>
    inline VectorConfig logmap(const TupleConfig<Config1, Config2>& c0, const TupleConfig<Config1, Config2>& cp) {
	  return c0.logmap(cp);
  }

  /**
   * Wrapper classes to act as containers for configs.  Note that these can be cascaded
   * recursively, as they are TupleConfigs, and are primarily a short form of the config
   * structure to make use of the TupleConfigs easier.
   *
   * The interface is designed to mimic PairConfig, but for 2-6 config types.
   */

  template<class C1>
  class TupleConfig1 : public TupleConfigEnd<C1> {
  public:
 	  // typedefs
 	  typedef C1 Config1;

 	  typedef TupleConfigEnd<C1> Base;
 	  typedef TupleConfig1<C1> This;

 	  TupleConfig1() {}
 	  TupleConfig1(const This& config);
 	  TupleConfig1(const Base& config);
 	  TupleConfig1(const Config1& cfg1);

 	  // access functions
 	  inline const Config1& first() const { return this->config(); }
  };

  template<class C1>
  TupleConfig1<C1> expmap(const TupleConfig1<C1>& c, const VectorConfig& delta) {
 	  return c.expmap(delta);
  }

  template<class C1>
  VectorConfig logmap(const TupleConfig1<C1>& c1, const TupleConfig1<C1>& c2) {
 	  return c1.logmap(c2);
  }

  template<class C1, class C2>
  class TupleConfig2 : public TupleConfig<C1, TupleConfigEnd<C2> > {
  public:
	  // typedefs
	  typedef C1 Config1;
	  typedef C2 Config2;

	  typedef TupleConfig<C1, TupleConfigEnd<C2> > Base;
	  typedef TupleConfig2<C1, C2> This;

	  TupleConfig2() {}
	  TupleConfig2(const This& config);
	  TupleConfig2(const Base& config);
	  TupleConfig2(const Config1& cfg1, const Config2& cfg2);

	  // access functions
	  inline const Config1& first() const { return this->config(); }
	  inline const Config2& second() const { return this->rest().config(); }
  };

  template<class C1, class C2>
  TupleConfig2<C1, C2> expmap(const TupleConfig2<C1, C2>& c, const VectorConfig& delta) {
	  return c.expmap(delta);
  }

  template<class C1, class C2>
  VectorConfig logmap(const TupleConfig2<C1, C2>& c1, const TupleConfig2<C1, C2>& c2) {
	  return c1.logmap(c2);
  }

  template<class C1, class C2, class C3>
  class TupleConfig3 : public TupleConfig<C1, TupleConfig<C2, TupleConfigEnd<C3> > > {
  public:
	  // typedefs
	  typedef C1 Config1;
	  typedef C2 Config2;
	  typedef C3 Config3;

	  TupleConfig3() {}
	  TupleConfig3(const TupleConfig<C1, TupleConfig<C2, TupleConfigEnd<C3> > >& config);
	  TupleConfig3(const TupleConfig3<C1, C2, C3>& config);
	  TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3);

	  // access functions
	  inline const Config1& first() const { return this->config(); }
	  inline const Config2& second() const { return this->rest().config(); }
	  inline const Config3& third() const { return this->rest().rest().config(); }
  };

  template<class C1, class C2, class C3>
  TupleConfig3<C1, C2, C3> expmap(const TupleConfig3<C1, C2, C3>& c, const VectorConfig& delta) {
	  return c.expmap(delta);
  }

  template<class C1, class C2, class C3, class C4>
  class TupleConfig4 : public TupleConfig<C1, TupleConfig<C2,TupleConfig<C3, TupleConfigEnd<C4> > > > {
  public:
	  // typedefs
	  typedef C1 Config1;
	  typedef C2 Config2;
	  typedef C3 Config3;
	  typedef C4 Config4;

	  typedef TupleConfig<C1, TupleConfig<C2,TupleConfig<C3, TupleConfigEnd<C4> > > > Base;
	  typedef TupleConfig4<C1, C2, C3, C4> This;

	  TupleConfig4() {}
	  TupleConfig4(const This& config);
	  TupleConfig4(const Base& config);
	  TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4);

	  // access functions
	  inline const Config1& first() const { return this->config(); }
	  inline const Config2& second() const { return this->rest().config(); }
	  inline const Config3& third() const { return this->rest().rest().config(); }
	  inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
  };

  template<class C1, class C2, class C3, class C4>
  TupleConfig4<C1, C2, C3, C4> expmap(const TupleConfig4<C1, C2, C3, C4>& c, const VectorConfig& delta) {
	  return c.expmap(delta);
  }

  template<class C1, class C2, class C3, class C4, class C5>
  class TupleConfig5 : public TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfigEnd<C5> > > > > {
  public:
	  // typedefs
	  typedef C1 Config1;
	  typedef C2 Config2;
	  typedef C3 Config3;
	  typedef C4 Config4;
	  typedef C5 Config5;

	  TupleConfig5() {}
	  TupleConfig5(const TupleConfig5<C1, C2, C3, C4, C5>& config);
	  TupleConfig5(const TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfigEnd<C5> > > > >& config);
	  TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
				   const Config4& cfg4, const Config5& cfg5);

	  // access functions
	  inline const Config1& first() const { return this->config(); }
	  inline const Config2& second() const { return this->rest().config(); }
	  inline const Config3& third() const { return this->rest().rest().config(); }
	  inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
	  inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); }
  };

  template<class C1, class C2, class C3, class C4, class C5>
  TupleConfig5<C1, C2, C3, C4, C5> expmap(const TupleConfig5<C1, C2, C3, C4, C5>& c, const VectorConfig& delta) {
	  return c.expmap(delta);
  }

  template<class C1, class C2, class C3, class C4, class C5, class C6>
  class TupleConfig6 : public TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfig<C5, TupleConfigEnd<C6> > > > > > {
  public:
	  // typedefs
	  typedef C1 Config1;
	  typedef C2 Config2;
	  typedef C3 Config3;
	  typedef C4 Config4;
	  typedef C5 Config5;
	  typedef C6 Config6;

	  TupleConfig6() {}
	  TupleConfig6(const TupleConfig6<C1, C2, C3, C4, C5, C6>& config);
	  TupleConfig6(const TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfig<C5, TupleConfigEnd<C6> > > > > >& config);
	  TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
				   const Config4& cfg4, const Config5& cfg5, const Config6& cfg6);
	  // access functions
	  inline const Config1& first() const { return this->config(); }
	  inline const Config2& second() const { return this->rest().config(); }
	  inline const Config3& third() const { return this->rest().rest().config(); }
	  inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
	  inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); }
	  inline const Config6& sixth() const { return this->rest().rest().rest().rest().rest().config(); }
  };

  template<class C1, class C2, class C3, class C4, class C5, class C6>
  TupleConfig6<C1, C2, C3, C4, C5, C6> expmap(const TupleConfig6<C1, C2, C3, C4, C5, C6>& c, const VectorConfig& delta) {
	  return c.expmap(delta);
  }

}