/*
 *  Copyright (c) 2011-2012, Los Alamos National Security, LLC.
 *  All rights Reserved.
 *
 *  Copyright 2011-2012. Los Alamos National Security, LLC. This software was produced 
 *  under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National 
 *  Laboratory (LANL), which is operated by Los Alamos National Security, LLC 
 *  for the U.S. Department of Energy. The U.S. Government has rights to use, 
 *  reproduce, and distribute this software.  NEITHER THE GOVERNMENT NOR LOS 
 *  ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR 
 *  ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE.  If software is modified
 *  to produce derivative works, such modified software should be clearly marked,
 *  so as not to confuse it with the version available from LANL.
 *
 *  Additionally, redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Los Alamos National Security, LLC, Los Alamos 
 *       National Laboratory, LANL, the U.S. Government, nor the names of its 
 *       contributors may be used to endorse or promote products derived from 
 *       this software without specific prior written permission.
 *  
 *  THIS SOFTWARE IS PROVIDED BY THE LOS ALAMOS NATIONAL SECURITY, LLC AND 
 *  CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT 
 *  NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL
 *  SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *  
 *  CLAMR -- LA-CC-11-094
 *  This research code is being developed as part of the 
 *  2011 X Division Summer Workshop for the express purpose
 *  of a collaborative code for development of ideas in
 *  the implementation of AMR codes for Exascale platforms
 *  
 *  AMR implementation of the Wave code previously developed
 *  as a demonstration code for regular grids on Exascale platforms
 *  as part of the Supercomputing Challenge and Los Alamos 
 *  National Laboratory
 *  
 *  Authors: Bob Robey       XCP-2   brobey@lanl.gov
 *           Neal Davis              davis68@lanl.gov, davis68@illinois.edu
 *           David Nicholaeff        dnic@lanl.gov, mtrxknight@aol.com
 *           Dennis Trujillo         dptrujillo@lanl.gov, dptru10@gmail.com
 *           Other LANL authors
 * 
 */
#include <math.h>
#include "KDTree.h"

#define MALLOC(n,t) ((t*)(malloc(n * sizeof(t))))
#define REALLOC(p,n,t) ((t*)(realloc((void*)p, n * sizeof(t))))
#define FREE(p) { if (p) free(p); }

static void median_sort(TKDTree* t,
                        unsigned int cut_direction, int k, int num, int* idx)
{
   int left, mid, right, a, i, j;
   
   for (left = 0, right = num - 1; (right - left) > 1;) {
      mid = (left + right) / 2;
      SWAP(idx[mid], idx[left + 1], int);
      if(Bounds_CenterAxis(&(t->elements[idx[left + 1]]), cut_direction) >
         Bounds_CenterAxis(&(t->elements[idx[right]]), cut_direction))
         SWAP(idx[left + 1], idx[right], int);
      if(Bounds_CenterAxis(&(t->elements[idx[left]]), cut_direction) >
         Bounds_CenterAxis(&(t->elements[idx[right]]), cut_direction))
         SWAP(idx[left], idx[right], int);
      if(Bounds_CenterAxis(&(t->elements[idx[left + 1]]), cut_direction) >
         Bounds_CenterAxis(&(t->elements[idx[left]]), cut_direction))
         SWAP(idx[left + 1], idx[left], int);
      a = idx[left];
      i = left + 1;
      j = right;
      while (1) {
         for (i++;
              Bounds_CenterAxis(&(t->elements[idx[i]]), cut_direction) <
                Bounds_CenterAxis(&(t->elements[a]), cut_direction);
              i++);
         for (j--;
              Bounds_CenterAxis(&(t->elements[idx[j]]), cut_direction) >
              Bounds_CenterAxis(&(t->elements[a]), cut_direction);
              j--);
         if(j < i)
            break;
         SWAP(idx[i], idx[j], int);
      }
      idx[left] = idx[j];
      idx[j] = a;
      if(j >= k)
         right = j - 1;
      if(j <= k)
         left = i;
   }
   if(((right - left) ==1) &&
      (Bounds_CenterAxis(&(t->elements[idx[right]]), cut_direction) <
       Bounds_CenterAxis(&(t->elements[idx[left]]), cut_direction)))
      SWAP(idx[right], idx[left], int);
}

