//
// Copyright (c) 2017-2020 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<int _axis>
  struct CartesianAxis
  {
    static constexpr int axis = _axis;
    static constexpr int dim = 3;

    typedef Eigen::Matrix<double, 3, 1> Vector3;

    template<typename V3_in, typename V3_out>
    inline static void
    cross(const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout);

    template<typename V3>
    static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) cross(const Eigen::MatrixBase<V3> & vin)
    {
      typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
      cross(vin, res);
      return res;
    }

    template<typename Scalar, typename V3_in, typename V3_out>
    inline static void alphaCross(
      const Scalar & s,
      const Eigen::MatrixBase<V3_in> & vin,
      const Eigen::MatrixBase<V3_out> & vout);

    template<typename Scalar, typename V3>
    static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3)
      alphaCross(const Scalar & s, const Eigen::MatrixBase<V3> & vin)
    {
      typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
      alphaCross(s, vin, res);
      return res;
    }

    template<typename Scalar>
    Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s) const
    {
      typedef Eigen::Matrix<Scalar, dim, 1> ReturnType;
      ReturnType res;
      for (Eigen::Index i = 0; i < dim; ++i)
        res[i] = i == axis ? s : Scalar(0);

      return res;
    }

    template<typename Scalar>
    friend inline Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s, const CartesianAxis &)
    {
      return CartesianAxis() * s;
    }

    template<typename Vector3Like>
    static void setTo(const Eigen::MatrixBase<Vector3Like> v3)
    {
      Vector3Like & v3_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Like, v3);
      typedef typename Vector3Like::Scalar Scalar;

      for (Eigen::Index i = 0; i < dim; ++i)
        v3_[i] = i == axis ? Scalar(1) : Scalar(0);
    }

    template<typename Scalar>
    static Eigen::Matrix<Scalar, 3, 1> vector()
    {
      typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
      return Vector3::Unit(axis);
    }

    static Vector3 vector()
    {
      return vector<double>();
    }

  }; // struct CartesianAxis

  template<>
  template<typename V3_in, typename V3_out>
  inline void CartesianAxis<0>::cross(
    const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
  {
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_in, 3);
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_out, 3);
    V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
    vout_[0] = 0.;
    vout_[1] = -vin[2];
    vout_[2] = vin[1];
  }

  template<>
  template<typename V3_in, typename V3_out>
  inline void CartesianAxis<1>::cross(
    const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
  {
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_in, 3);
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_out, 3);
    V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
    vout_[0] = vin[2];
    vout_[1] = 0.;
    vout_[2] = -vin[0];
  }

  template<>
  template<typename V3_in, typename V3_out>
  inline void CartesianAxis<2>::cross(
    const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
  {
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_in, 3);
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_out, 3);
    V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
    vout_[0] = -vin[1];
    vout_[1] = vin[0];
    vout_[2] = 0.;
  }

  template<>
  template<typename Scalar, typename V3_in, typename V3_out>
  inline void CartesianAxis<0>::alphaCross(
    const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
  {
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_in, 3);
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_out, 3);
    V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
    vout_[0] = 0.;
    vout_[1] = -s * vin[2];
    vout_[2] = s * vin[1];
  }

  template<>
  template<typename Scalar, typename V3_in, typename V3_out>
  inline void CartesianAxis<1>::alphaCross(
    const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
  {
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_in, 3);
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_out, 3);
    V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
    vout_[0] = s * vin[2];
    vout_[1] = 0.;
    vout_[2] = -s * vin[0];
  }

  template<>
  template<typename Scalar, typename V3_in, typename V3_out>
  inline void CartesianAxis<2>::alphaCross(
    const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
  {
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_in, 3);
    PINOCCHIO_EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(V3_out, 3);
    V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
    vout_[0] = -s * vin[1];
    vout_[1] = s * vin[0];
    vout_[2] = 0.;
  }

  typedef CartesianAxis<0> XAxis;
  typedef XAxis AxisX;

  typedef CartesianAxis<1> YAxis;
  typedef YAxis AxisY;

  typedef CartesianAxis<2> ZAxis;
  typedef ZAxis AxisZ;

} // namespace pinocchio
