//
// Copyright (c) 2016-2020 CNRS INRIA
//
#pragma once

// IWYU pragma: begin_keep
#include <cassert>
#include <cstddef>
#include <cmath>
#include <complex>

#include <Eigen/Core>
#include <Eigen/Cholesky>

#include "pinocchio/macros.hpp"
#include "pinocchio/unsupported.hpp"
#include "pinocchio/eigen-common.hpp"

#include "pinocchio/math.hpp"
#include "pinocchio/multibody.hpp"

#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
// IWYU pragma: end_keep

namespace pinocchio
{

  ///
  /// \brief Compute the forward dynamics with contact constraints. Internally,
  /// pinocchio::computeAllTerms is called.
  /// \warning This function is no more supported.
  /// Please use the class RigidConstraintModel to define new contacts, and
  /// initConstraintDynamics(model, data, contact_models) and
  /// constraintDynamics(model, data, q, v, tau, contact_models, contact_datas[,prox_settings])
  /// instead.
  /// \note It solves the following problem: <BR>
  ///       <CENTER> \f$ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} -
  ///       \ddot{q}_{\text{free}} \|_{M(q)} \\{}
  ///           \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$
  ///           </CENTER> <BR>
  ///       where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without
  ///       constraints), \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$
  ///       \gamma \f$ is the constraint drift.
  ///  By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse
  ///  is performed.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
  /// \tparam TangentVectorType2 Type of the joint torque vector.
  /// \tparam ConstraintMatrixType Type of the constraint matrix.
  /// \tparam DriftVectorType Type of the drift vector.

  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration (vector dim model.nq).
  /// \param[in] v The joint velocity (vector dim model.nv).
  /// \param[in] tau The joint torque vector (dim model.nv).
  /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
  /// \param[in] gamma The drift of the constraints (dim nb_constraints).
  /// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if
  /// constraints are full rank.
  ///            \note A hint: 1e-12 as the damping factor gave good result in the particular case
  ///            of redundancy in contact constraints on the two feet.
  ///
  /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers
  /// linked to the contact forces are available throw data.lambda_c vector.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType1,
    typename TangentVectorType2,
    typename ConstraintMatrixType,
    typename DriftVectorType>
  PINOCCHIO_UNSUPPORTED inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::
    TangentVectorType &
    forwardDynamics(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      DataTpl<Scalar, Options, JointCollectionTpl> & data,
      const Eigen::MatrixBase<ConfigVectorType> & q,
      const Eigen::MatrixBase<TangentVectorType1> & v,
      const Eigen::MatrixBase<TangentVectorType2> & tau,
      const Eigen::MatrixBase<ConstraintMatrixType> & J,
      const Eigen::MatrixBase<DriftVectorType> & gamma,
      const Scalar inv_damping = 0.);