void KDTree_Initialize(TKDTree* t)
{
   assert(t);
   /* Flush the overall tree extent */
   Bounds_Infinite(&(t->extent));
   /* Allocate the initial memory for tree elements */
   t->elements_num = 0;
   t->elements_allocated = KDTREE_ELEMENT_BLOCKING_SIZE;
   t->elements = MALLOC(t->elements_allocated, TBounds);
   assert(t->elements);
   /* Start without a built tree */
   t->tree_built = false;
   t->tree_size = 0;
   t->tree_safety_boxes = NULL;
   t->tree_link = NULL;
}

void KDTree_Finalize(TKDTree* t)
{
   free(t->elements);
}

void KDTree_Destroy(TKDTree* t)
{
   assert(t);
   /* Flush the overall tree extent */
   Bounds_Infinite(&(t->extent));
   /* Destroy the element list */
   t->elements_num = 0;
   t->elements_allocated = 0;
   FREE(t->elements);
   t->elements = NULL;
   /* Destroy the actual tree */
   t->tree_built = false;
   t->tree_size = 0;
   FREE(t->tree_safety_boxes);
   t->tree_safety_boxes = NULL;
   FREE(t->tree_link);
   t->tree_link = NULL;
}


void KDTree_AddElement(TKDTree* t, TBounds* add)
{
   assert(t && add);
   /* Destroy the current tree if it is built */
   if(t->tree_built) {
      t->tree_built = false;
      t->tree_size = 0;
      FREE(t->tree_safety_boxes);
      t->tree_safety_boxes = NULL;
      FREE(t->tree_link);
      t->tree_link = NULL;
   }
   /* Expand the element array if necessary */
   if(t->elements_num == t->elements_allocated) {
      t->elements_allocated += KDTREE_ELEMENT_BLOCKING_SIZE;
      t->elements = REALLOC(t->elements, t->elements_allocated, TBounds);
      assert(t->elements);
   }
   /* Add the new element to the overall extent and the element list */
   Bounds_AddBounds(&(t->extent), add);
   Bounds_Copy(add, &(t->elements[t->elements_num]));
   t->elements_num++;
}

