//
// Copyright (c) 2020 INRIA
//

#include <iostream>

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

#include "pinocchio/algorithm/utils/force.hpp"

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_force)
{
  using namespace pinocchio;

  const SE3 placement = SE3::Random();
  const Force f_in = Force::Random();

  // test case 1-2-3
  {
    BOOST_CHECK(changeReferenceFrame(placement, f_in, LOCAL, LOCAL) == f_in);
    BOOST_CHECK(changeReferenceFrame(placement, f_in, WORLD, WORLD) == f_in);
    BOOST_CHECK(
      changeReferenceFrame(placement, f_in, LOCAL_WORLD_ALIGNED, LOCAL_WORLD_ALIGNED) == f_in);
  }

  const ReferenceFrame all_choices[3] = {LOCAL, WORLD, LOCAL_WORLD_ALIGNED};
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
      BOOST_CHECK(
        changeReferenceFrame(SE3::Identity(), f_in, all_choices[i], all_choices[j]) == f_in);

  // LOCAL/WORLD and WORLD/LOCAL
  {
    Force f_sol_w = placement.act(f_in);
    BOOST_CHECK(changeReferenceFrame(placement, f_in, LOCAL, WORLD) == f_sol_w);
    BOOST_CHECK(changeReferenceFrame(placement, f_sol_w, WORLD, LOCAL).isApprox(f_in));
  }

  // LOCAL/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/LOCAL
  {
    const SE3 placement_LWA(placement.rotation(), SE3::Vector3::Zero());
    Force f_sol_lwa = placement_LWA.act(f_in);
    BOOST_CHECK(changeReferenceFrame(placement, f_in, LOCAL, LOCAL_WORLD_ALIGNED) == f_sol_lwa);
    BOOST_CHECK(
      changeReferenceFrame(placement, f_sol_lwa, LOCAL_WORLD_ALIGNED, LOCAL).isApprox(f_in));
  }

  // WORLD/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/WORLD
  {
    const SE3 placement_W(SE3::Matrix3::Identity(), placement.translation());
    Force f_sol_w = placement_W.act(f_in);
    BOOST_CHECK(
      changeReferenceFrame(placement, f_in, LOCAL_WORLD_ALIGNED, WORLD).isApprox(f_sol_w));
    BOOST_CHECK(
      changeReferenceFrame(placement, f_sol_w, WORLD, LOCAL_WORLD_ALIGNED).isApprox(f_in));
  }
}

BOOST_AUTO_TEST_SUITE_END()
