#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>

#include "SwingTwist.h"

#define FLOAT_INFINITY std::numeric_limits<float>::infinity()

TEST(SwingTwistSingle, Quaternion) {
  Quaternion q;
  SwingTwistSingle s;
  Quaternion rq;

  q = Quaternion::identity;
  s = SwingTwistSingle::FromQuaternion(q);
  rq = s.ToQuaternion();
  EXPECT_EQ(q, rq) << " 0 0 0 1 <-> SwingTwist";

  q = Quaternion::Euler(90, 0, 0);
  s = SwingTwistSingle::FromQuaternion(q);
  rq = s.ToQuaternion();
  EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 90 0 0 <-> SwingTwist";

  q = Quaternion::Euler(0, 90, 0);
  s = SwingTwistSingle::FromQuaternion(q);
  rq = s.ToQuaternion();
  EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";

  q = Quaternion::Euler(0, 0, 90);
  s = SwingTwistSingle::FromQuaternion(q);
  rq = s.ToQuaternion();
  EXPECT_EQ(q, rq) << " Euler 0 0 90 <-> SwingTwist";

  q = Quaternion::Euler(0, 180, 0);  // ==> spherical S(180 0)T0
  s = SwingTwistSingle::FromQuaternion(q);
  rq = s.ToQuaternion();
  EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";

  q = Quaternion::Euler(0, 135, 0);  // ==> spherical S(180 45)T0
  s = SwingTwistSingle::FromQuaternion(q);
  rq = s.ToQuaternion();
  EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
}

TEST(SwingTwistSingle, AngleAxis) {
  SwingTwistSingle s;
  SwingTwistSingle r;

  s = SwingTwistSingle::AngleAxis(0, DirectionSingle::up);
  EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 up";

  r = SwingTwistSingle::AngleAxis(90, DirectionSingle::up);
  s = SwingTwistSingle::Degrees(90, 0, 0);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "90 up";

  r = SwingTwistSingle::AngleAxis(180, DirectionSingle::up);
  s = SwingTwistSingle::Degrees(180, 0, 0);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "180 up";

  r = SwingTwistSingle::AngleAxis(270, DirectionSingle::up);
  s = SwingTwistSingle::Degrees(-90, 0, 0);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "270 up";

  r = SwingTwistSingle::AngleAxis(90, DirectionSingle::right);
  s = SwingTwistSingle::Degrees(0, 90, 0);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "90 right";

  r = SwingTwistSingle::AngleAxis(180, DirectionSingle::right);
  s = SwingTwistSingle::Degrees(0, 180, 0);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "180 right";
  r = SwingTwistSingle::AngleAxis(270, DirectionSingle::right);
  s = SwingTwistSingle::Degrees(0, -90, 0);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "270 right";

  r = SwingTwistSingle::AngleAxis(90, DirectionSingle::forward);
  s = SwingTwistSingle::Degrees(0, 0, 90);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "90 up";

  r = SwingTwistSingle::AngleAxis(180, DirectionSingle::forward);
  s = SwingTwistSingle::Degrees(0, 0, 180);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "180 up";

  r = SwingTwistSingle::AngleAxis(270, DirectionSingle::forward);
  s = SwingTwistSingle::Degrees(0, 0, -90);
  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
      << "270 up";

  auto r16 = SwingTwist16::AngleAxis(13, Direction16::down);
  auto s16 = SwingTwist16::Degrees(-13, 0, 0);
  EXPECT_LT(SwingTwist16::Angle(r16, s16), Angle16::Degrees(10e-2f))
      << "270 up";
}

TEST(SwingTwistSingle, Normalize) {
  SwingTwistSingle s;

  s = SwingTwistSingle::Degrees(0, 0, 0);
  EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 0 0 Normalized";

  s = SwingTwistSingle::Degrees(0, 180, 0);
  EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 180)) << "0 180 0 Normalized";

  s = SwingTwistSingle::Degrees(0, 180, 180);
  EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 0)) << "0 180 180 Normalized";

  s = SwingTwistSingle::Degrees(270, 90, 0);
  EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 90, 0)) << "270 90 0 Normalized";

  s = SwingTwistSingle::Degrees(270, 270, 0);
  EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, -90, 0))
      << "270 270 0 Normalized";

  s = SwingTwistSingle::Degrees(270, 225, 0);
  EXPECT_EQ(s, SwingTwistSingle::Degrees(90, -45, -180))
      << "270 225 0 Normalized";

  s = SwingTwistSingle::Degrees(270, 0, 225);
  EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 0, -135))
      << "270 0 225 Normalized";
}

#endif