/*
   File:        incremental.c
   Author:      Dan Pelleg and Andrew W. Moore
   Created:     Fri Feb 19 17:19:46 EST 1999
   Description: Incrementally growing kdtrees

   Copyright 1999, The Auton Lab
*/

#include "incremental.h"
#include "amgr.h" /* Only needed for testing */

static const char *const CVSID = "$Id: incremental.c,v 1.20 2004/01/20 12:28:53 dpelleg Exp $";

static dyvp_array *mk_dyvp_array_points_in_kdtree(const simple_kdtree *kd);

static void collect_dyv_points_in_kdtree(dyvp_array **ret, const simple_kdtree *kd);

void draw_tree_outline_helper(simple_kdtree *kd,hrect *hr)
{
  if ( kd->points == NULL )
  {
    double temp_lo = hrect_lo_ref(hr,kd->split_dim);
    double temp_hi = hrect_hi_ref(hr,kd->split_dim);
    if ( kd->split_dim == 0 )
      ag_line(kd->split_val,hrect_lo_ref(hr,1),kd->split_val,hrect_hi_ref(hr,1));
    else
      ag_line(hrect_lo_ref(hr,0),kd->split_val,hrect_hi_ref(hr,0),kd->split_val);
    hrect_hi_set(hr,kd->split_dim,kd->split_val);
    draw_tree_outline_helper(kd->left,hr);
    hrect_hi_set(hr,kd->split_dim,temp_hi);
    hrect_lo_set(hr,kd->split_dim,kd->split_val);
    draw_tree_outline_helper(kd->right,hr);
    hrect_lo_set(hr,kd->split_dim,temp_lo);
  }
}

void draw_tree_outline(simple_kdtree *kd)
{
  hrect *hr = mk_copy_hrect(kd->hr);
  ag_hrect(hr);
  draw_tree_outline_helper(kd,hr);
  free_hrect(hr);
}

void draw_node(simple_kdtree *root,dym *x,simple_kdtree *kd)
{
  double cx = dyv_ref(kd->sum,0)/kd->num_points;
  double cy = dyv_ref(kd->sum,1)/kd->num_points;
  ag_on("");
  ag_set_pen_color(AG_YELLOW);
  ag_box(hrect_lo_ref(kd->hr,0),hrect_lo_ref(kd->hr,1),hrect_hi_ref(kd->hr,0),hrect_hi_ref(kd->hr,1));
  ag_set_pen_color(AG_GREEN);
  ag_hrect(kd->hr);
  ag_set_pen_color(AG_CYAN);
  draw_tree_outline(root);
  ag_set_pen_color(AG_BLACK);
  draw_simple_x(x);
  if ( kd->points != NULL )
  {
    int i;

    ag_set_pen_color(AG_RED);
    for ( i = 0 ; i < dyv_array_size(kd->points) ; i++ )
    {
      const dyv *row = dyv_array_ref(kd->points, i);
      ag_dot(dyv_ref(row,0),dyv_ref(row,1));
    }
  }
  ag_set_pen_color(AG_BLUE);
  ag_dot(cx,cy);
  ag_print(cx,cy,"Cen");
  ag_off();
}    

void graphical_tree_walk_helper(simple_kdtree *root,dym *x,simple_kdtree *kd)
{
  if ( kd->left != NULL )
  {
    draw_node(root,x,kd);
    wait_for_key();
    graphical_tree_walk_helper(root,x,kd->left);
  }
   
  draw_node(root,x,kd);
  wait_for_key();

  if ( kd->right != NULL )
  {
    graphical_tree_walk_helper(root,x,kd->right);
    draw_node(root,x,kd);
    wait_for_key();
  }
}    

void graphical_tree_walk(simple_kdtree *root,dym *x)
{
  graphical_tree_walk_helper(root,x,root);
}

/* Call a simple_kdtree node PARTMADE if the following fields are correct...
     split_dim, split_val, left, right, points, data
   but the following fields are incorrect
     num_points is -77
     sum is NULL
     sum_mags is -77.7
     hr is NULL
     rel_width is -77.7
*/

