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

#include "Spherical.h"
#include "Vector3.h"

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

TEST(Spherical16, FromVector3) {
  Vector3 v = Vector3(0, 0, 1);
  Spherical16 s = Spherical16::FromVector3(v);

  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
  EXPECT_FLOAT_EQ((float)s.direction.horizontal.InDegrees(), 0.0F)
      << "s.hor 0 0 1";
  EXPECT_FLOAT_EQ((float)s.direction.vertical.InDegrees(), 0.0F)
      << "s.vert 0 0 1";

  v = Vector3(0, 1, 0);
  s = Spherical16::FromVector3(v);

  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
  EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0";
  EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0";

  v = Vector3(1, 0, 0);
  s = Spherical16::FromVector3(v);

  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
  EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0";
  EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
}

TEST(Spherical16, Vector3) {
  Vector3 v = Vector3(1, 2, 3);
  Spherical16 rd = Spherical16::FromVector3(v);
  Vector3 rv = rd.ToVector3();
  EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";

  v = Vector3(1, 2, -3);
  rd = Spherical16::FromVector3(v);
  rv = rd.ToVector3();
  EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
}

// TEST(Spherical16, FromPolar) {
//   Polar p = Polar(1, 0);
//   Spherical16 s = Spherical16::FromPolar(p);

//   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
//   EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 0.0F) << "s.hor Polar(1 0)";
//   EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 0)";

//   p = Polar(1, 45);
//   s = Spherical16::FromPolar(p);

//   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
//   EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 45.0F) << "s.hor Polar(1 45)";
//   EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 45)";

//   p = Polar(1, -45);
//   s = Spherical16::FromPolar(p);

//   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
//   EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), -45.0F) << "s.hor Polar(1 -45)";
//   EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 -45)";

//   p = Polar(0, 0);
//   s = Spherical16::FromPolar(p);

//   EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
//   EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 0.0F) << "s.hor Polar(0 0)";
//   EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(0 0)";

//   p = Polar(-1, 0);
//   s = Spherical16::FromPolar(p);

//   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
//   EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), -180.0F) << "s.hor Polar(-1 0)";
//   EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(-1 0)";
// }

TEST(Spherical16, Incident1) {
  Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
  Spherical16 s = Spherical16::FromVector3(v);

  Spherical16 sr =
      Spherical16(2.49F, Angle16::Degrees(98.18f), Angle16::Degrees(24.4F));
  EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
  EXPECT_NEAR(s.direction.horizontal.InDegrees(),
              sr.direction.horizontal.InDegrees(), 1.0e-02);
  EXPECT_NEAR(s.direction.vertical.InDegrees(),
              sr.direction.vertical.InDegrees(), 1.0e-02);

  Vector3 r =
      Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
          .ToVector3();
  EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
  EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0";
  EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
}

TEST(Spherical16, Incident2) {
  Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
  Spherical16 s = Spherical16::FromVector3(v);

  Spherical16 sr = Spherical16(1.4142135623F, Angle16::Degrees(45.0f),
                               Angle16::Degrees(0.0F));
  EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
  EXPECT_NEAR(s.direction.horizontal.InDegrees(),
              sr.direction.horizontal.InDegrees(), 1.0e-05);
  EXPECT_NEAR(s.direction.vertical.InDegrees(),
              sr.direction.vertical.InDegrees(), 1.0e-05);

  Vector3 r =
      Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
          .ToVector3();
  EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
  EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
  EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);

  v = Vector3(0.0f, 1.0f, 1.0f);
  s = Spherical16::FromVector3(v);

  sr = Spherical16(1.4142135623F, Angle16::Degrees(0), Angle16::Degrees(45));
  EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
  EXPECT_NEAR(s.direction.horizontal.InDegrees(),
              sr.direction.horizontal.InDegrees(), 1.0e-05);
  EXPECT_NEAR(s.direction.vertical.InDegrees(),
              sr.direction.vertical.InDegrees(), 1.0e-05);

  r = Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
          .ToVector3();
  EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
  EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
  EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);

  v = Vector3(1.0f, 1.0f, 1.0f);
  s = Spherical16::FromVector3(v);
  r = Spherical16(s.distance, s.direction.horizontal, s.direction.vertical)
          .ToVector3();

  EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
  EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02);
  EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02);

  EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04);
  EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04);
  EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-04);

  // s = Spherical16(10, 45, 45);
  // r = s.ToVector3();
  // EXPECT_NEAR(r.x, 5, 1.0e-06);
  // EXPECT_NEAR(r.y, 7.07, 1.0e-06);
  // EXPECT_NEAR(r.z, 5, 1.0e-06);
}

TEST(Spherical16, Addition) {
  Spherical16 v1 = Spherical16(1, Angle16::Degrees(45), Angle16::Degrees(0));
  Spherical16 v2 = Spherical16::zero;
  Spherical16 r = Spherical16::zero;

  r = v1 + v2;
  EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";

  r = v1;
  r += v2;
  EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";

  v2 = Spherical16(1, Angle16::Degrees(-45), Angle16::Degrees(0));
  r = v1 + v2;
  EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
  EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
  EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";

  v2 = Spherical16(1, Angle16::Degrees(0), Angle16::Degrees(90));
  r = v1 + v2;
  EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
  EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";
  EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)";
}

TEST(Spherical16, AdditionPerformance) {
  const int numIterations = 1000000; // Number of additions to test
  std::vector<Spherical16> sphericalObjects;

  // Populate the vector with random SphericalOf objects
  for (int i = 0; i < numIterations; ++i) {
    float distance = (float)(rand() % 100);
    float horizontal = (float)(rand() % 180);
    float vertical = (float)(rand() % 360);
    Spherical16 s = Spherical16::Deg(distance, horizontal, vertical);
    sphericalObjects.push_back(s);
  }

  // Measure the time to perform multiple additions
  auto start = std::chrono::high_resolution_clock::now();

  Spherical16 result = Spherical16::zero; // Start with a
                                          // zero-initialized object

  for (int i = 0; i < numIterations - 1; ++i) {
    result = result + sphericalObjects[i]; // Add objects
                                           // together
  }

  auto end = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> duration = end - start;
  std::cout << "Time to perform " << numIterations - 1
            << " additions: " << duration.count() << " seconds." << std::endl;

  // Assert that the time taken is less than
  // 1 second (or any other performance
  // requirement)
  ASSERT_LE(duration.count(), 2.0) << "Performance test failed: "
                                      "Additions took longer than 1 "
                                      "second.";
}

#endif