//

// Copyright (c) 2018 CNRS

//

#ifndef __pinocchio_cartesian_product_variant_hpp__

#define __pinocchio_cartesian_product_variant_hpp__

#include "pinocchio/multibody/liegroup/liegroup-base.hpp"

#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"

#include "pinocchio/container/aligned-vector.hpp"

namespace pinocchio

{

  template<typename Scalar, int Options = 0,

    template<typename,int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>

      struct CartesianProductOperationVariantTpl;

  typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl> CartesianProductOperationVariant;

  template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>

  struct traits<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >

  {

    typedef _Scalar Scalar;

    enum {

      Options = _Options,

      NQ = Eigen::Dynamic,

      NV = Eigen::Dynamic

    };

  };

  ///

  /// \brief Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant

  ///

  template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>

  struct CartesianProductOperationVariantTpl

  : public LieGroupBase<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >

  {

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperationVariantTpl);

    typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;

    typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;

    typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;

    /// \brief Default constructor

    : m_nq(0), m_nv(0)

    , lg_nqs(0), lg_nvs(0)

    ///

    /// \brief Constructor with one single Lie group

    ///

    /// \param[in] lg Lie group variant to insert inside the Cartesian product

    ///

    : m_nq(0), m_nv(0)

    , lg_nqs(0), lg_nvs(0)

    {

    ///

    /// \brief Constructor with two Lie groups

    ///

    /// \param[in] lg1 Lie group variant to insert inside the Cartesian product

    /// \param[in] lg2 Lie group variant to insert inside the Cartesian product

    ///

                                        const LieGroupGeneric & lg2)

    : m_nq(0), m_nv(0)

    , lg_nqs(0), lg_nvs(0)

    {

    ///

    /// \brief Append a Lie group to the Cartesian product

    ///

    /// \param[in] lg Lie group variant to insert inside the Cartesian product

    ///

    void append(const LieGroupGeneric & lg);

    ///

    /// \brief Append a Lie group to the Cartesian product

    ///

    /// \param[in] lg Lie group to insert inside the Cartesian product

    ///

    template<typename LieGroupDerived>

    {

    ///

    /// \brief Cartesian product between *this and other.

    ///

    /// \param[in] other CartesianProductOperation to compose with this

    ///

    /// \returns A new Cartesian product betwenn *this and other.

    ///

    CartesianProductOperationVariantTpl operator* (const CartesianProductOperationVariantTpl& other) const;

    ///

    /// \brief Append other to *this.

    ///

    /// \param[in] other CartesianProductOperation to append to *this.

    ///

    CartesianProductOperationVariantTpl& operator*= (const CartesianProductOperationVariantTpl& other);

    ///

    /// \brief Append a Lie group to *this.

    ///

    /// \param[in] lg LieGroupGeneric to append to *this.

    ///

    inline CartesianProductOperationVariantTpl& operator*= (const LieGroupGeneric& lg)

    {

      append(lg);

      return *this;

    }

    ///

    /// \brief Append a Lie group to *this.

    ///

    /// \param[in] lg LieGroupGeneric to append to *this.

    ///

    template<typename LieGroupDerived>

    inline CartesianProductOperationVariantTpl& operator*= (const LieGroupBase<LieGroupDerived>& lg)

    {

      append<LieGroupDerived>(lg);

      return *this;

    }

    template <class ConfigL_t, class ConfigR_t, class Tangent_t>

    void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,

                         const Eigen::MatrixBase<ConfigR_t> & q1,

                         const Eigen::MatrixBase<Tangent_t> & d) const;

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>

    void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,

                          const Eigen::MatrixBase<ConfigR_t> & q1,

                          const Eigen::MatrixBase<JacobianOut_t> & J) const;

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>

    void dDifference_product_impl(const ConfigL_t & q0,

                                  const ConfigR_t & q1,

                                  const JacobianIn_t & Jin,

                                  JacobianOut_t & Jout,

                                  bool dDifferenceOnTheLeft,

                                  const AssignmentOperatorType op) const;

    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>

    void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,

                        const Eigen::MatrixBase<Velocity_t> & v,

                        const Eigen::MatrixBase<ConfigOut_t> & qout) const;

    template <class Config_t, class Jacobian_t>

    void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,

                                         const Eigen::MatrixBase<Jacobian_t> & J) const;

    template <class Config_t, class Tangent_t, class JacobianOut_t>

    void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,

                            const Eigen::MatrixBase<Tangent_t> & v,

                            const Eigen::MatrixBase<JacobianOut_t> & J,

                            const AssignmentOperatorType op=SETTO) const;

    template <class Config_t, class Tangent_t, class JacobianOut_t>

    void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,

                            const Eigen::MatrixBase<Tangent_t> & v,

                            const Eigen::MatrixBase<JacobianOut_t> & J,

                            const AssignmentOperatorType op=SETTO) const;

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>

    void dIntegrate_product_impl(const Config_t & q,

                                 const Tangent_t & v,

                                 const JacobianIn_t & Jin,

                                 JacobianOut_t & Jout,

                                 bool dIntegrateOnTheLeft,

                                 const ArgumentPosition arg,

                                 const AssignmentOperatorType op) const;

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>

    void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,

                                     const Eigen::MatrixBase<Tangent_t> & v,

                                     const Eigen::MatrixBase<JacobianIn_t> & J_in,

                                     const Eigen::MatrixBase<JacobianOut_t> & J_out) const;

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>

    void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,

                                     const Eigen::MatrixBase<Tangent_t> & v,

                                     const Eigen::MatrixBase<JacobianIn_t> & J_in,

                                     const Eigen::MatrixBase<JacobianOut_t> & J_out) const;

    template <class Config_t, class Tangent_t, class JacobianOut_t>

    void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,

                                     const Eigen::MatrixBase<Tangent_t> & v,

                                     const Eigen::MatrixBase<JacobianOut_t> & J) const;

    template <class Config_t, class Tangent_t, class JacobianOut_t>

    void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,

                                     const Eigen::MatrixBase<Tangent_t> & v,

                                     const Eigen::MatrixBase<JacobianOut_t> & J) const;

    template <class ConfigL_t, class ConfigR_t>

    Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,

                                const Eigen::MatrixBase<ConfigR_t> & q1) const;

    template <class Config_t>

    void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const;

    template <class Config_t>

    bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qout,

                            const Scalar& prec) const;

    template <class Config_t>

    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const;

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>

    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,

                                  const Eigen::MatrixBase<ConfigR_t> & upper,

                                  const Eigen::MatrixBase<ConfigOut_t> & qout) const;

    template <class ConfigL_t, class ConfigR_t>

    bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,

                                  const Eigen::MatrixBase<ConfigR_t> & q1,

                                  const Scalar & prec) const;

    bool isEqual_impl (const CartesianProductOperationVariantTpl& other) const;

    template <typename LieGroup1, typename LieGroup2>

    bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;

  protected:

    PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;

    Index m_nq, m_nv;

    std::vector<Index> lg_nqs, lg_nvs;

    std::string m_name;

    ConfigVector_t m_neutral;

  };

} // namespace pinocchio

#include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>

#endif // ifndef __pinocchio_cartesian_product_variant_hpp__