// This file is part of Sophus.
//
// Copyright 2012-2013 Hauke Strasdat
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#ifndef SOPHUS_SO2_HPP
#define SOPHUS_SO2_HPP

#include <complex>

#include "sophus.hpp"

////////////////////////////////////////////////////////////////////////////
// Forward Declarations / typedefs
////////////////////////////////////////////////////////////////////////////

namespace Sophus {
template<typename _Scalar, int _Options=0> class SO2Group;
typedef SO2Group<double> SO2 EIGEN_DEPRECATED;
typedef SO2Group<double> SO2d; /**< double precision SO2 */
typedef SO2Group<float> SO2f;  /**< single precision SO2 */
}

////////////////////////////////////////////////////////////////////////////
// Eigen Traits (For querying derived types in CRTP hierarchy)
////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////
// Eigen Traits (For querying derived types in CRTP hierarchy)
////////////////////////////////////////////////////////////////////////////

namespace Eigen {
namespace internal {

template<typename _Scalar, int _Options>
struct traits<Sophus::SO2Group<_Scalar,_Options> > {
  typedef _Scalar Scalar;
  typedef Matrix<Scalar,2,1> ComplexType;
};

template<typename _Scalar, int _Options>
struct traits<Map<Sophus::SO2Group<_Scalar>, _Options> >
    : traits<Sophus::SO2Group<_Scalar, _Options> > {
  typedef _Scalar Scalar;
  typedef Map<Matrix<Scalar,2,1>,_Options> ComplexType;
};

template<typename _Scalar, int _Options>
struct traits<Map<const Sophus::SO2Group<_Scalar>, _Options> >
    : traits<const Sophus::SO2Group<_Scalar, _Options> > {
  typedef _Scalar Scalar;
  typedef Map<const Matrix<Scalar,2,1>,_Options> ComplexType;
};

}
}

namespace Sophus {
using namespace Eigen;

/**
 * \brief SO2 base type - implements SO2 class but is storage agnostic
 *
 * [add more detailed description/tutorial]
 */
template<typename Derived>
class SO2GroupBase {
public:
  /** \brief scalar type */
  typedef typename internal::traits<Derived>::Scalar Scalar;
  /** \brief complex number reference type */
  typedef typename internal::traits<Derived>::ComplexType &
  ComplexReference;
  /** \brief complex number const reference type */
  typedef const typename internal::traits<Derived>::ComplexType &
  ConstComplexReference;

  /** \brief degree of freedom of group
   *         (one for in-plane rotation) */
  static const int DoF = 1;
  /** \brief number of internal parameters used
   *         (unit complex number for rotation) */
  static const int num_parameters = 2;
  /** \brief group transformations are NxN matrices */
  static const int N = 2;
  /** \brief group transfomation type */
  typedef Matrix<Scalar,N,N> Transformation;
  /** \brief point type */
  typedef Matrix<Scalar,2,1> Point;
  /** \brief tangent vector type */
  typedef Scalar Tangent;
  /** \brief adjoint transformation type */
  typedef Scalar Adjoint;

  /**
   * \brief Adjoint transformation
   *
   * This function return the adjoint transformation \f$ Ad \f$ of the
   * group instance \f$ A \f$  such that for all \f$ x \f$
   * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
   * with \f$\ \widehat{\cdot} \f$ being the hat()-operator.
   *
   * For SO2, it simply returns 1.
   */
  inline
  const Adjoint Adj() const {
    return 1;
  }

  /**
   * \returns copy of instance casted to NewScalarType
   */
  template<typename NewScalarType>
  inline SO2Group<NewScalarType> cast() const {
    return SO2Group<NewScalarType>(unit_complex()
                                   .template cast<NewScalarType>() );
  }

  /**
   * \returns pointer to internal data
   *
   * This provides unsafe read/write access to internal data. SO2 is represented
   * by a complex number with unit length (two parameters). When using direct
   * write access, the user needs to take care of that the complex number stays
   * normalized.
   *
   * \see normalize()
   */
  inline Scalar* data() {
    return unit_complex_nonconst().data();
  }

