//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#pragma once

// IWYU pragma: private, include "pinocchio/spatial.hpp"

#ifdef PINOCCHIO_LSP
  #undef PINOCCHIO_LSP
  #include "pinocchio/spatial.hpp"
#endif // PINOCCHIO_LSP

namespace pinocchio
{

  template<typename Scalar, int Options>
  struct SE3GroupAction<MotionZeroTpl<Scalar, Options>>
  {
    typedef MotionZeroTpl<Scalar, Options> ReturnType;
  };

  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction<MotionZeroTpl<Scalar, Options>, MotionDerived>
  {
    typedef MotionZeroTpl<Scalar, Options> ReturnType;
  };

  template<typename _Scalar, int _Options>
  struct traits<MotionZeroTpl<_Scalar, _Options>>
  {
    static constexpr int Options = _Options;
    static constexpr int LINEAR = 0;
    static constexpr int ANGULAR = 3;
    typedef _Scalar Scalar;
    typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
    typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
    typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
    typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
    typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef Vector3 AngularType;
    typedef const Vector3 ConstAngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstLinearType;
    typedef MotionTpl<Scalar, _Options> MotionPlain;
    typedef MotionPlain PlainReturnType;

  }; // traits MotionZeroTpl

  template<typename Scalar, int Options>
  struct MotionZeroTpl : public MotionBase<MotionZeroTpl<Scalar, Options>>
  {
    typedef typename traits<MotionZeroTpl>::MotionPlain MotionPlain;
    typedef typename traits<MotionZeroTpl>::PlainReturnType PlainReturnType;

    static PlainReturnType plain()
    {
      return MotionPlain::Zero();
    }

    template<typename D2>
    static bool isEqual_impl(const MotionDense<D2> & other)
    {
      return other.linear().isZero(0) && other.angular().isZero(0);
    }

    static bool isEqual_impl(const MotionZeroTpl &)
    {
      return true;
    }

    template<typename D2>
    static void addTo(const MotionBase<D2> &)
    {
    }

    template<typename D2>
    static void setTo(MotionBase<D2> & other)
    {
      other.setZero();
    }

    template<typename M1>
    MotionZeroTpl motionAction(const MotionBase<M1> &) const
    {
      return MotionZeroTpl();
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2, O2> &, MotionDense<D2> & v) const
    {
      v.setZero();
    }

    template<typename S2, int O2>
    MotionZeroTpl se3Action_impl(const SE3Tpl<S2, O2> &) const
    {
      return MotionZeroTpl();
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2, O2> &, MotionDense<D2> & v) const
    {
      v.setZero();
    }

    template<typename S2, int O2>
    MotionZeroTpl se3ActionInverse_impl(const SE3Tpl<S2, O2> &) const
    {
      return MotionZeroTpl();
    }

  }; // struct MotionZeroTpl

  template<typename M1, typename Scalar, int Options>
  inline const M1 & operator+(const MotionBase<M1> & v, const MotionZeroTpl<Scalar, Options> &)
  {
    return v.derived();
  }

  template<typename Scalar, int Options, typename M1>
  inline const M1 & operator+(const MotionZeroTpl<Scalar, Options> &, const MotionBase<M1> & v)
  {
    return v.derived();
  }

} // namespace pinocchio