void KDTree_CreateTree(TKDTree* t)
{
   unsigned int i;
   int next_node, stack_ptr, min, mid, max, parent, cut_direction;
   double width, max_width;
   int* stack;
   int* idx;
   
   assert(t);
   /* If the tree is already built, we don't have to do anything */
   if(t->tree_built)
      return;
   /* If there are no elements in the tree, we don't have to do anything */
   if(t->elements_num > 0) {
      /* Allocate the k-D tree memory */
      t->tree_size = 2 * t->elements_num;
      t->tree_safety_boxes = MALLOC(t->tree_size, TBounds);
      t->tree_link = MALLOC(t->tree_size, int);
      /* Create and initialize temporary arrays */
      next_node = 0;
      stack_ptr = 0;
      stack = MALLOC(3 * t->tree_size, int);
      idx = MALLOC(t->elements_num, int);
      for (i = 0; (int)i <  t->elements_num; i++) {
         idx[i] = i;
      }
      /* Setup the root node of the tree and put it on the stack */
      stack[stack_ptr++] = 0;                   /* Node Number in the Tree */
      stack[stack_ptr++] = 0;                   /* Element Span Minumum */
      stack[stack_ptr++] = t->elements_num - 1; /* Element Span Maximum */
      Bounds_Copy(&(t->extent), &(t->tree_safety_boxes[0]));
      next_node++;
      /* Construct k-D tree by setting up each pair of child nodes */
      while (stack_ptr) {
         /* Pop the top entry off the stack */
         max = stack[--stack_ptr];
         min = stack[--stack_ptr];
         parent = stack[--stack_ptr];
         /* If the current node should be a leaf node, make it one */
         if ((max - min) == 0) {
            Bounds_Copy(&(t->elements[idx[min]]), &(t->tree_safety_boxes[parent]));
            t->tree_link[parent] = - idx[min];
            continue;
         }
         /* Select optimum cutting direction for the parent node's safety box */
         cut_direction = -1;
         max_width = NEGATIVE_INFINITY;
         for (i = 0; i < 2; i++) {
            width = Bounds_WidthAxis(&(t->tree_safety_boxes[parent]), i);
            if(width > max_width) {
               max_width = width;
               cut_direction = i;
            }
         }
         assert(cut_direction >= 0);
         /* Do a median sort of the elements under the parent node. The sort key
            is the center point of the element bounding boxes along the selected
            cutting direction. */
         mid = (min + max) /2;
         median_sort(t, (unsigned int)cut_direction, mid - min, max - min + 1, &(idx[min]));
         /* Give the parent a reference to its two children */
         t->tree_link[parent] = next_node;
         /* Add the "left" child to the tree and the stack */
         stack[stack_ptr++] = next_node;  /* Node Number in the Tree */
         stack[stack_ptr++] = min;        /* Element Span Minimum */
         stack[stack_ptr++] = mid;        /* Element Span Maximum */
         Bounds_Infinite(&(t->tree_safety_boxes[next_node]));
         for (i = min; (int)i <= mid; i++) {
            Bounds_AddBounds(&(t->tree_safety_boxes[next_node]),
                             &(t->elements[idx[i]]));
         }
         next_node++;
         /* Add the "right" child to the tree and the stack */
         stack[stack_ptr++] = next_node;  /* Node Number in the Tree */
         stack[stack_ptr++] = mid + 1;    /* Element Span Minimum */
         stack[stack_ptr++] = max;        /* Element Span Maximum */
         Bounds_Infinite(&(t->tree_safety_boxes[next_node]));
         for (i = min + 1; (int)i <= max; i++) {
            Bounds_AddBounds(&(t->tree_safety_boxes[next_node]),
                             &(t->elements[idx[i]]));
         }
         next_node++;
      }
      /* Destroy the temporary arrays */
      FREE(stack);
      FREE(idx);
   }
   /* Mark the tree "built" */
   t->tree_built = true;
}

void KDTree_QueryBoxIntersect(TKDTree* t,
                              int* result_num, int* result_indicies,
                              TBounds* box)
{
   int stack_ptr, node;
   TBounds sb;
   int* stack;
   
   assert(t && result_num && result_indicies && box);
   /* Build the k-D tree if necessary */
   if(!t->tree_built){
      //printf("BUILDING TREE... \n");
      //fflush(stdout);
      KDTree_CreateTree(t);
   }
   /* Allocate the results array */
   *result_num = 0;
   /* Create the temporary stack array */
   stack_ptr = 0;
   stack = MALLOC(t->tree_size, int);
   
   /* Put the root node of the tree onto the stack */
   stack[stack_ptr++] = 0;
   /* Search the k-D tree until the stack is empty */
   
   while (stack_ptr) {
      /* Pop the top entry off the stack */
      node = stack[--stack_ptr];
      /* Check if the query box intersects an epsilon-expanded safety box for
         the current node. */
      Bounds_Copy(&(t->tree_safety_boxes[node]), &sb);
      //Bounds_AddEpsilon(&sb, ENTITY_COINCIDENCE_TOLERANCE);
      /* If the query box doesn't intersect this node's safety box, we are done
         visiting the node and should continue with the next node */
      if(!Bounds_IsOverlappingBounds(&sb, box))
         continue;
      /* If the current node is a leaf node, add it to the collision list. If
         the current node is an interior node, add its children to the stack. */
      if(t->tree_link[node] <= 0) {
         result_indicies[*result_num] = - t->tree_link[node];
         (*result_num)++;
      }
      else {
         stack[stack_ptr++] = t->tree_link[node];
         stack[stack_ptr++] = t->tree_link[node] + 1;
      }
   }
   /* Destroy the temporary stack array */
   FREE(stack);
}