/* Warning, this function puts both its arguments in as pointers without
   copying, so don't touch or free points or data after calling this. */
simple_kdtree *mk_incremental_node_incfree_points(dyv_array *data, ivec *id_data, int numpoints)
{
  simple_kdtree *kd = AM_MALLOC(simple_kdtree);
  kd -> split_dim         = -77;
  kd -> split_val         = -77.7;
  kd -> left              = NULL;
  kd -> right             = NULL;
  kd -> cached            = NULL;
  kd -> num_points        = numpoints;
  kd -> sum               = NULL;
  kd -> sum_mags          = -77.7;
  kd -> hr                = NULL;
  kd -> rel_width         = -77.7;
  kd -> points            = data;
  kd -> point_ids         = id_data;
  return kd;
}

void free_partmade_node_but_not_its_children(simple_kdtree *kd)
{
  if ( kd -> sum != NULL ) my_error("kd->sum should be NULL in a partmade node");
  if ( kd -> hr != NULL ) my_error("kd->hr should be NULL in a partmade node");
  if ( kd->points != NULL ) {
	free_dyv_array(kd->points);
  }
  if ( kd->point_ids != NULL ) {
	free_ivec(kd->point_ids);
  }
  kd->num_points = 0;
  AM_FREE(kd,simple_kdtree);
}

/* Adds the point to the partmade simple_kdtree. 
   The same hr must always be used when adding to the same kd */
void add_to_partmade_simple_kdtree(simple_kdtree *kd, const hrect *universe_hr,
                                   const dyv *point, int pt_id,
                                   double min_box_width_rel, int max_leaf_size)
{
  bool finished = FALSE;
  hrect *kd_hr = mk_copy_hrect(universe_hr);
  /* Invariant: kd_hr will be the known largest possible hyperrect around
     the current node (i.e. around kd) */

  while ( !finished )
  {
    bool is_leaf = kd->points != NULL;
    if ( is_leaf )
    {							/* try to add point to current node */
	  /* is there room for it in the current node? */
      if ( kd->num_points < max_leaf_size && kd->num_points < dyv_array_size(kd->points))
      {
		/* yes -- add it */
		dyv_array_set(kd->points, kd->num_points, point);
        ivec_set(kd->point_ids, kd->num_points, pt_id);
        kd->num_points++;
        finished = TRUE;
      }
      else						/* make room in node */
      {
        /* 
           Try this sometime:
           The split dim is normally the widest one. However if the node is excessively
           big, that means our splitting fails. In that case just choose a random dimension
           to split by.
           int split_dim = kd->num_points > 4*max_leaf_size ?
           int_random(dyv_size(dyv_array_ref(kd->points, 0))) :
           hrect_widest_dim(kd_hr);
        */
        int split_dim = hrect_widest_dim(kd_hr);
        double split_val = hrect_middle_ref(kd_hr,split_dim);
        double rel_width = hrect_width_ref(kd_hr,split_dim) / hrect_width_ref(universe_hr,split_dim);
        const int min_leaf_size = max_leaf_size/2;
        dyv_array *left_rows;
        dyv_array *right_rows;
        ivec *left_ids;
        ivec *right_ids;
        lrsize sizes;
        bool big_node = (1+kd->num_points > 4*max_leaf_size) || (rel_width > min_box_width_rel);

        kd->split_dim = split_dim;
        kd->split_val = split_val;
        kd->rel_width = rel_width;

        /* see if a split has enough points in each leaf */
        sizes = get_left_and_right_sizes(kd->points,
                                         kd->num_points,
                                         kd->split_dim, kd->split_val);
        if((big_node && sizes.left > 0 && sizes.right > 0)
           || (sizes.left > min_leaf_size && sizes.right > min_leaf_size)) {
          /* yes -- create children, allocate and split */
          sizes = make_left_and_right_points(kd->points,
                                             kd->point_ids,
                                             kd->num_points,
                                             kd->split_dim, kd->split_val,
                                             &left_rows, &right_rows,
                                             &left_ids, &right_ids,
                                             max_leaf_size);

          free_dyv_array(kd->points); /* no need for these anymore */
          kd -> points = NULL;
          free_ivec(kd->point_ids);	/* no need for these anymore */
          kd -> point_ids = NULL;
          kd -> num_points = 0;
          kd -> left = mk_incremental_node_incfree_points(left_rows, left_ids, sizes.left);
          kd -> right = mk_incremental_node_incfree_points(right_rows, right_ids, sizes.right);
        } else {                /* no even split -- just alloc more in current leaf if needed */
          if(kd->num_points >= dyv_array_size(kd->points)) {
            int i;
            dyv *zero = mk_zero_dyv(dyv_size(dyv_array_ref(kd->points, 0)));
            for(i=0; i<max_leaf_size; i++) {
              add_to_dyv_array(kd->points, zero);
              add_to_ivec(kd->point_ids, 0);
            }
            free_dyv(zero);
          }
          /* add current point */
          dyv_array_set(kd->points, kd->num_points, point);
          ivec_set(kd->point_ids, kd->num_points, pt_id);
          kd->num_points++;
          finished = TRUE;
        }
      }
    }
    if ( !finished )			/* traverse tree to find a good home */
    {
      if ( dyv_ref(point,kd->split_dim) <= kd->split_val )
      {
        hrect_hi_set(kd_hr,kd->split_dim,kd->split_val);
        kd = kd -> left;
      }
      else
      {
        hrect_lo_set(kd_hr,kd->split_dim,kd->split_val);
        kd = kd -> right;
      }
    }
  }
  free_hrect(kd_hr);
}

