#include "minunit.h"
#include "tests.h"
#include "geometry.h"

mu_test test_contains_point();
mu_test test_subdivide();
mu_test test_insert();
mu_test test_insert_two();
mu_test test_get_closest();

void quadtree_tests()
{
   mu_run_test("check if contains_point() works",
	       test_contains_point);
   mu_run_test("subdivide quadtree node",
	       test_subdivide);
   mu_run_test("insert second point",
	       test_insert);
   mu_run_test("insert two more points",
	       test_insert_two);
   mu_run_test("look up nearest point",
	       test_get_closest);
}


/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

mu_test test_contains_point()
{
   struct quad_region_t region;
   region.center = (struct point_t){ 0.5, 0.5 };
   region.half_dim = 0.5;

   mu_assert(quad_contains_point(region,
				 (struct point_t){ 0.5, 0.5 }));
   mu_assert(quad_contains_point(region,
				 (struct point_t){ 0.25, 0.5 }));
   mu_assert(quad_contains_point(region,
				 (struct point_t){ 0.5, 0.25 }));
   mu_assert(quad_contains_point(region,
				 (struct point_t){ 0, 0 }));


   mu_assert(!quad_contains_point(region,
				  (struct point_t){ 1.25, 0.5 }));
   mu_assert(!quad_contains_point(region,
				  (struct point_t){ 0.25, 1.5 }));
   mu_assert(!quad_contains_point(region,
				  (struct point_t){ 1, 1 }));

   return 0;
}


mu_test test_subdivide()
{
   struct quadtree_node_t node;
   struct point_t pts[1] = { { 0.75, 0.75 } };

   node.region = (struct quad_region_t){ { 0.5, 0.5 }, 0.5 };
   node.id = 0;
   node.nw = NULL;
   node.ne = NULL;
   node.sw = NULL;
   node.se = NULL;

   quadtree_subdivide(&node, pts);

   mu_assert_equal(node.id, -1);

   mu_assert_equal(node.nw->region.center.x, 0.25);
   mu_assert_equal(node.nw->region.center.y, 0.25);
   mu_assert_equal(node.nw->region.half_dim, 0.25);
   mu_assert_equal(node.nw->nw, NULL);
   mu_assert_equal(node.nw->id, -1);

   mu_assert_equal(node.ne->region.center.x, 0.75);
   mu_assert_equal(node.ne->region.center.y, 0.25);
   mu_assert_equal(node.ne->region.half_dim, 0.25);
   mu_assert_equal(node.ne->nw, NULL);
   mu_assert_equal(node.ne->id, -1);

   mu_assert_equal(node.sw->region.center.x, 0.25);
   mu_assert_equal(node.sw->region.center.y, 0.75);
   mu_assert_equal(node.sw->region.half_dim, 0.25);
   mu_assert_equal(node.sw->nw, NULL);
   mu_assert_equal(node.sw->id, -1);

   mu_assert_equal(node.se->region.center.x, 0.75);
   mu_assert_equal(node.se->region.center.y, 0.75);
   mu_assert_equal(node.se->region.half_dim, 0.25);
   mu_assert_equal(node.se->nw, NULL);
   mu_assert_equal(node.se->id, 0);

   return 0;
}


mu_test test_insert()
{
   struct quadtree_node_t node;
   struct point_t pts[2] = { {0.75, 0.75}, {0.25, 0.25} };

   node.region = (struct quad_region_t){ { 0.5, 0.5 }, 0.5 };
   node.id = 0;
   node.nw = NULL;
   node.ne = NULL;
   node.sw = NULL;
   node.se = NULL;

   quadtree_insert(&node, pts, 1);

   mu_assert_equal(node.id, -1);

   mu_assert_equal(node.nw->region.center.x, 0.25);
   mu_assert_equal(node.nw->region.center.y, 0.25);
   mu_assert_equal(node.nw->region.half_dim, 0.25);
   mu_assert_equal(node.nw->nw, NULL);
   mu_assert_equal(node.nw->id, 1);

   mu_assert_equal(node.ne->region.center.x, 0.75);
   mu_assert_equal(node.ne->region.center.y, 0.25);
   mu_assert_equal(node.ne->region.half_dim, 0.25);
   mu_assert_equal(node.ne->nw, NULL);
   mu_assert_equal(node.ne->id, -1);

   mu_assert_equal(node.sw->region.center.x, 0.25);
   mu_assert_equal(node.sw->region.center.y, 0.75);
   mu_assert_equal(node.sw->region.half_dim, 0.25);
   mu_assert_equal(node.sw->nw, NULL);
   mu_assert_equal(node.sw->id, -1);

   mu_assert_equal(node.se->region.center.x, 0.75);
   mu_assert_equal(node.se->region.center.y, 0.75);
   mu_assert_equal(node.se->region.half_dim, 0.25);
   mu_assert_equal(node.se->nw, NULL);
   mu_assert_equal(node.se->id, 0);

   return 0;
}