void KDTree_QueryCircleIntersect_Double(TKDTree* t,
                                 int* result_num, int* result_indicies,
                                 double circ_radius, int ncells, 
                                 double *x, double *dx, double *y, double *dy)
{
   assert(t && result_num && result_indicies && circ_radius);
   /* Build the k-D tree if necessary */
   if(!t->tree_built){
      //printf("BUILDING TREE... \n");
      //fflush(stdout);
      KDTree_CreateTree(t);
   }
   
   int nez;
   int *ind=(int *)malloc(ncells*sizeof(int));
   
   TBounds box;
   box.min.x = -circ_radius;
   box.max.x =  circ_radius;
   box.min.y = -circ_radius;
   box.max.y =  circ_radius;
   KDTree_QueryBoxIntersect(t, &nez, ind, &box);
   
   //for (int ic=0; ic<nez; ic++) {
   //   printf("box is ind[%d]=%d\n",ic,ind[ic]);
   //}
   
   /* Allocate the results array */
   *result_num = 0;

   double rad1, rad2, rad3, rad4;
   int ii;
   for (int i=0; i<nez; ++i){
      ii = ind[i];
      rad1 = sqrt( pow(x[ii],       2.0) + pow(y[ii],       2.0) );
      rad2 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii],       2.0) );
      rad3 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii]+dy[ii],2.0) );
      rad4 = sqrt( pow(x[ii]       ,2.0) + pow(y[ii]+dy[ii],2.0) );
      
      if ((circ_radius < rad1 && circ_radius > rad2 ) ||
          (circ_radius > rad1 && circ_radius < rad2 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      } else if ((circ_radius < rad2 && circ_radius > rad3 ) ||
          (circ_radius > rad2 && circ_radius < rad3 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      } else if ((circ_radius < rad3 && circ_radius > rad4 ) ||
          (circ_radius > rad3 && circ_radius < rad4 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      } else if ((circ_radius < rad4 && circ_radius > rad1 ) ||
          (circ_radius > rad4 && circ_radius < rad1 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      }
   } // for  
   free(ind);
}

void KDTree_QueryCircleIntersect_Float(TKDTree* t,
                                 int* result_num, int* result_indicies,
                                 double circ_radius, int ncells, 
                                 float *x, float *dx, float *y, float *dy)
{
   assert(t && result_num && result_indicies && circ_radius);
   /* Build the k-D tree if necessary */
   if(!t->tree_built){
      //printf("BUILDING TREE... \n");
      //fflush(stdout);
      KDTree_CreateTree(t);
   }
   
   int nez;
   int *ind=(int *)malloc(ncells*sizeof(int));
   
   TBounds box;
   box.min.x = -circ_radius;
   box.max.x =  circ_radius;
   box.min.y = -circ_radius;
   box.max.y =  circ_radius;
   KDTree_QueryBoxIntersect(t, &nez, ind, &box);
   
   //for (int ic=0; ic<nez; ic++) {
   //   printf("box is ind[%d]=%d\n",ic,ind[ic]);
   //}
   
   /* Allocate the results array */
   *result_num = 0;

   double rad1, rad2, rad3, rad4;
   int ii;
   for (int i=0; i<nez; ++i){
      ii = ind[i];
      rad1 = sqrt( pow(x[ii],       2.0) + pow(y[ii],       2.0) );
      rad2 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii],       2.0) );
      rad3 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii]+dy[ii],2.0) );
      rad4 = sqrt( pow(x[ii]       ,2.0) + pow(y[ii]+dy[ii],2.0) );
      
      if ((circ_radius < rad1 && circ_radius > rad2 ) ||
          (circ_radius > rad1 && circ_radius < rad2 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      } else if ((circ_radius < rad2 && circ_radius > rad3 ) ||
          (circ_radius > rad2 && circ_radius < rad3 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      } else if ((circ_radius < rad3 && circ_radius > rad4 ) ||
          (circ_radius > rad3 && circ_radius < rad4 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      } else if ((circ_radius < rad4 && circ_radius > rad1 ) ||
          (circ_radius > rad4 && circ_radius < rad1 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      }
   } // for  
   free(ind);
}

void KDTree_QueryCircleIntersectWeighted_Double(TKDTree* t,
                                 int* result_num, int* result_indicies, double *weight,
                                 double circ_radius, int ncells, 
                                 double *x, double *dx, double *y, double *dy)
{
   assert(t && result_num && result_indicies && circ_radius);
   /* Build the k-D tree if necessary */
   if(!t->tree_built){
      //printf("BUILDING TREE... \n");
      //fflush(stdout);
      KDTree_CreateTree(t);
   }
   
   int nez;
   int *ind=(int *)malloc(ncells*sizeof(int));
   
   TBounds box;
   box.min.x = -circ_radius;
   box.max.x =  circ_radius;
   box.min.y = -circ_radius;
   box.max.y =  circ_radius;
   KDTree_QueryBoxIntersect(t, &nez, ind, &box);
   
   //for (int ic=0; ic<nez; ic++) {
   //   printf("box is ind[%d]=%d\n",ic,ind[ic]);
   //}
   
   /* Allocate the results array */
   *result_num = 0;
   
   double rad1, rad2, rad3, rad4;
   int cuts_bottom, cuts_top, cuts_left, cuts_right;
   int vertical_half, horizontal_half;
   int ii;
   for (int i=0; i<nez; ++i){
      ii = ind[i];
      rad1 = sqrt( pow(x[ii],       2.0) + pow(y[ii],       2.0) );
      rad2 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii],       2.0) );
      rad3 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii]+dy[ii],2.0) );
      rad4 = sqrt( pow(x[ii]       ,2.0) + pow(y[ii]+dy[ii],2.0) );
      
      cuts_bottom=0;
      cuts_top=0;
      cuts_left=0;
      cuts_right=0;
      if ((circ_radius < rad1 && circ_radius > rad2 ) ||
          (circ_radius > rad1 && circ_radius < rad2 ) ) {
         cuts_bottom=1;
      }
      if ((circ_radius < rad2 && circ_radius > rad3 ) ||
          (circ_radius > rad2 && circ_radius < rad3 ) ) {
         cuts_right=1;
      }
      if ((circ_radius < rad3 && circ_radius > rad4 ) ||
          (circ_radius > rad3 && circ_radius < rad4 ) ) {
         cuts_top=1;
      }
      if ((circ_radius < rad4 && circ_radius > rad1 ) ||
          (circ_radius > rad4 && circ_radius < rad1 ) ) {
         cuts_left=1;
      }
      
      horizontal_half=0;
      vertical_half=0;
      if (x[ii]+0.5*dx[ii] > 0.0) horizontal_half = RIGHT_HALF;
      if (y[ii]+0.5*dy[ii] > 0.0) vertical_half   = TOP_HALF;
      
      
      if        (horizontal_half == RIGHT_HALF && vertical_half == TOP_HALF) { /* quadrant 1 */
         weight[*result_num] = (circ_radius - rad1)/(rad3-rad1);
      } else if (horizontal_half == LEFT_HALF  && vertical_half == TOP_HALF) { /* quadrant 2 */
         weight[*result_num] = (circ_radius - rad2)/(rad4-rad2);
      } else if (horizontal_half == LEFT_HALF  && vertical_half == BOTTOM_HALF) { /* quadrant 3 */
         weight[*result_num] = (circ_radius - rad3)/(rad1-rad3);
      } else if (horizontal_half == RIGHT_HALF && vertical_half == BOTTOM_HALF) { /* quadrant 4 */
         weight[*result_num] = (circ_radius - rad4)/(rad2-rad4);
      } else {
         weight[*result_num] = 0.5;
      }
      if (cuts_bottom || cuts_top || cuts_left || cuts_right) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      }

   } // for  
   free(ind);
}