/* Ignores line if contains non-numerics, and proceeds to next line.
   Returns NULL if reaches end of file.
   Ignores line if empty.
   my_errors if line contains wrong number of numbers.
*/

dyv *mk_dyv_from_line_of_file(FILE *s,int size) {
  dyv *point = mk_dyv(size);
  dyv_from_line_of_file(s, size, &point);
  return point;
}


void dyv_from_line_of_file(FILE *s, int size, dyv **point)
{
  bool finished = FALSE;
  int index = 0;
  int c;

  c = getc(s);

  while ( !finished && c != EOF )
  {
    while ( c != EOF && c != '\n' && (c == ' ' || c == ',' || c == '\t') )
      c = getc(s);

    if ( c == '\n' )
    {
      if ( index == size )
        finished = TRUE;
      else if ( index > 0 )
        my_error("Line had too few numbers\n");
      else
	c = getc(s);
    }

    if ( c != EOF && c != '\n' )
    {
      double value;
      char buff[1000];
      int i = 0;
      while ( c != EOF && c != '\n' && c != ',' && c != ' ' && c != '\t' )
      {
        buff[i] = (char)c;
        c = getc(s);
        i += 1;
      }
      buff[i] = '\0';
      value = atof(buff);
      if ( value == 0.0 && !is_a_number(buff) )
      {
	printf("Ignoring non-numeric line\n");
	index = 0;
	while ( c != EOF && c != '\n' )
	  c = getc(s);
	c = getc(s);
      }
      else
      {
        dyv_set(*point,index,value);
        index += 1;
      }
    }
  }

  if ( c == EOF )
  {
    if ( index > 0 ) 
      my_error("File ended in middle of a line\n");
    free_dyv(*point);
    *point = NULL;
  }
}
    
bool is_a_power_of_two(int n)
{
  bool result = TRUE;
  if ( n > 0xFFFF )
  {
    if ( (n & 0xFFFF) == 0 )
      n = n >> 16;
    else
      result = FALSE;
  }
  if ( result && n > 0xFF )
  {
    if ( (n & 0xFF) == 0 )
      n = n >> 8;
    else
      result = FALSE;
  }
  if ( result && n > 0xF )
  {
    if ( (n & 0xF) == 0 )
      n = n >> 4;
    else
      result = FALSE;
  }
  result = result && (n==1 || n==2 || n==4 || n==8);
  return result;
}
   