  /**
   * \returns const pointer to internal data
   *
   * Const version of data().
   */
  inline const Scalar* data() const {
    return unit_complex().data();
  }

  /**
   * \brief Fast group multiplication
   *
   * This method is a fast version of operator*=(), since it does not perform
   * normalization. It is up to the user to call normalize() once in a while.
   *
   * \see operator*=()
   */
  inline
  void fastMultiply(const SO2Group<Scalar>& other) {
    Scalar lhs_real = unit_complex().x();
    Scalar lhs_imag = unit_complex().y();
    const Scalar & rhs_real = other.unit_complex().x();
    const Scalar & rhs_imag = other.unit_complex().y();
    // complex multiplication
    unit_complex_nonconst().x() = lhs_real*rhs_real - lhs_imag*rhs_imag;
    unit_complex_nonconst().y() = lhs_real*rhs_imag + lhs_imag*rhs_real;
  }

  /**
   * \returns group inverse of instance
   */
  inline
  const SO2Group<Scalar> inverse() const {
    return SO2Group<Scalar>(unit_complex().x(), -unit_complex().y());
  }

  /**
   * \brief Logarithmic map
   *
   * \returns tangent space representation (=rotation angle) of instance
   *
   * \see  log().
   */
  inline
  const Scalar log() const {
    return SO2Group<Scalar>::log(*this);
  }

  /**
   * \brief Normalize complex number
   *
   * It re-normalizes complex number to unit length. This method only needs to
   * be called in conjunction with fastMultiply() or data() write access.
   */
  inline
  void normalize() {
    Scalar length =
        std::sqrt(unit_complex().x()*unit_complex().x()
             + unit_complex().y()*unit_complex().y());
    if(length < SophusConstants<Scalar>::epsilon()) {
      throw SophusException("Complex number is (near) zero!");
    }
    unit_complex_nonconst().x() /= length;
    unit_complex_nonconst().y() /= length;
  }

  /**
   * \returns 2x2 matrix representation of instance
   *
   * For SO2, the matrix representation is an orthogonal matrix R with det(R)=1,
   * thus the so-called rotation matrix.
   */
  inline
  const Transformation matrix() const {
    const Scalar & real = unit_complex().x();
    const Scalar & imag = unit_complex().y();
    Transformation R;
    R << real, -imag
        ,imag,  real;
    return R;
  }

  /**
   * \brief Assignment operator
   */
  template<typename OtherDerived> inline
  SO2GroupBase<Derived>& operator=(const SO2GroupBase<OtherDerived> & other) {
    unit_complex_nonconst() = other.unit_complex();
    return *this;
  }

  /**
   * \brief Group multiplication
   * \see operator*=()
   */
  inline
  const SO2Group<Scalar> operator*(const SO2Group<Scalar>& other) const {
    SO2Group<Scalar> result(*this);
    result *= other;
    return result;
  }

  /**
   * \brief Group action on \f$ \mathbf{R}^2 \f$
   *
   * \param p point \f$p \in \mathbf{R}^2 \f$
   * \returns point \f$p' \in \mathbf{R}^2 \f$, rotated version of \f$p\f$
   *
   * This function rotates a point \f$ p \f$ in  \f$ \mathbf{R}^2 \f$ by the
   * SO2 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$.
   */
  inline
  const Point operator*(const Point & p) const {
    const Scalar & real = unit_complex().x();
    const Scalar & imag = unit_complex().y();
    return Point(real*p[0] - imag*p[1], imag*p[0] + real*p[1]);
  }

  /**
   * \brief In-place group multiplication
   *
   * \see fastMultiply()
   * \see operator*()
   */
  inline
  void operator*=(const SO2Group<Scalar>& other) {
    fastMultiply(other);
    normalize();
  }

  /**
   * \brief Setter of internal unit complex number representation
   *
   * \param complex
   * \pre   the complex number must not be near zero
   *
   * The complex number is normalized to unit length.
   */
  inline
  void setComplex(const Point & complex) {
    unit_complex() = complex;
    normalize();
  }