  ///
  /// \brief Compute the forward dynamics with contact constraints, assuming
  /// pinocchio::computeAllTerms has been called.
  /// \warning This function is no more supported.
  /// Please use the class RigidConstraintModel to define new contacts, and
  /// initConstraintDynamics(model, data, contact_models) and
  /// constraintDynamics(model, data, q, v, tau, contact_models, contact_datas[,prox_settings])
  /// instead.
  /// \note It solves the following problem: <BR>
  ///       <CENTER> \f$ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} -
  ///       \ddot{q}_{\text{free}} \|_{M(q)} \\{}
  ///           \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$
  ///           </CENTER> <BR>
  ///       where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without
  ///       constraints), \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$
  ///       \gamma \f$ is the constraint drift.
  ///  By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse
  ///  is performed.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
  /// \tparam TangentVectorType2 Type of the joint torque vector.
  /// \tparam ConstraintMatrixType Type of the constraint matrix.
  /// \tparam DriftVectorType Type of the drift vector.

  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] tau The joint torque vector (dim model.nv).
  /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
  /// \param[in] gamma The drift of the constraints (dim nb_constraints).
  /// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if
  /// constraints are full rank.
  ///            \note A hint: 1e-12 as the damping factor gave good result in the particular case
  ///            of redundancy in contact constraints on the two feet.
  ///
  /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers
  /// linked to the contact forces are available throw data.lambda_c vector.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename TangentVectorType,
    typename ConstraintMatrixType,
    typename DriftVectorType>
  PINOCCHIO_UNSUPPORTED inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::
    TangentVectorType &
    forwardDynamics(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      DataTpl<Scalar, Options, JointCollectionTpl> & data,
      const Eigen::MatrixBase<TangentVectorType> & tau,
      const Eigen::MatrixBase<ConstraintMatrixType> & J,
      const Eigen::MatrixBase<DriftVectorType> & gamma,
      const Scalar inv_damping = 0.);

  ///
  /// \brief Computes the inverse of the KKT matrix for dynamics with contact constraints.
  /// It computes the following matrix: <BR>
  ///       <CENTER> \f$
  ///       \left[\begin{matrix}\mathbf{M}^{-1}-\mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1}
  ///       & \mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}
  ///       \\ \widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} &
  ///       -\widehat{\mathbf{M}}^{-1}\end{matrix}\right] \f$ </CENTER> <BR>
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration (vector dim model.nq).
  /// \param[in] J Jacobian of the constraints.
  /// \param[out] KKTMatrix_inv inverse of the MJtJ matrix.
  /// \param[in] inv_damping regularization coefficient.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename ConstraintMatrixType,
    typename KKTMatrixType>
  void computeKKTContactDynamicMatrixInverse(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<ConstraintMatrixType> & J,
    const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv,
    const Scalar & inv_damping = 0.);

  ///
  /// \brief Computes the inverse of the KKT matrix for dynamics with contact constraints.
  /// \warning forwardDynamics/impuseDynamics are unsupported algorithm, and this function signature
  /// needs forwardDynamics to be called before. Please use the inverse() function in
  /// ConstraintCholeskyDecomposition class instead. It computes the following matrix: <BR>
  ///       <CENTER> \f$
  ///       \left[\begin{matrix}\mathbf{M}^{-1}-\mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1}
  ///       & \mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}
  ///       \\ \widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} &
  ///       -\widehat{\mathbf{M}}^{-1}\end{matrix}\right] \f$ </CENTER> <BR>
  ///
  /// \remarks The matrix is defined when one's call forwardDynamics/impulseDynamics. This method
  /// makes use of the matrix decompositions performed during the forwardDynamics/impulseDynamics
  /// and returns the inverse. The jacobian should be the same that the one provided to
  /// forwardDynamics/impulseDynamics. Thus forward Dynamics/impulseDynamics should have been called
  /// first.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] J Jacobian of the constraints.
  /// \param[out] KKTMatrix_inv inverse of the MJtJ matrix.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConstraintMatrixType,
    typename KKTMatrixType>
  PINOCCHIO_UNSUPPORTED void getKKTContactDynamicMatrixInverse(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Eigen::MatrixBase<ConstraintMatrixType> & J,
    const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv);

  ///
  /// \brief Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is
  /// called.
  /// \warning This function is no more supported.
  /// Please use the class RigidConstraintModel to define new contacts, and
  /// initConstraintDynamics(model, data, contact_models) and
  /// impulseDynamics(model, data, q, v_before, contact_models, contact_datas[,r_coeff, mu])
  /// instead.
  /// \note It solves the following problem: <BR>
  ///       <CENTER> \f$ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} -
  ///       \dot{q}^{-} \|_{M(q)} \\{}
  ///           \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-}  \end{eqnarray} \f$
  ///           </CENTER> <BR>
  ///       where \f$ \dot{q}^{-} \f$ is the generalized velocity before impact,
  ///       \f$ M \f$ is the joint space mass matrix, \f$ J \f$ the constraint Jacobian and \f$
  ///       \epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a
  ///       rigid impact).
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  /// \tparam TangentVectorType Type of the joint velocity vector.
  /// \tparam ConstraintMatrixType Type of the constraint matrix.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration (vector dim model.nq).
  /// \param[in] v_before The joint velocity before impact (vector dim model.nv).
  /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
  /// \param[in] r_coeff The coefficient of restitution. Must be in [0;1].
  /// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if
  /// constraints are full rank.
  ///
  /// \return A reference to the generalized velocity after impact stored in data.dq_after. The
  /// Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename ConstraintMatrixType>
  PINOCCHIO_UNSUPPORTED inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::
    TangentVectorType &
    impulseDynamics(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      DataTpl<Scalar, Options, JointCollectionTpl> & data,
      const Eigen::MatrixBase<ConfigVectorType> & q,
      const Eigen::MatrixBase<TangentVectorType> & v_before,
      const Eigen::MatrixBase<ConstraintMatrixType> & J,
      const Scalar r_coeff = 0.,
      const Scalar inv_damping = 0.);

  ///
  /// \brief Compute the impulse dynamics with contact constraints, assuming pinocchio::crba has
  /// been called.
  /// \warning This function is no more supported.
  /// Please use the class RigidConstraintModel to define new contacts, and
  /// initConstraintDynamics(model, data, contact_models) and
  /// impulseDynamics(model, data, q, v_before, contact_models, contact_datas[,r_coeff, mu])
  /// instead.
  /// \note It solves the following problem: <BR>
  ///       <CENTER> \f$ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} -
  ///       \dot{q}^{-} \|_{M(q)} \\{}
  ///           \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-}  \end{eqnarray} \f$
  ///           </CENTER> <BR>
  ///       where \f$ \dot{q}^{-} \f$ is the generalized velocity before impact,
  ///       \f$ M \f$ is the joint space mass matrix, \f$ J \f$ the constraint Jacobian and \f$
  ///       \epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a
  ///       rigid impact).
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  /// \tparam TangentVectorType Type of the joint velocity vector.
  /// \tparam ConstraintMatrixType Type of the constraint matrix.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] v_before The joint velocity before impact (vector dim model.nv).
  /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
  /// \param[in] r_coeff The coefficient of restitution. Must be in [0;1].
  /// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if
  /// constraints are full rank.
  ///
  /// \return A reference to the generalized velocity after impact stored in data.dq_after. The
  /// Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename ConstraintMatrixType>
  PINOCCHIO_UNSUPPORTED inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::
    TangentVectorType &
    impulseDynamics(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      DataTpl<Scalar, Options, JointCollectionTpl> & data,
      const Eigen::MatrixBase<TangentVectorType> & v_before,
      const Eigen::MatrixBase<ConstraintMatrixType> & J,
      const Scalar r_coeff = 0.,
      const Scalar inv_damping = 0.);

} // namespace pinocchio

// IWYU pragma: begin_exports
#include "pinocchio/src/algorithm/contact-dynamics.hxx"
// IWYU pragma: end_exports