void make_partmade_simple_kdtree_from_stream(FILE *s, const hrect *universe_hr,
                                             double min_box_width_rel, int max_leaf_size,
                                             simple_kdtree **r_kd)
{
  int size = hrect_size(universe_hr);
  dyv_array *points = mk_empty_dyv_array();
  ivec *point_ids;
  bool finished = FALSE;
  int npoints = 0;
  dyv *point = mk_zero_dyv(size);

  /* pre-allocate dyv_array and ivec */
  int i;
  for(i=0; i<max_leaf_size; i++) {
    add_to_dyv_array(points, point);
  }

  point_ids  = mk_zero_ivec(max_leaf_size);
  constant_ivec(point_ids, KDTREE_NO_POINT);

  *r_kd = mk_incremental_node_incfree_points(points, point_ids, 0);
  while ( !finished )
  {
    dyv_from_line_of_file(s,size, &point);
    if ( point == NULL )
      finished = TRUE;
    else
    {
      if ( !is_in_hrect(universe_hr,point) ) {
        const int offending_dim = first_outlying_dim(universe_hr, point);
        fprintf_dyv(stderr, "outlier", point, "");
        my_errorf("You tried to include a point outside the universe!\n"
                  "In dimension #%d the value is %f but it should be in the interval "
                  "[%f, %f]\n",
                  offending_dim,
                  dyv_ref(point, offending_dim),
                  hrect_lo_ref(universe_hr, offending_dim),
                  hrect_hi_ref(universe_hr, offending_dim)
                  );
      }
      add_to_partmade_simple_kdtree(*r_kd, universe_hr, point, npoints,
                                    min_box_width_rel, max_leaf_size);
	  npoints++;
    }
    if ( finished || is_a_power_of_two(npoints) )
    {
      printf("(%d point%s) ", npoints, (npoints==1)?"":"s");
      fflush(stdout);
    }
  }
  printf("\n");
  if(point != NULL)
	free_dyv(point);
}

hrect *mk_bounder_of_two_hrects(hrect *a,hrect *b)
{
  hrect *hr = mk_copy_hrect(a);
  int i;
  for ( i = 0 ; i < hrect_size(a) ; i++ )
  {
    double blo = hrect_lo_ref(b,i);
    double bhi = hrect_hi_ref(b,i);
    if ( blo < hrect_lo_ref(hr,i) )
      hrect_lo_set(hr,i,blo);
    if ( bhi > hrect_hi_ref(hr,i) )
      hrect_hi_set(hr,i,bhi);
  }
  return hr;
}

/* This walks through the tree and if it ever finds a node with a child
   containing no points, it removes the parent node and the pointless
   node, putting the point-having child in place of the parent, and adding
   any points the small node might have had (not implemented). The resulting tree contains the
   same set of points as the input tree...just no small nodes.

   Node you must pass a pointer to a simple_kdtree pointer, i.e. call
   with merge_partmade_simple_kdtree(&kd) */
void merge_partmade_simple_kdtree(simple_kdtree **kd_ptr)
{
  simple_kdtree *kd = *kd_ptr;
  if ( kd->points == NULL )
  {
    bool left_pointless;
    bool right_pointless;
    
    merge_partmade_simple_kdtree(&kd->left);
    merge_partmade_simple_kdtree(&kd->right);
  
    left_pointless = kd->left->points != NULL && kd->left->num_points == 0;
    right_pointless = kd->right->points != NULL && kd->right->num_points == 0;
  
    if ( left_pointless && right_pointless ) /* I should really handle this */
      my_error("Impossible for a partmade tree to have left and right both pointless");
    else if ( !left_pointless && !right_pointless )
    {
      /* skip */
    }
    else
    {
      simple_kdtree *bad  = (left_pointless) ? kd->left : kd->right;
      simple_kdtree *good = (left_pointless) ? kd->right : kd->left;
      free_partmade_node_but_not_its_children(bad);
      free_partmade_node_but_not_its_children(kd);
      *kd_ptr = good;
    }
  }
}