void KDTree_QueryCircleIntersectWeighted_Float(TKDTree* t,
                                 int* result_num, int* result_indicies, double *weight,
                                 double circ_radius, int ncells, 
                                 float *x, float *dx, float *y, float *dy)
{
   assert(t && result_num && result_indicies && circ_radius);
   /* Build the k-D tree if necessary */
   if(!t->tree_built){
      //printf("BUILDING TREE... \n");
      //fflush(stdout);
      KDTree_CreateTree(t);
   }
   
   int nez;
   int *ind=(int *)malloc(ncells*sizeof(int));
   
   TBounds box;
   box.min.x = -circ_radius;
   box.max.x =  circ_radius;
   box.min.y = -circ_radius;
   box.max.y =  circ_radius;
   KDTree_QueryBoxIntersect(t, &nez, ind, &box);
   
   //for (int ic=0; ic<nez; ic++) {
   //   printf("box is ind[%d]=%d\n",ic,ind[ic]);
   //}
   
   /* Allocate the results array */
   *result_num = 0;
   
   double rad1, rad2, rad3, rad4;
   int cuts_bottom, cuts_top, cuts_left, cuts_right;
   int vertical_half, horizontal_half;
   int ii;
   for (int i=0; i<nez; ++i){
      ii = ind[i];
      rad1 = sqrt( pow(x[ii],       2.0) + pow(y[ii],       2.0) );
      rad2 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii],       2.0) );
      rad3 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii]+dy[ii],2.0) );
      rad4 = sqrt( pow(x[ii]       ,2.0) + pow(y[ii]+dy[ii],2.0) );
      
      cuts_bottom=0;
      cuts_top=0;
      cuts_left=0;
      cuts_right=0;
      if ((circ_radius < rad1 && circ_radius > rad2 ) ||
          (circ_radius > rad1 && circ_radius < rad2 ) ) {
         cuts_bottom=1;
      }
      if ((circ_radius < rad2 && circ_radius > rad3 ) ||
          (circ_radius > rad2 && circ_radius < rad3 ) ) {
         cuts_right=1;
      }
      if ((circ_radius < rad3 && circ_radius > rad4 ) ||
          (circ_radius > rad3 && circ_radius < rad4 ) ) {
         cuts_top=1;
      }
      if ((circ_radius < rad4 && circ_radius > rad1 ) ||
          (circ_radius > rad4 && circ_radius < rad1 ) ) {
         cuts_left=1;
      }
      
      horizontal_half=0;
      vertical_half=0;
      if (x[ii]+0.5*dx[ii] > 0.0) horizontal_half = RIGHT_HALF;
      if (y[ii]+0.5*dy[ii] > 0.0) vertical_half   = TOP_HALF;
      
      
      if        (horizontal_half == RIGHT_HALF && vertical_half == TOP_HALF) { /* quadrant 1 */
         weight[*result_num] = (circ_radius - rad1)/(rad3-rad1);
      } else if (horizontal_half == LEFT_HALF  && vertical_half == TOP_HALF) { /* quadrant 2 */
         weight[*result_num] = (circ_radius - rad2)/(rad4-rad2);
      } else if (horizontal_half == LEFT_HALF  && vertical_half == BOTTOM_HALF) { /* quadrant 3 */
         weight[*result_num] = (circ_radius - rad3)/(rad1-rad3);
      } else if (horizontal_half == RIGHT_HALF && vertical_half == BOTTOM_HALF) { /* quadrant 4 */
         weight[*result_num] = (circ_radius - rad4)/(rad2-rad4);
      } else {
         weight[*result_num] = 0.5;
      }
      if (cuts_bottom || cuts_top || cuts_left || cuts_right) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      }

   } // for  
   free(ind);
}

