//
// Copyright (c) 2017-2019 CNRS INRIA
//

#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 Vector6ArgType>
  struct traits<MotionRef<Vector6ArgType>>
  {
    typedef typename Vector6ArgType::Scalar Scalar;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
    static constexpr int LINEAR = 0;
    static constexpr int ANGULAR = 3;
    static constexpr int Options = Vector6::Options;
    typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
    typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
    typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
    typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef MotionTpl<Scalar, Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
    typedef DataRefType ToVectorReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
    typedef ConstDataRefType ToVectorConstReturnType;
    typedef MotionRef<Vector6ArgType> MotionRefType;

  }; // traits MotionRef

  template<typename Vector6ArgType>
  struct SE3GroupAction<MotionRef<Vector6ArgType>>
  {
    typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
  };

  template<typename Vector6ArgType, typename MotionDerived>
  struct MotionAlgebraAction<MotionRef<Vector6ArgType>, MotionDerived>
  {
    typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
  };

  namespace internal
  {
    template<typename Vector6ArgType, typename Scalar>
    struct RHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar>
    {
      typedef typename pinocchio::traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
    };

    template<typename Vector6ArgType, typename Scalar>
    struct LHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar>
    {
      typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
    };
  } // namespace internal

  template<typename Vector6ArgType>
  class MotionRef : public MotionDense<MotionRef<Vector6ArgType>>
  {
  public:
    typedef MotionDense<MotionRef> Base;
    typedef typename traits<MotionRef>::DataRefType DataRefType;
    MOTION_TYPEDEF_TPL(MotionRef);

    using Base::operator=;
    using Base::angular;
    using Base::linear;

    using Base::__mequ__;
    using Base::__minus__;
    using Base::__mult__;
    using Base::__opposite__;
    using Base::__pequ__;
    using Base::__plus__;

    /// \brief Default constructor from a 6 dimensional vector.
    MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
    : m_ref(v_like)
    {
      EIGEN_STATIC_ASSERT(
        Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
      assert(v_like.size() == 6);
    }

    /// \brief Copy constructor from another MotionRef.
    MotionRef(const MotionRef & other)
    : m_ref(other.m_ref)
    {
    }

    ToVectorConstReturnType toVector_impl() const
    {
      return m_ref;
    }
    ToVectorReturnType toVector_impl()
    {
      return m_ref;
    }

    // Getters
    ConstAngularType angular_impl() const
    {
      return ConstAngularType(m_ref.derived(), ANGULAR);
    }
    ConstLinearType linear_impl() const
    {
      return ConstLinearType(m_ref.derived(), LINEAR);
    }
    AngularType angular_impl()
    {
      return m_ref.template segment<3>(ANGULAR);
    }
    LinearType linear_impl()
    {
      return m_ref.template segment<3>(LINEAR);
    }

    template<typename V3>
    void angular_impl(const Eigen::MatrixBase<V3> & w)
    {
      PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3, 3);
      angular_impl() = w;
    }

    template<typename V3>
    void linear_impl(const Eigen::MatrixBase<V3> & v)
    {
      PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3, 3);
      linear_impl() = v;
    }

    // Specific operators for MotionTpl and MotionRef
    template<typename S1, int O1>
    MotionPlain __plus__(const MotionTpl<S1, O1> & v) const
    {
      return MotionPlain(m_ref + v.toVector());
    }

    template<typename Vector6Like>
    MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
    {
      return MotionPlain(m_ref + v.toVector());
    }

    template<typename S1, int O1>
    MotionPlain __minus__(const MotionTpl<S1, O1> & v) const
    {
      return MotionPlain(m_ref - v.toVector());
    }

    template<typename Vector6Like>
    MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
    {
      return MotionPlain(m_ref - v.toVector());
    }

    template<typename S1, int O1>
    MotionRef & __pequ__(const MotionTpl<S1, O1> & v)
    {
      m_ref += v.toVector();
      return *this;
    }

    template<typename Vector6Like>
    MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v)
    {
      m_ref += v.toVector();
      return *this;
    }

    template<typename S1, int O1>
    MotionRef & __mequ__(const MotionTpl<S1, O1> & v)
    {
      m_ref -= v.toVector();
      return *this;
    }

    template<typename Vector6Like>
    MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v)
    {
      m_ref -= v.toVector();
      return *this;
    }

    template<typename OtherScalar>
    MotionPlain __mult__(const OtherScalar & alpha) const
    {
      return MotionPlain(alpha * m_ref);
    }

    MotionRef & ref()
    {
      return *this;
    }

    inline PlainReturnType plain() const
    {
      return PlainReturnType(m_ref);
    }

  protected:
    DataRefType m_ref;

  }; // class MotionRef<Vector6Like>

  template<typename Vector6ArgType>
  struct traits<MotionRef<const Vector6ArgType>>
  {
    typedef typename Vector6ArgType::Scalar Scalar;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
    static constexpr int LINEAR = 0;
    static constexpr int ANGULAR = 3;
    static constexpr int Options = Vector6::Options;
    typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
    typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
    typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef ConstLinearType LinearType;
    typedef ConstAngularType AngularType;
    typedef MotionTpl<Scalar, Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
    typedef ConstDataRefType ToVectorConstReturnType;
    typedef ConstDataRefType DataRefType;
    typedef DataRefType ToVectorReturnType;
    typedef MotionRef<const Vector6ArgType> MotionRefType;

  }; // traits MotionRef<const Vector6ArgType>

  template<typename Vector6ArgType>
  class MotionRef<const Vector6ArgType> : public MotionDense<MotionRef<const Vector6ArgType>>
  {
  public:
    typedef MotionDense<MotionRef> Base;
    typedef typename traits<MotionRef>::DataRefType DataRefType;
    MOTION_TYPEDEF_TPL(MotionRef);

    using Base::operator=;
    using Base::angular;
    using Base::linear;

    using Base::__minus__;
    using Base::__mult__;
    using Base::__opposite__;
    using Base::__plus__;

    MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
    : m_ref(v_like)
    {
      EIGEN_STATIC_ASSERT(
        Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
      assert(v_like.size() == 6);
    }

    /// \brief Copy constructor from another MotionRef.
    MotionRef(const MotionRef & other)
    : m_ref(other.m_ref)
    {
    }

    ToVectorConstReturnType toVector_impl() const
    {
      return m_ref;
    }

    // Getters
    ConstAngularType angular_impl() const
    {
      return ConstAngularType(m_ref.derived(), ANGULAR);
    }
    ConstLinearType linear_impl() const
    {
      return ConstLinearType(m_ref.derived(), LINEAR);
    }

    // Specific operators for MotionTpl and MotionRef
    template<typename S1, int O1>
    MotionPlain __plus__(const MotionTpl<S1, O1> & v) const
    {
      return MotionPlain(m_ref + v.toVector());
    }

    template<typename Vector6Like>
    MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
    {
      return MotionPlain(m_ref + v.toVector());
    }

    template<typename S1, int O1>
    MotionPlain __minus__(const MotionTpl<S1, O1> & v) const
    {
      return MotionPlain(m_ref - v.toVector());
    }

    template<typename Vector6Like>
    MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
    {
      return MotionPlain(m_ref - v.toVector());
    }

    template<typename OtherScalar>
    MotionPlain __mult__(const OtherScalar & alpha) const
    {
      return MotionPlain(alpha * m_ref);
    }

    const MotionRef & ref() const
    {
      return *this;
    }

    inline PlainReturnType plain() const
    {
      return PlainReturnType(m_ref);
    }

  protected:
    DataRefType m_ref;

  }; // class MotionRef<const Vector6Like>

} // namespace pinocchio