/* Finds the best values split_dim and split_val such that
   x in a implies   x[splitdim] <= split_val
   x in b implies   x[splitdim] >= split_val
   my_errors if no such values exist (i.e. if a and b intersect)
   chooses the values so as to put the split as far away from possible
   between rectangles */
void choose_split_from_hrects(hrect *a,hrect *b,int *r_split_dim,double *r_split_val)
{
  int i;
  double widest = -777.7;
  *r_split_dim = -1;
  *r_split_val = -777.77;
  for ( i = 0 ; i < hrect_size(a) ; i++ )
  {
    double ahi = hrect_hi_ref(a,i);
    double blo = hrect_lo_ref(b,i);
    if ( ahi < blo )
    {
      double width = blo - ahi;
      if ( *r_split_dim < 0 || width > widest )
      {
        widest = width;
        *r_split_dim = i;
        *r_split_val = (ahi + blo)/2.0;
      }
    }
  }
  if ( *r_split_dim < 0 )
    my_error("No split between rectangles!");
}

double hrect_sqd_norm(const hrect *hr)
{
  double ret = 0.0;
  int size = hrect_size(hr);
  int i;
  for(i=0; i<size; i++) {
	double width = hrect_width_ref(hr, i);
	ret += width*width;
  }
  return ret;
}

void complete_partmade_simple_kdtree(simple_kdtree *kd,hrect *universe_hr)
{
  int widest_dim;
  if ( kd -> points == NULL )
  {
    complete_partmade_simple_kdtree(kd->left,universe_hr);
    complete_partmade_simple_kdtree(kd->right,universe_hr);
    choose_split_from_hrects(kd->left->hr,kd->right->hr,&kd->split_dim,&kd->split_val);
    kd -> num_points = kd->left->num_points + kd->right->num_points;
    kd -> sum = mk_dyv_plus(kd->left->sum,kd->right->sum);
    kd -> sum_mags = kd->left->sum_mags + kd->right->sum_mags;
    kd -> hr = mk_bounder_of_two_hrects(kd->left->hr,kd->right->hr);
  }
  else
  {
    /* trim kd->points in indices above kd->num_points */
    while(dyv_array_size(kd->points) > kd->num_points)
      dyv_array_remove(kd->points, dyv_array_size(kd->points)-1);
    kd -> sum = mk_sum_of_dyv_array(kd->points);
    kd -> sum_mags = sum_mags_of_dyv_array(kd->points);
    if ( dyv_array_size(kd->points) == 0 )
      my_error("Should not find a node with no points");

    kd -> hr = mk_hrect_bounding_dyv_array(kd->points);
  }
  widest_dim = hrect_widest_dim(kd->hr);
  kd -> rel_width = hrect_width_ref(kd->hr,widest_dim) / hrect_width_ref(universe_hr,widest_dim);

  /* calculate bias: npoints . sum_{i dims}(hr.width_i/universe.width_i)^2 */
  {
	int i;
	int num_dims = hrect_size(kd->hr);
	kd->bias = 0;
	for(i=0; i<num_dims; i++) {
	  double tmp = hrect_width_ref(kd->hr,i)/hrect_width_ref(universe_hr,i);
	  tmp *= tmp;
	  kd->bias += tmp;
	}
	kd->bias *= kd->num_points;
  }
}

void show_leaflist_sizes(const simple_kdtree *kd)
{
  if ( kd -> points != NULL )
  {
    printf("%d\n", kd->num_points);
  }
  else
  {
    show_leaflist_sizes(kd->left);
    show_leaflist_sizes(kd->right);
  }
}