void KDTree_QueryCircleInterior_Double(TKDTree* t,
                            int* result_num, int* result_indicies,
                            double circ_radius, int ncells, 
                            double *x, double *dx, double *y, double *dy)
{
   assert(t && result_num && result_indicies && circ_radius);
   /* Build the k-D tree if necessary */
   if(!t->tree_built){
      //printf("BUILDING TREE... \n");
      //fflush(stdout);
      KDTree_CreateTree(t);
   }
   
   int nez;
   int *ind=(int *)malloc(ncells*sizeof(int));
   
   TBounds box;
   box.min.x = -circ_radius;
   box.max.x =  circ_radius;
   box.min.y = -circ_radius;
   box.max.y =  circ_radius;
   KDTree_QueryBoxIntersect(t, &nez, ind, &box);
   
   //for (int ic=0; ic<nez; ic++) {
   //   printf("box is ind[%d]=%d\n",ic,ind[ic]);
   //}
   
   /* Allocate the results array */
   *result_num = 0;
   
   double rad1, rad2, rad3, rad4;
   int ii;
   for (int i=0; i<nez; ++i){
      ii = ind[i];
      rad1 = sqrt( pow(x[ii],       2.0) + pow(y[ii],       2.0) );
      rad2 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii],       2.0) );
      rad3 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii]+dy[ii],2.0) );
      rad4 = sqrt( pow(x[ii]       ,2.0) + pow(y[ii]+dy[ii],2.0) );
      
      if ((circ_radius > rad1 || circ_radius > rad2 ) ||
          (circ_radius > rad3 || circ_radius > rad4 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      }
   } // for  
   free(ind);
}