  /**
   * \brief Accessor of unit complex number
   *
   * No direct write access is given to ensure the complex stays normalized.
   */
  EIGEN_STRONG_INLINE
  ConstComplexReference unit_complex() const {
    return static_cast<const Derived*>(this)->unit_complex();
  }

  ////////////////////////////////////////////////////////////////////////////
  // public static functions
  ////////////////////////////////////////////////////////////////////////////

  /**
   * \brief Group exponential
   *
   * \param theta tangent space element (=rotation angle \f$ \theta \f$)
   * \returns     corresponding element of the group SO2
   *
   * To be more specific, this function computes \f$ \exp(\widehat{\theta}) \f$
   * with \f$ \exp(\cdot) \f$ being the matrix exponential
   * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO2.
   *
   * \see hat()
   * \see log()
   */
  inline static
  const SO2Group<Scalar> exp(const Tangent & theta) {
    return SO2Group<Scalar>(std::cos(theta), std::sin(theta));
  }

  /**
   * \brief Generator
   *
   * The infinitesimal generator of SO2
   * is \f$
   *        G_0 = \left( \begin{array}{ccc}
   *                          0& -1& \\
   *                          1&  0&
   *                     \end{array} \right).
   * \f$
   * \see hat()
   */
  inline static
  const Transformation generator() {
    return hat(1);
  }

  /**
   * \brief hat-operator
   *
   * \param theta scalar representation of Lie algebra element
   * \returns     2x2-matrix representatin of Lie algebra element
   *
   * Formally, the hat-operator of SO2 is defined
   * as \f$ \widehat{\cdot}: \mathbf{R}^2 \rightarrow \mathbf{R}^{2\times 2},
   * \quad \widehat{\theta} = G_0\cdot \theta \f$
   * with \f$ G_0 \f$ being the infinitesial generator().
   *
   * \see generator()
   * \see vee()
   */
  inline static
  const Transformation hat(const Tangent & theta) {
    Transformation Omega;
    Omega <<  static_cast<Scalar>(0), -theta
        ,  theta,     static_cast<Scalar>(0);
    return Omega;
  }

  /**
   * \brief Lie bracket
   *
   * \param theta1 scalar representation of Lie algebra element
   * \param theta2 scalar representation of Lie algebra element
   * \returns      zero
   *
   * It computes the bracket. For the Lie algebra so2, the Lie bracket is
   * simply \f$ [\theta_1, \theta_2]_{so2} = 0 \f$ since SO2 is a
   * commutative group.
   *
   * \see hat()
   * \see vee()
   */
  inline static
  const Tangent lieBracket(const Tangent & theta1,
                           const Tangent & theta2) {
    return static_cast<Scalar>(0);
  }

  /**
   * \brief Logarithmic map
   *
   * \param other element of the group SO2
   * \returns     corresponding tangent space element
   *              (=rotation angle \f$ \theta \f$)
   *
   * Computes the logarithmic, the inverse of the group exponential.
   * To be specific, this function computes \f$ \log({\cdot})^\vee \f$
   * with \f$ \vee(\cdot) \f$ being the matrix logarithm
   * and \f$ \vee{\cdot} \f$ the vee()-operator of SO2.
   *
   * \see exp()
   * \see vee()
   */
  inline static
  const Tangent log(const SO2Group<Scalar> & other) {
    // todo: general implementation for Scalar not being float or double.
    return atan2(other.unit_complex_.y(), other.unit_complex().x());
  }

  /**
   * \brief vee-operator
   *
   * \param Omega 2x2-matrix representation of Lie algebra element
   * \pre         Omega need to be a skew-symmetric matrix
   * \returns     scalar representatin of Lie algebra element
   *s
   * This is the inverse of the hat()-operator.
   *
   * \see hat()
   */
  inline static
  const Tangent vee(const Transformation & Omega) {
    return static_cast<Scalar>(0.5)*(Omega(1,0) - Omega(0,1));
  }

private:
  // Mutator of complex number is private so users are hampered
  // from setting non-unit complex numbers.
  EIGEN_STRONG_INLINE
  ComplexReference unit_complex_nonconst() {
    return static_cast<Derived*>(this)->unit_complex_nonconst();
  }

};

/**
 * \brief SO2 default type - Constructors and default storage for SO2 Type
 */
template<typename _Scalar, int _Options>
class SO2Group : public SO2GroupBase<SO2Group<_Scalar,_Options> > {
  typedef SO2GroupBase<SO2Group<_Scalar,_Options> > Base;
public:
  /** \brief scalar type */
  typedef typename internal::traits<SO2Group<_Scalar,_Options> >
  ::Scalar Scalar;
  /** \brief complex number reference type */
  typedef typename internal::traits<SO2Group<_Scalar,_Options> >
  ::ComplexType & ComplexReference;
  /** \brief complex number const reference type */
  typedef const typename internal::traits<SO2Group<_Scalar,_Options> >
  ::ComplexType & ConstComplexReference;