void make_simple_kdtree_from_stream(FILE *s,hrect *universe_hr,
                                    double min_box_width_rel, int max_leaf_size,
                                    simple_kdtree **r_kd, 
                                    dyvp_array **ret_data_list)
{
  make_partmade_simple_kdtree_from_stream(s,universe_hr,min_box_width_rel,max_leaf_size,r_kd);
  merge_partmade_simple_kdtree(r_kd);
  complete_partmade_simple_kdtree(*r_kd,universe_hr);
  if(ret_data_list) {
    *ret_data_list = mk_dyvp_array_points_in_kdtree(*r_kd);
  }
}

hrect *mk_hrect_from_filename(char *filename,char **r_errmess)
{
  dym *dm = mk_dym_from_filename(filename,r_errmess);
  hrect *hr = NULL;
  if ( dm == NULL )
  {
    char *newerrmess = 
      mk_printf("Problem loading data for making a hyper-rectangle: %s",
                *r_errmess);
    free_string(*r_errmess);
    *r_errmess = newerrmess;
  }
  else if ( dym_cols(dm) != 2 )
    *r_errmess = mk_printf("When loading a hyper-rectangle I expect two\n"
                           "items on each line, and one line for each\n"
                           "dimension. But You gave me %d items(s) per line\n",
                           dym_cols(dm));
  else
  {
    dyv *lo = mk_dyv_from_dym_col(dm,0);
    dyv *hi = mk_dyv_from_dym_col(dm,1);
    hr = mk_hrect(lo,hi);
    free_dyv(lo);
    free_dyv(hi);
  }
  if ( dm != NULL ) free_dym(dm);
  return hr;
}

void save_hrect(const char *filename, const hrect *hr)
{
  int i;
  FILE *s = safe_fopen(filename,"w");
  fprintf(s,"# hrect (hyper-rectangle) data structure saved by Auton code\n");
  for ( i = 0 ; i < hrect_size(hr) ; i++ ) {
	/* find an accuracy which does not do harm to the boundaries */
    /* NOTE: merging the calls won't work: hrect_boundary_format_str
       returns result in a static vector! */
    fprintf(s, "%s ",
            hrect_boundary_format_str(hrect_lo_ref(hr,i), FALSE));
    fprintf(s, "%s\n",
            hrect_boundary_format_str(hrect_hi_ref(hr,i), TRUE));
  }
  fclose(s);
}

const char *const hrect_boundary_format_str(double num, bool hi)
{
  static char ret[128];
  int ndigits;
  bool done = FALSE;
  int nincs = 0;

  do {
    for(ndigits = 3; !done && ndigits<15; ndigits++) {
      char format[128];
      double dummy;
      sprintf(format, "%%.%df", ndigits);	/* the format string */
      sprintf(ret, format, num);	/* print it using the format string */
      sscanf(ret, "%lf", &dummy);
      if(hi && dummy >= num)
        done = TRUE;
      if(!hi && dummy <= num) 
        done = TRUE;
    }
    if(!done) {
      const double rel_sill = 1e-3; /* how much the window increases by, relatively */
      const double sill = rel_sill*fabs(num);
      double newnum = hi ? num + sill : num - sill;
      if(fabs(newnum - num) < 1e-14) {       /* check if this helped */
        /* if here, number is very small, use magnitude as sill */
        newnum = hi ? num + fabs(num) : num - fabs(num);
      }
      num = newnum;  /* increase/decrese number for next round */
    }

  } while(!done && ++nincs <= 10);

  if(!done)
	my_error("hrect_boundary_format_str can't find good precision");
  
  return ret;
}

hrect *mk_hrect_from_filename_simple(char *filename)
{
  char *errmess;
  hrect *hr = mk_hrect_from_filename(filename,&errmess);
  if ( hr == NULL )
    my_error(errmess);
  return hr;
}