mu_test test_insert_two()
{
   struct quadtree_node_t node;
   struct point_t pts[3] =
      { {0.75, 0.75}, {0.3, 0.3}, {0.2, 0.2} };

   node.region = (struct quad_region_t){ { 0.5, 0.5 }, 0.5 };
   node.id = 0;
   node.nw = NULL;
   node.ne = NULL;
   node.sw = NULL;
   node.se = NULL;

   quadtree_insert(&node, pts, 1);
   quadtree_insert(&node, pts, 2);

   mu_assert_equal(node.id, -1);

   mu_assert_equal(node.nw->region.center.x, 0.25);
   mu_assert_equal(node.nw->region.center.y, 0.25);
   mu_assert_equal(node.nw->region.half_dim, 0.25);
   mu_assert_equal(node.nw->id, -1);

   /*printf("%d, %d, %d, %d\n",
	  node.nw->nw->id,
	  node.nw->ne->id,
	  node.nw->sw->id,
	  node.nw->se->id);*/

   mu_assert_equal(node.nw->nw->id,  2);
   mu_assert_equal(node.nw->nw->nw,  NULL);
   mu_assert_equal(node.nw->ne->id, -1);
   mu_assert_equal(node.nw->ne->nw,  NULL);
   mu_assert_equal(node.nw->sw->id, -1);
   mu_assert_equal(node.nw->sw->nw,  NULL);
   mu_assert_equal(node.nw->se->id,  1);
   mu_assert_equal(node.nw->se->nw,  NULL);

   mu_assert_equal(node.ne->region.center.x, 0.75);
   mu_assert_equal(node.ne->region.center.y, 0.25);
   mu_assert_equal(node.ne->region.half_dim, 0.25);
   mu_assert_equal(node.ne->nw, NULL);
   mu_assert_equal(node.ne->id, -1);

   mu_assert_equal(node.sw->region.center.x, 0.25);
   mu_assert_equal(node.sw->region.center.y, 0.75);
   mu_assert_equal(node.sw->region.half_dim, 0.25);
   mu_assert_equal(node.sw->nw, NULL);
   mu_assert_equal(node.sw->id, -1);

   mu_assert_equal(node.se->region.center.x, 0.75);
   mu_assert_equal(node.se->region.center.y, 0.75);
   mu_assert_equal(node.se->region.half_dim, 0.25);
   mu_assert_equal(node.se->nw, NULL);
   mu_assert_equal(node.se->id, 0);

   return 0;
}


mu_test test_get_closest()
{
   struct quadtree_node_t node;
   struct point_t pts[3] =
      { {0.75, 0.75}, {0.3, 0.3}, {0.2, 0.2} };

   node.region = (struct quad_region_t){ { 0.5, 0.5 }, 0.5 };
   node.id = 0;
   node.nw = NULL;
   node.ne = NULL;
   node.sw = NULL;
   node.se = NULL;

   quadtree_insert(&node, pts, 1);
   quadtree_insert(&node, pts, 2);

   mu_assert_equal(node.nw->nw->id,  2);
   mu_assert_equal(node.nw->nw->nw,  NULL);
   mu_assert_equal(node.nw->ne->id, -1);
   mu_assert_equal(node.nw->ne->nw,  NULL);
   mu_assert_equal(node.nw->sw->id, -1);
   mu_assert_equal(node.nw->sw->nw,  NULL);
   mu_assert_equal(node.nw->se->id,  1);
   mu_assert_equal(node.nw->se->nw,  NULL);

   int closest_75 =
      quadtree_get_closest(&node, pts,
			   (struct point_t){0.75, 0.75});
   mu_assert_equal(closest_75, 0);

   int closest_33 =
      quadtree_get_closest(&node, pts,
			   (struct point_t){0.3, 0.3});
   mu_assert_equal(closest_33, 1);

   int closest_22 =
      quadtree_get_closest(&node, pts,
			   (struct point_t){0.2, 0.2});
   mu_assert_equal(closest_22, 2);

   int closest_252 =
      quadtree_get_closest(&node, pts,
			   (struct point_t){0.251, 0.21});
   mu_assert_equal(closest_252, 2);

   int closest_nil =
      quadtree_get_closest(&node, pts,
			   (struct point_t){100, 100});
   mu_assert_equal(closest_nil, -1);

   return 0;
}