  /** \brief degree of freedom of group */
  static const int DoF = Base::DoF;
  /** \brief number of internal parameters used */
  static const int num_parameters = Base::num_parameters;
  /** \brief group transformations are NxN matrices */
  static const int N = Base::N;
  /** \brief group transfomation type */
  typedef typename Base::Transformation Transformation;
  /** \brief point type */
  typedef typename Base::Point Point;
  /** \brief tangent vector type */
  typedef typename Base::Tangent Tangent;
  /** \brief adjoint transformation type */
  typedef typename Base::Adjoint Adjoint;

  // base is friend so unit_complex_nonconst can be accessed from base
  friend class SO2GroupBase<SO2Group<_Scalar,_Options> >;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * \brief Default constructor
   *
   * Initialize complex number to identity rotation.
   */
  inline SO2Group()
    : unit_complex_(static_cast<Scalar>(1), static_cast<Scalar>(0)) {
  }

  /**
   * \brief Copy constructor
   */
  template<typename OtherDerived> inline
  SO2Group(const SO2GroupBase<OtherDerived> & other)
    : unit_complex_(other.unit_complex()) {
  }

  /**
   * \brief Constructor from rotation matrix
   *
   * \pre rotation matrix need to be orthogonal with determinant of 1
   */
  inline explicit
  SO2Group(const Transformation & R)
    : unit_complex_(static_cast<Scalar>(0.5)*(R(0,0)+R(1,1)),
                    static_cast<Scalar>(0.5)*(R(1,0)-R(0,1))) {
    if (std::abs(R.determinant()-static_cast<Scalar>(1))
        > SophusConstants<Scalar>::epsilon()) {
      throw SophusException("det(R) is not near 1.");
    }
  }

  /**
   * \brief Constructor from pair of real and imaginary number
   *
   * \pre pair must not be zero
   */
  inline SO2Group(const Scalar & real, const Scalar & imag)
    : unit_complex_(real, imag) {
    Base::normalize();
  }

  /**
   * \brief Constructor from 2-vector
   *
   * \pre vector must not be zero
   */
  inline explicit
  SO2Group(const Matrix<Scalar,2,1> & complex)
    : unit_complex_(complex) {
    Base::normalize();
  }

  /**
   * \brief Constructor from std::complex
   *
   * \pre complex number must not be zero
   */
  inline explicit
  SO2Group(const std::complex<Scalar> & complex)
    : unit_complex_(complex.real(), complex.imag()) {
    Base::normalize();
  }

  /**
   * \brief Constructor from an angle
   */
  inline explicit
  SO2Group(Scalar theta) {
    unit_complex_nonconst() = SO2Group<Scalar>::exp(theta).unit_complex();
  }

  /**
   * \brief Accessor of unit complex number
   *
   * No direct write access is given to ensure the complex number stays
   * normalized.
   */
  EIGEN_STRONG_INLINE
  ConstComplexReference unit_complex() const {
    return unit_complex_;
  }

protected:
  // Mutator of complex number is protected so users are hampered
  // from setting non-unit complex numbers.
  EIGEN_STRONG_INLINE
  ComplexReference unit_complex_nonconst() {
    return unit_complex_;
  }

  static bool isNearZero(const Scalar & real, const Scalar & imag) {
    return (real*real + imag*imag < SophusConstants<Scalar>::epsilon());
  }