void make_simple_kdtree_from_file(const char *filename,
                                  double min_box_width_rel, int max_leaf_size,
                                  simple_kdtree **r_kd, dyvp_array **data_list)
{
  char universe_name[200];
  sprintf(universe_name,"%s.universe",filename);
  if ( !file_exists(universe_name) )
    my_error(mk_printf("Universe file %s doesn't exist.\n"
                       "Create it with kmeans makeuni in %s",
                       universe_name,filename));
  else
  {
    hrect *universe_hr = mk_hrect_from_filename_simple(universe_name);
    FILE *s = safe_fopen(filename,"r");
    fprint_hrect(stdout,"","LoadedHrect",universe_hr);
    make_simple_kdtree_from_stream(s,universe_hr,min_box_width_rel,
                                   max_leaf_size, r_kd, data_list);
    fclose(s);
    free_hrect(universe_hr);
  }
}

/* Increases the length of each side of the hrect by the given fraction,
   while keeping the hrect's center at the same place */
void expand_hrect(hrect *hr,double frac)
{
  int i;
  for ( i = 0 ; i < hrect_size(hr) ; i++ )
  {
    double w = hrect_width_ref(hr,i) * (1.0 + frac) / 2.0;
    double m = hrect_middle_ref(hr,i);
    hrect_lo_set(hr,i,m-w);
    hrect_hi_set(hr,i,m+w);
  }
}

void makeuni_main(int argc,char *argv[])
{
  if ( argc < 4 )
    printf("Usage: kmeans makeuni in <datafilename>\n");
  else
  {
    const char *filename = string_from_args("in",argc,argv,"default.csv");
    dym *x = mk_simple_dym_from_ds_file(filename,argc,argv);
    char universe_name[200];
    hrect *hr = mk_hrect_bounding_dym_rows(x,NULL);
    sprintf(universe_name,"%s.universe",filename);
    /* no need to expand with new, format-checking, save_hrect */
    /* expand_hrect(hr,0.001); */
    save_hrect(universe_name,hr);
    free_dym(x);
    free_hrect(hr);
    printf("Saved bounding hyper-rectangle to %s\n",universe_name);
  }
}

dyvp_array *mk_dyvp_array_points_in_kdtree(const simple_kdtree *kd) {
  dyvp_array *ret = mk_dyvp_array(kd->num_points);
  collect_dyv_points_in_kdtree(&ret, kd);
  return ret;
}

void collect_dyv_points_in_kdtree(dyvp_array **ret, const simple_kdtree *kd)
{
  if(is_kd_leaf(kd)) {
    int i;
    for(i=0; i<kd->num_points; i++) {
      dyvp_array_set(*ret,
                     ivec_ref(kd->point_ids, i),
                     dyv_array_ref(kd->points, i));
    }
    return;
  } else {
    collect_dyv_points_in_kdtree(ret, kd->left);
    collect_dyv_points_in_kdtree(ret, kd->right);
    return;
  }
}

void incremental_main(int argc,char *argv[])
{
#if you_want_to_fix_this_go_ahead
  char *filename = string_from_args("in",argc,argv,"default.csv");
  dyv_array *x;
  simple_kdtree *kd;
  double min_box_width = double_from_args("min_box_width", argc, argv, 1e-3);
  int max_leaf_size = int_from_args("max_leaf_size", argc, argv, 40);
  int winsize = int_from_args("winsize",argc,argv,512);

  if(max_leaf_size + (min_box_width > 5) + (filename != NULL))
    /* just un-warning those unused variables */ ;

  make_simple_kdtree_and_x_from_file(filename,min_box_width,max_leaf_size,&kd,&x);
  if ( dyv_array_size(x) < 50 )
    fprintf_dyv_array(stdout,"x",x,"\n");
  if ( dyv_size(dyv_array_ref(x, 0)) == 2 )
  {
    hrect_set_ag_frame_with_border(kd->hr,0,1);
    ag_window_shape(winsize,winsize);
    ag_on("");

    /* graphical_tree_walk(kd,x); */
    ag_disappear();
  }
  fprint_simple_kdtree(stdout,"","kd",kd);
  wait_for_key();
  free_simple_kdtree(kd);
  free_dyv_array(x);
#endif /* you_want_to_fix_this_go_ahead */
}
