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

 * 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 Lie.h
 * @brief Base class and basic functions for Lie types
 * @author Richard Roberts
 * @author Alex Cunningham
 */

#pragma once

#include <string>

#include <gtsam/base/Matrix.h>

namespace gtsam {

  /**
   * These core global functions can be specialized by new Lie types
   * for better performance.
   */

  /** Compute l0 s.t. l2=l1*l0 */
  template<class T>
  inline T between_default(const T& l1, const T& l2) { return l1.inverse().compose(l2); }

  /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */
  template<class T>
  inline Vector logmap_default(const T& l0, const T& lp) { return T::Logmap(l0.between(lp)); }

  /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
  template<class T>
  inline T expmap_default(const T& t, const Vector& d) { return t.compose(T::Expmap(d)); }

  /**
   * Base class for Lie group type
   * This class uses the Curiously Recurring Template design pattern to allow for
   * concept checking using a private function.
   *
   * T is the derived Lie type, like Point2, Pose3, etc.
   *
   * By convention, we use capital letters to designate a static function
   */
  template <class T>
  class Lie {
  private:

	  /** concept checking function - implement the functions this demands */
	  static void concept_check(const T& t) {

		  /** assignment */
		  T t2 = t;

		  /**
		   * Returns dimensionality of the tangent space
		   */
		  size_t dim_ret = t.dim();

		  /**
		   * Returns Exponential map update of T
		   * Default implementation calls global binary function
		   */
		  T expmap_ret = t.expmap(gtsam::zero(dim_ret));

		  /** expmap around identity */
		  T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));

		  /**
		   * Returns Log map
		   * Default Implementation calls global binary function
		   */
		  Vector logmap_ret = t.logmap(t2);

		  /** Logmap around identity */
		  Vector logmap_identity_ret = T::Logmap(t);

		  /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
		  T between_ret = t.between(t2);

		  /** compose with another object */
		  T compose_ret = t.compose(t2);

		  /** invert the object and yield a new one */
		  T inverse_ret = t.inverse();
	  }

	  /**
	   * The necessary functions to implement for Lie are defined
	   * below with additional details as to the interface.  The
	   * concept checking function above will check whether or not
	   * the function exists and throw compile-time errors.
	   */


	  /**
	   * Returns dimensionality of the tangent space
	   */
//	  inline size_t dim() const;

	  /**
	   * Returns Exponential map update of T
	   * A default implementation of expmap(*this, lp) is available:
	   * expmap_default()
	   */
//	  T expmap(const Vector& v) const;

	  /** expmap around identity */
//	  static T Expmap(const Vector& v);

	  /**
	   * Returns Log map
	   * A default implementation of logmap(*this, lp) is available:
	   * logmap_default()
	   */
//	  Vector logmap(const T& lp) const;

	  /** Logmap around identity */
//	  static Vector Logmap(const T& p);

	  /**
	   * Compute l0 s.t. l2=l1*l0, where (*this) is l1
	   * A default implementation of between(*this, lp) is available:
	   * between_default()
	   */
//	  T between(const T& l2) const;

	  /** compose with another object */
//	  inline T compose(const T& p) const;

	  /** invert the object and yield a new one */
//	  inline T inverse() const;

  };
  
  /** Call print on the object */
  template<class T>
  inline void print(const T& object, const std::string& s = "") {
    object.print(s);
  }

  /** Call equal on the object */
  template<class T>
  inline bool equal(const T& obj1, const T& obj2, double tol) {
    return obj1.equals(obj2, tol);
  }

  /** Call equal on the object without tolerance (use default tolerance) */
  template<class T>
  inline bool equal(const T& obj1, const T& obj2) {
    return obj1.equals(obj2);
  }

  /**
   *  Three term approximation of the Baker�Campbell�Hausdorff formula
   *  In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
   *  it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
   *  formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
   *  http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula
   */
  template<class T>
  T BCH(const T& X, const T& Y) {
  	static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
  	T X_Y = bracket(X, Y);
  	return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
  			bracket(X, X_Y));
  }

  /**
   * Declaration of wedge (see Murray94book) used to convert
   * from n exponential coordinates to n*n element of the Lie algebra
   */
  template <class T> Matrix wedge(const Vector& x);

  /**
   * Exponential map given exponential coordinates
   * class T needs a wedge<> function and a constructor from Matrix
   * @param x exponential coordinates, vector of size n
   * @ return a T
   */
  template <class T>
  T expm(const Vector& x, int K=7) {
  	Matrix xhat = wedge<T>(x);
    return expm(xhat,K);
  }

} // namespace gtsam