//
// Copyright (c) 2019-2020 INRIA
//

#include <iostream>

#include <pinocchio/math/rotation.hpp>
#include <pinocchio/math/sincos.hpp>
#include <Eigen/Geometry>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_toRotationMatrix)
{
  using namespace pinocchio;
#ifndef NDEBUG
  const int max_tests = 1e5;
#else
  const int max_tests = 1e2;
#endif
  for (int k = 0; k < max_tests; ++k)
  {
    const double theta = std::rand();
    double cos_value, sin_value;

    pinocchio::SINCOS(theta, &sin_value, &cos_value);
    const Eigen::Vector3d axis(Eigen::Vector3d::Random().normalized());

    Eigen::Matrix3d rot;
    toRotationMatrix(axis, cos_value, sin_value, rot);
    Eigen::Matrix3d rot2;
    toRotationMatrix(axis, theta, rot2);

    const Eigen::Matrix3d rot_ref = Eigen::AngleAxisd(theta, axis).toRotationMatrix();

    BOOST_CHECK(rot.isApprox(rot_ref));
    BOOST_CHECK(rot2.isApprox(rot_ref));
  }
}

BOOST_AUTO_TEST_CASE(test_orthogonal_projection)
{
  using namespace pinocchio;
#ifndef NDEBUG
  const int max_tests = 1e5;
#else
  const int max_tests = 1e2;
#endif

  typedef Eigen::Vector4d Vector4;
  typedef Eigen::Quaterniond Quaternion;
  typedef Eigen::Matrix3d Matrix3;

  for (int k = 0; k < max_tests; ++k)
  {
    const Vector4 vec4 = Vector4::Random();
    const Quaternion quat(vec4.normalized());

    const Matrix3 rot = quat.toRotationMatrix();

    Matrix3 rot_proj = orthogonalProjection(rot);
    BOOST_CHECK(rot.isApprox(rot_proj));
  }

  for (int k = 0; k < max_tests; ++k)
  {
    const Matrix3 mat = Matrix3::Random();

    Matrix3 rot_proj = orthogonalProjection(mat);
    BOOST_CHECK((rot_proj.transpose() * rot_proj).isIdentity());
    BOOST_CHECK(fabs(rot_proj.determinant() - 1.) <= Eigen::NumTraits<double>::dummy_precision());
  }
}

BOOST_AUTO_TEST_SUITE_END()
