#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <memory>

#include "../src/orthtree.hpp"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

TEST(orthtree, insert1) {
  typedef Orthtree<2, float, bool> O;
  Orthtree<2, float, bool> t(O::Point::Zero(), 16);
  auto p = O::Point(1, -7);
  t.depth(p, 3, [&](auto &tp, [[maybe_unused]] auto c) {
    tp = std::make_unique<bool>(true);
    return true;
  });

  t.all_data([&](auto &tp, auto c) -> bool {
    EXPECT_TRUE(tp && *tp) << c.transpose();
    EXPECT_TRUE(c == p) << c.transpose();
    return true;
  });
}
TEST(orthtree, bbox1) {
  typedef Orthtree<2, float, bool> O;
  EXPECT_EQ(O::BBox(O::Point(0, 0), O::Point(10, 10))
                .relation(O::BBox(O::Point(0, 0), O::Point(10, 11))),
            O::BBox::OVERLAP);

  EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(10, 10))
                .relation(O::BBox(O::Point(0, 0), O::Point(10, 11))),
            O::BBox::OVERLAP);

  EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(1, 1))
                .relation(O::BBox(O::Point(2, 2), O::Point(10, 11))),
            O::BBox::NO_OVERLAP);

  EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(1, 1))
                .relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
            O::BBox::CONTAINED);

  EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(100, 100))
                .relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
            O::BBox::CONTAINS);

  EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
                .relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
            O::BBox::OVERLAP);

  EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
                .relation(O::BBox(O::Point(-10, -2), O::Point(10, 11))),
            O::BBox::OVERLAP);

  EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
                .relation(O::BBox(O::Point(-10, -1), O::Point(10, 11))),
            O::BBox::NO_OVERLAP);
}

TEST(orthtree, insert2) {
  typedef Orthtree<2, float, Eigen::Vector2f> O;
  O t(O::Point::Zero(), 16);
  for (int i = 0; i <= 360; i += 90) {

    Eigen::Rotation2D<float> rot(float(i) / 180.0 * M_PI);
    O::Point p = O::Point(-4, 4) + (rot * O::Point(1.9, 0));
    t.depth(p, 3, [&](auto &tp, [[maybe_unused]] auto c) {
      tp = std::make_unique<O::Point>(p);
      return true;
    });
  }
  std::atomic<int> i = 0;
  t.data_in_bb(O::BBox(O::Point(-2, 0), O::Point(0, 32)),
               [&](auto &tp, auto c) -> bool {
                 EXPECT_TRUE(tp) << c.transpose();
                 i += 1;
                 return true;
               });
  EXPECT_EQ(i, 2);
}

int main(int argc, char **argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}