  Matrix<Scalar,2,1> unit_complex_;
};

} // end namespace


namespace Eigen {
/**
 * \brief Specialisation of Eigen::Map for SO2GroupBase
 *
 * Allows us to wrap SO2 Objects around POD array
 * (e.g. external c style complex number)
 */
template<typename _Scalar, int _Options>
class Map<Sophus::SO2Group<_Scalar>, _Options>
    : public Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> > {
  typedef Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> > Base;

public:
  /** \brief scalar type */
  typedef typename internal::traits<Map>::Scalar Scalar;
  /** \brief complex number reference type */
  typedef typename internal::traits<Map>::ComplexType & ComplexReference;
  /** \brief complex number const reference type */
  typedef const typename internal::traits<Map>::ComplexType &
  ConstComplexReference;

  /** \brief degree of freedom of group */
  static const int DoF = Base::DoF;
  /** \brief number of internal parameters used */
  static const int num_parameters = Base::num_parameters;
  /** \brief group transformations are NxN matrices */
  static const int N = Base::N;
  /** \brief group transfomation type */
  typedef typename Base::Transformation Transformation;
  /** \brief point type */
  typedef typename Base::Point Point;
  /** \brief tangent vector type */
  typedef typename Base::Tangent Tangent;
  /** \brief adjoint transformation type */
  typedef typename Base::Adjoint Adjoint;

  // base is friend so unit_complex_nonconst can be accessed from base
  friend class Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> >;

  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
  using Base::operator*=;
  using Base::operator*;

  EIGEN_STRONG_INLINE
  Map(Scalar* coeffs) : unit_complex_(coeffs) {
  }

  /**
   * \brief Accessor of unit complex number
   *
   * No direct write access is given to ensure the complex number stays
   * normalized.
   */
  EIGEN_STRONG_INLINE
  ConstComplexReference unit_complex() const {
    return unit_complex_;
  }

protected:
  // Mutator of complex number is protected so users are hampered
  // from setting non-unit complex number.
  EIGEN_STRONG_INLINE
  ComplexReference unit_complex_nonconst() {
    return unit_complex_;
  }

  Map<Matrix<Scalar,2,1>,_Options> unit_complex_;
};

/**
 * \brief Specialisation of Eigen::Map for const SO2GroupBase
 *
 * Allows us to wrap SO2 Objects around POD array
 * (e.g. external c style complex number)
 */
template<typename _Scalar, int _Options>
class Map<const Sophus::SO2Group<_Scalar>, _Options>
    : public Sophus::SO2GroupBase<
    Map<const Sophus::SO2Group<_Scalar>, _Options> > {
  typedef Sophus::SO2GroupBase<Map<const Sophus::SO2Group<_Scalar>, _Options> >
  Base;

public:
  /** \brief scalar type */
  typedef typename internal::traits<Map>::Scalar Scalar;
  /** \brief complex number const reference type */
  typedef const typename internal::traits<Map>::ComplexType &
  ConstComplexReference;


  /** \brief degree of freedom of group */
  static const int DoF = Base::DoF;
  /** \brief number of internal parameters used */
  static const int num_parameters = Base::num_parameters;
  /** \brief group transformations are NxN matrices */
  static const int N = Base::N;
  /** \brief group transfomation type */
  typedef typename Base::Transformation Transformation;
  /** \brief point type */
  typedef typename Base::Point Point;
  /** \brief tangent vector type */
  typedef typename Base::Tangent Tangent;
  /** \brief adjoint transformation type */
  typedef typename Base::Adjoint Adjoint;

  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
  using Base::operator*=;
  using Base::operator*;

  EIGEN_STRONG_INLINE
  Map(const Scalar* coeffs) : unit_complex_(coeffs) {
  }

  /**
   * \brief Accessor of unit complex number
   *
   * No direct write access is given to ensure the complex number stays
   * normalized.
   */
  EIGEN_STRONG_INLINE
  ConstComplexReference unit_complex() const {
    return unit_complex_;
  }

protected:
  const Map<const Matrix<Scalar,2,1>,_Options> unit_complex_;
};

}


#endif // SOPHUS_SO2_HPP