void KDTree_QueryCircleInterior_Float(TKDTree* t,
                            int* result_num, int* result_indicies,
                            double circ_radius, int ncells, 
                            float *x, float *dx, float *y, float *dy)
{
   assert(t && result_num && result_indicies && circ_radius);
   /* Build the k-D tree if necessary */
   if(!t->tree_built){
      //printf("BUILDING TREE... \n");
      //fflush(stdout);
      KDTree_CreateTree(t);
   }
   
   int nez;
   int *ind=(int *)malloc(ncells*sizeof(int));
   
   TBounds box;
   box.min.x = -circ_radius;
   box.max.x =  circ_radius;
   box.min.y = -circ_radius;
   box.max.y =  circ_radius;
   KDTree_QueryBoxIntersect(t, &nez, ind, &box);
   
   //for (int ic=0; ic<nez; ic++) {
   //   printf("box is ind[%d]=%d\n",ic,ind[ic]);
   //}
   
   /* Allocate the results array */
   *result_num = 0;
   
   double rad1, rad2, rad3, rad4;
   int ii;
   for (int i=0; i<nez; ++i){
      ii = ind[i];
      rad1 = sqrt( pow(x[ii],       2.0) + pow(y[ii],       2.0) );
      rad2 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii],       2.0) );
      rad3 = sqrt( pow(x[ii]+dx[ii],2.0) + pow(y[ii]+dy[ii],2.0) );
      rad4 = sqrt( pow(x[ii]       ,2.0) + pow(y[ii]+dy[ii],2.0) );
      
      if ((circ_radius > rad1 || circ_radius > rad2 ) ||
          (circ_radius > rad3 || circ_radius > rad4 ) ) {
         result_indicies[*result_num] = ind[i];
         (*result_num)++;
      }
   } // for  
   free(ind);
}