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

#include <limits>
#include <math.h>

#include "Angle.h"

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

TEST(AngleSingle, Construct) {
  float angle = 0.0F;
  AngleSingle a = AngleSingle::Degrees(angle);
  EXPECT_FLOAT_EQ(a.InDegrees(), angle);

  angle = -180.0F;
  a = AngleSingle::Degrees(angle);
  EXPECT_FLOAT_EQ(a.InDegrees(), angle);

  angle = 270.0F;
  a = AngleSingle::Degrees(angle);
  EXPECT_FLOAT_EQ(a.InDegrees(), -90);
}

TEST(AngleSingle, Negate) {
  float angle = 0;
  AngleSingle a = AngleSingle::Degrees(angle);
  a = -a;
  EXPECT_FLOAT_EQ(a.InDegrees(), angle);

  angle = 90.0F;
  a = AngleSingle::Degrees(angle);
  a = -a;
  EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}

TEST(AngleSingle, Add) {
  AngleSingle a = AngleSingle::Degrees(-45);
  AngleSingle b = AngleSingle::Degrees(45.0F);
  AngleSingle r = a + b;
  EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}

TEST(AngleSingle, Subtract) {
  AngleSingle a = AngleSingle::Degrees(0);
  AngleSingle b = AngleSingle::Degrees(45.0F);
  AngleSingle r = a - b;
  EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}

TEST(AngleSingle, Compare) {
  AngleSingle a = AngleSingle::Degrees(45);
  bool r = false;

  r = a > AngleSingle::Degrees(0);
  EXPECT_TRUE(r) << "45 > 0";

  r = a > AngleSingle::Degrees(90);
  EXPECT_FALSE(r) << "45 > 90";

  r = a > AngleSingle::Degrees(-90);
  EXPECT_TRUE(r) << "45 > -90";
}

TEST(AngleSingle, Normalize) {
  Angle r = AngleSingle();

  r = AngleSingle::Normalize(AngleSingle::Degrees(90.0f));
  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";

  r = AngleSingle::Normalize(AngleSingle::Degrees(-90));
  EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";

  r = AngleSingle::Normalize(AngleSingle::Degrees(270));
  EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";

  r = AngleSingle::Normalize(AngleSingle::Degrees(270 + 360));
  EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";

  r = AngleSingle::Normalize(AngleSingle::Degrees(-270));
  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";

  r = AngleSingle::Normalize(AngleSingle::Degrees(-270 - 360));
  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";

  r = AngleSingle::Normalize(AngleSingle::Degrees(0));
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";

  if (std::numeric_limits<float>::is_iec559) {
    r = AngleSingle::Normalize(AngleSingle::Degrees(FLOAT_INFINITY));
    EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";

    r = AngleSingle::Normalize(AngleSingle::Degrees(-FLOAT_INFINITY));
    EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
  }
}

TEST(AngleSingle, Clamp) {
  Angle r = AngleSingle();

  r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
                         AngleSingle::Degrees(2));
  EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 2";

  r = AngleSingle::Clamp(AngleSingle::Degrees(-1), AngleSingle::Degrees(0),
                         AngleSingle::Degrees(2));
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -1 0 2";

  r = AngleSingle::Clamp(AngleSingle::Degrees(3), AngleSingle::Degrees(0),
                         AngleSingle::Degrees(2));
  EXPECT_FLOAT_EQ(r.InDegrees(), 2) << "Clamp 3 0 2";

  r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
                         AngleSingle::Degrees(0));
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 1 0 0";

  r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
                         AngleSingle::Degrees(0));
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";

  r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(1),
                         AngleSingle::Degrees(-1));
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 1 -1";

  if (std::numeric_limits<float>::is_iec559) {
    r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
                           AngleSingle::Degrees(FLOAT_INFINITY));
    EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 INFINITY";

    r = AngleSingle::Clamp(AngleSingle::Degrees(1),
                           AngleSingle::Degrees(-FLOAT_INFINITY),
                           AngleSingle::Degrees(1));
    EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 -INFINITY 1";
  }
}

// TEST(AngleSingle, Difference) {
//   Angle r = 0;

//   r = AngleSingle::Difference(0, 90);
//   EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";

//   r = AngleSingle::Difference(0, -90);
//   EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";

//   r = AngleSingle::Difference(0, 270);
//   EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";

//   r = AngleSingle::Difference(0, -270);
//   EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";

//   r = AngleSingle::Difference(90, 0);
//   EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";

//   r = AngleSingle::Difference(-90, 0);
//   EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";

//   r = AngleSingle::Difference(0, 0);
//   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";

//   r = AngleSingle::Difference(90, 90);
//   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";

//   if (std::numeric_limits<float>::is_iec559) {
//     r = AngleSingle::Difference(0, INFINITY);
//     EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";

//     r = AngleSingle::Difference(0, -INFINITY);
//     EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";

//     r = AngleSingle::Difference(-INFINITY, INFINITY);
//     EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
//     INFINITY";
//   }
// }

TEST(AngleSingle, MoveTowards) {
  AngleSingle r = AngleSingle();

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(90), 30);
  EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 90 30";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(90), 90);
  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(90), 180);
  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 180";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(90), 270);
  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(90), -30);
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(-90), -30);
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(-90), -90);
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(-90), -180);
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(-90), -270);
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                               AngleSingle::Degrees(90), 0);
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
                               0);
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";

  r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
                               30);
  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";

  if (std::numeric_limits<float>::is_iec559) {
    r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                                 AngleSingle::Degrees(90), FLOAT_INFINITY);
    EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";

    r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                                 AngleSingle::Degrees(FLOAT_INFINITY), 30);
    EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";

    r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                                 AngleSingle::Degrees(-90), -FLOAT_INFINITY);
    EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -FLOAT_INFINITY";

    r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
                                 AngleSingle::Degrees(-FLOAT_INFINITY), -30);
    EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -FLOAT_INFINITY -30";
  }
}

#endif