#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "util.h"
#include "geometry.h"

bool quad_contains_point(struct quad_region_t region,
			 struct point_t pt)
{
   if (region.center.x - region.half_dim > pt.x ||
       region.center.x + region.half_dim <= pt.x)
      return false;

   if (region.center.y - region.half_dim > pt.y ||
       region.center.y + region.half_dim <= pt.y)
      return false;
   return true;
}


struct quadtree_node_t quadtree_new_node(struct point_t center,
					 double half_dim)
{
   struct quadtree_node_t node;

   node.region = (struct quad_region_t) { center, half_dim };
   node.id = -1;
   node.nw = NULL;
   node.ne = NULL;
   node.sw = NULL;
   node.se = NULL;

   return node;
}


void quadtree_subdivide(struct quadtree_node_t *node,
			struct point_t *points)
{
   node->nw = malloc(sizeof(struct quadtree_node_t));
   node->ne = malloc(sizeof(struct quadtree_node_t));
   node->sw = malloc(sizeof(struct quadtree_node_t));
   node->se = malloc(sizeof(struct quadtree_node_t));

   struct point_t center = node->region.center;
   double quarter_dim = node->region.half_dim / 2;

   struct point_t c_nw =
      { center.x - quarter_dim, center.y - quarter_dim };
   *(node->nw) = quadtree_new_node(c_nw, quarter_dim);

   struct point_t c_ne =
      { center.x + quarter_dim, center.y - quarter_dim };
   *(node->ne) = quadtree_new_node(c_ne, quarter_dim);

   struct point_t c_sw =
      { center.x - quarter_dim, center.y + quarter_dim };
   *(node->sw) = quadtree_new_node(c_sw, quarter_dim);

   struct point_t c_se =
      { center.x + quarter_dim, center.y + quarter_dim };
   *(node->se) = quadtree_new_node(c_se, quarter_dim);

   if (node->id != -1) {
   /* move point to a child */
      if (quad_contains_point(node->nw->region,
			      points[node->id]))
	 node->nw->id = node->id;
      else if (quad_contains_point(node->ne->region,
				   points[node->id]))
	 node->ne->id = node->id;
      else if (quad_contains_point(node->sw->region,
				   points[node->id]))
	 node->sw->id = node->id;
      else
	 node->se->id = node->id;
      node->id = -1;
   }
}

bool quadtree_insert(struct quadtree_node_t *node,
		     struct point_t *points, int id)
{
   if (!quad_contains_point(node->region, points[id]))
      return false;

   if (node->id == -1 && node->nw == NULL) {
      node->id = id;
      return true;
   }

   if (node->nw == NULL)
      quadtree_subdivide(node, points);

   if (quadtree_insert(node->nw, points, id)) return true;
   if (quadtree_insert(node->ne, points, id)) return true;
   if (quadtree_insert(node->sw, points, id)) return true;
   if (quadtree_insert(node->se, points, id)) return true;

   return false;
}


static int get_points_in_region(struct quadtree_node_t *node,
				int* buffer)
{
   if (node->id != -1) {
      *buffer = node->id;
      return 1;
   }

   if (node->nw != NULL) {
      int n_points = 0;
      n_points += get_points_in_region(node->nw, buffer + n_points);
      n_points += get_points_in_region(node->ne, buffer + n_points);
      n_points += get_points_in_region(node->sw, buffer + n_points);
      n_points += get_points_in_region(node->se, buffer + n_points);
      return n_points;
   }

   return 0;
}


int quadtree_get_closest(struct quadtree_node_t *node,
			 struct point_t *points,
			 struct point_t pt)
{
   if (!quad_contains_point(node->region, pt))
      return -1;

   if (node->id != -1)
      return node->id;

   int closest;
   if (node->nw != NULL) {
      if ((closest = quadtree_get_closest(node->nw, points, pt)) != -1)
	 return closest;
      if ((closest = quadtree_get_closest(node->ne, points, pt)) != -1)
	 return closest;
      if ((closest = quadtree_get_closest(node->sw, points, pt)) != -1)
	 return closest;
      if ((closest = quadtree_get_closest(node->se, points, pt)) != -1)
	 return closest;

      /* closest point is in a subregion, but not the *same*
       * subregion as the point itself, so we need to check against
       * all points contained within the region
       */

      int buffer[1000];
      int n_points = get_points_in_region(node, buffer);
      double dist = 1e9;
      for (int i=0; i<n_points; i++) {
	 int id = buffer[i];
	 double d = distance(points[id], pt);
	 if (d < dist) {
	    closest = id;
	    dist = d;
	 }
      }

      return closest;
   }

   return -1;
}


void quadtree_free(struct quadtree_node_t *node)
{
   if (node->nw != NULL) {
      quadtree_free(node->nw);
      quadtree_free(node->ne);
      quadtree_free(node->sw);
      quadtree_free(node->se);
   }

   free(node);
}