/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/optional_ref.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/poses/poses_frwds.h>

/** \defgroup mrpt_poses_lie_grp Lie Algebra methods for SO(2),SO(3),SE(2),SE(3)
 * \ingroup poses_grp */

namespace mrpt::poses::Lie
{
/** \addtogroup mrpt_poses_lie_grp
 *  @{ */

/** Traits for SE(n), rigid-body transformations in R^n space.
 * \ingroup mrpt_poses_lie_grp
 */
template <unsigned int n>
struct SE;

/** Traits for SE(3), rigid-body transformations in R^3 space.
 * See indidual members for documentation, or \cite blanco_se3_tutorial for a
 * general overview.
 * \ingroup mrpt_poses_lie_grp
 */
template <>
struct SE<3>
{
  /** Number of actual degrees of freedom for this transformation */
  constexpr static size_t DOFs = 6;

  /** Dimensionality of the matrix manifold (3x4=12 upper part of the 4x4) */
  constexpr static size_t MANIFOLD_DIM = 3 * 4;

  using tangent_vector = mrpt::math::CVectorFixedDouble<DOFs>;
  using manifold_vector = mrpt::math::CVectorFixedDouble<MANIFOLD_DIM>;

  using type = CPose3D;
  using light_type = mrpt::math::TPose3D;

  /** Type for Jacobian: tangent space -> SO(n) matrix */
  using tang2mat_jacob = mrpt::math::CMatrixDouble12_6;

  /** Type for Jacobian: SO(n) matrix -> tangent space */
  using mat2tang_jacob = mrpt::math::CMatrixDouble6_12;

  /** Type for Jacobians between so(n) vectors on the tangent space */
  using matrix_TxT = mrpt::math::CMatrixDouble66;

  /** Type for Jacobians between SO(n) 3x4 (sub)matrices in the manifold */
  using matrix_MxM = mrpt::math::CMatrixFixed<double, 12, 12>;

  /** Retraction to SE(3), a **pseudo-exponential** map \f$ x \rightarrow
   * PseudoExp(x^\wedge) \f$ and its Jacobian.
   * - Input: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]
   * - Output: translation and rotation in SE(3) as CPose3D
   * Note that this method implements retraction via a **pseudo-exponential**,
   * where only the rotational part undergoes a real matrix exponential,
   * while the translation is left unmodified. This is done for computational
   * efficiency, and does not change the results of optimizations as long
   * as the corresponding local coordinates (pseudo-logarithm) are used as
   * well.
   *
   * See section 9.4.2 in \cite blanco_se3_tutorial
   */
  static type exp(const tangent_vector& x);

  /** SE(3) **pseudo-logarithm** map \f$ \mathbf{R} \rightarrow
   * \log(\mathbf{R}^\vee)\f$
   * - Input: translation and rotation in SE(3) as CPose3D
   * - Output: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]
   *
   * See exp() for the explanation about the "pseudo" name.
   * For the formulas, see section 9.4.2 in \cite blanco_se3_tutorial
   */
  static tangent_vector log(const type& P);

  /** Returns a vector with all manifold matrix elements in column-major
   * order. For SE(3), it is a 3x4=12 vector. */
  static manifold_vector asManifoldVector(const type& pose);

  /** The inverse operation of asManifoldVector() */
  static type fromManifoldVector(const manifold_vector& v);

  /** Jacobian for the pseudo-exponential exp().
   * See 10.3.1 in \cite blanco_se3_tutorial
   *
   * - Input: 6-len vector in Lie algebra se(3)
   * - Jacobian: Jacobian of the 3x4 matrix (stacked as a column major
   * 12-vector) wrt the vector in the tangent space.
   */
  static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x);

  /** Jacobian for the pseudo-logarithm log()
   * See 10.3.11 in \cite blanco_se3_tutorial
   *
   * - Input: a SE(3) pose as a CPose3D object
   * - Jacobian: Jacobian of the tangent space vector wrt the 3x4 matrix
   * elements (stacked as a column major 12-vector).
   */
  static mat2tang_jacob jacob_dlogv_dv(const type& P);

  /** Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
   * \note Section 10.3.3 in \cite blanco_se3_tutorial
   */
  static tang2mat_jacob jacob_dexpeD_de(const CPose3D& D);

  /** Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.
   * \note Section 10.3.4 in \cite blanco_se3_tutorial
   */
  static tang2mat_jacob jacob_dDexpe_de(const CPose3D& D);

