#include "drake/systems/controllers/setpoint.h"

#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"

namespace drake {
namespace systems {
namespace controllers {

// Test rotational difference between the desired and measured orientation.
// They are both generated by rotating around the same vector, and they differ
// in the angle of rotation.
GTEST_TEST(testQPInverseDynamicsController, testPoseSetpoint) {
  // Desired values are specified with suffix "_d"
  const double ang_d = 0.3;
  const Vector3<double> vec_d = Vector3<double>(-0.3, 0.6, 0.9).normalized();

  // Desired orientation
  const math::RigidTransform<double> pose_d(AngleAxis<double>(ang_d, vec_d),
                                            Vector3<double>::Zero());

  // Set Kp to 1, and everything else to zeros, so the computed acceleration
  // is the rotation difference.
  CartesianSetpoint<double> setpoint(pose_d.GetAsIsometry3(),
                                     Vector6<double>::Zero(),
                                     Vector6<double>::Zero(),
                                     Vector6<double>::Constant(1),
                                     Vector6<double>::Zero());
  math::RigidTransform<double> pose = pose_d;
  Vector6<double> acc, expected;
  expected.setZero();

  for (double ang = ang_d; ang < ang_d + 2 * M_PI + 0.1; ang += 0.1) {
    pose.set_rotation(AngleAxis<double>(ang, vec_d));
    acc = setpoint.ComputeTargetAcceleration(
        pose.GetAsIsometry3(), Vector6<double>::Zero());

    double err = ang_d - ang;
    if (err > M_PI)
      err -= 2 * M_PI;
    else if (err < -M_PI)
      err += 2 * M_PI;

    expected.head<3>() = err * vec_d;
    EXPECT_TRUE(
        CompareMatrices(acc, expected, 1e-8, MatrixCompareType::absolute));
  }
}

}  // namespace controllers
}  // namespace systems
}  // namespace drake