  /** Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie
   * Algebra.
   * \note Eq. 10.3.7 in \cite blanco_se3_tutorial
   */
  static tang2mat_jacob jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D);

  /** Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices,
   * with respect to A.
   * \note See section 7.3.1 of in \cite blanco_se3_tutorial
   */
  static matrix_MxM jacob_dAB_dA(const type& A, const type& B);

  /** Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices,
   * with respect to B.
   * \note See section 7.3.1 of in \cite blanco_se3_tutorial
   */
  static matrix_MxM jacob_dAB_dB(const type& A, const type& B);

  /** Return one or both of the following 6x6 Jacobians, useful in
   * graph-slam problems:
   * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
   * \f[  \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\f]
   * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ increments in the
   * linearized manifold for P1 and P2.
   * \note Section 10.3.10 in \cite blanco_se3_tutorial
   */
  static void jacob_dDinvP1invP2_de1e2(
      const type& Dinv,
      const type& P1,
      const type& P2,
      mrpt::optional_ref<matrix_TxT> df_de1 = std::nullopt,
      mrpt::optional_ref<matrix_TxT> df_de2 = std::nullopt);
};

/** Traits for SE(2), rigid-body transformations in R^2 space.
 * See indidual members for documentation, or \cite blanco_se3_tutorial for a
 * general overview.
 * \ingroup mrpt_poses_lie_grp
 */
template <>
struct SE<2>
{
  /** Number of actual degrees of freedom for this transformation */
  constexpr static size_t DOFs = 3;

  /** Dimensionality of the matrix manifold; this should be 3x3=9 for SE(2),
   * but for efficiency, we define it as simply 3x1=3 and use the (x,y,phi)
   * representation as well as the "manifold". This is done for API
   * consistency with SE(3), where the actual matrix is used instead. */
  constexpr static size_t MANIFOLD_DIM = 3;

  using tangent_vector = mrpt::math::CVectorFixedDouble<DOFs>;
  using manifold_vector = mrpt::math::CVectorFixedDouble<MANIFOLD_DIM>;

  using type = CPose2D;
  using light_type = mrpt::math::TPose2D;

  /** Type for Jacobians between SO(n) transformations */
  using matrix_TxT = mrpt::math::CMatrixDouble33;

  /** In SE(3), this type represents Jacobians between SO(n) (sub)matrices in
   * the manifold, but in SE(2) it simply models Jacobians between (x,y,phi)
   * vectors. */
  using matrix_MxM = mrpt::math::CMatrixDouble33;

  /** Type for Jacobian: tangent space -> SO(n) matrix */
  using tang2mat_jacob = mrpt::math::CMatrixDouble33;
  /** Type for Jacobian: SO(n) matrix -> tangent space */
  using mat2tang_jacob = mrpt::math::CMatrixDouble33;

  /** Exponential map in SE(2), takes [x,y,phi] and returns a CPose2D */
  static type exp(const tangent_vector& x);

  /** Logarithm map in SE(2), takes a CPose2D and returns [X,Y, phi] */
  static tangent_vector log(const type& P);

  /** Returns a vector with all manifold matrix elements in column-major
   * order. For SE(2), though, it directly returns the vector [x,y,phi] for
   * efficiency in comparison to 3x3 homogeneous coordinates. */
  static manifold_vector asManifoldVector(const type& pose);

  /** The inverse operation of asManifoldVector() */
  static type fromManifoldVector(const manifold_vector& v);

  /** Jacobian of the pose composition A*B for SE(2) with respect to A.
   * \note See appendix B of \cite blanco_se3_tutorial
   */
  static matrix_MxM jacob_dAB_dA(const type& A, const type& B);

  /** Jacobian of the pose composition A*B for SE(2) with respect to B.
   * \note See appendix B of \cite blanco_se3_tutorial
   */
  static matrix_MxM jacob_dAB_dB(const type& A, const type& B);

  /** Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.
   * \note See appendix B in \cite blanco_se3_tutorial
   */
  static tang2mat_jacob jacob_dDexpe_de(const type& D);

  /** Return one or both of the following 3x3 Jacobians, useful in
   * graph-slam problems:
   * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
   * \f[  \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\f]
   * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ increments in the
   * linearized manifold for P1 and P2.
   * \note See appendix B in \cite blanco_se3_tutorial
   */
  static void jacob_dDinvP1invP2_de1e2(
      const type& Dinv,
      const type& P1,
      const type& P2,
      mrpt::optional_ref<matrix_TxT> df_de1 = std::nullopt,
      mrpt::optional_ref<matrix_TxT> df_de2 = std::nullopt);
};

}  // namespace mrpt::poses::Lie
