/* Gerris - The GNU Flow Solver
 * Copyright (C) 2010-2012 National Institute of Water and Atmospheric Research
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
 * 02111-1307, USA.  
 */

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <fcntl.h>
#include <unistd.h>

#include "kdt.h"

#define MIN(a,b) ((a) < (b) ? (a) : (b))
#define MAX(a,b) ((a) > (b) ? (a) : (b))

#define VERSION 20120405 /* the file format version */

/* same as the system tmpfile() but uses the working directory (rather
   than /tmp) */
FILE * kdt_tmpfile (void)
{
  char name[] = "kdtXXXXXX";
  int fd = mkstemp (name);
  if (fd == -1) {
    perror ("kdt_tmpfile");
    exit (1);
  }
  FILE * fp = fdopen (fd, "r+w");
  assert (unlink (name) == 0);
  if (fp == NULL) {
    perror ("kdt_tmpfile");
    exit (1);
  }
  return fp;
}

/* refcounted buffer */

typedef struct {
  KdtPoint * p;
  int ref;
} Buffer;

static Buffer * buffer_new (long len)
{
  Buffer * b = malloc (sizeof (Buffer));
  b->p = malloc (len*sizeof (KdtPoint));
  b->ref = 1;
  return b;
}

static Buffer * buffer_ref (Buffer * b)
{
  b->ref++;
  return b;
}

static void buffer_unref (Buffer * b)
{
  b->ref--;
  if (b->ref == 0) {
    free (b->p);
    free (b);
  }
}

/* KdtHeap */

void kdt_heap_resize (KdtHeap * h, long len)
{
  assert (h->len < 0 || len < h->len);
  if (h->len == h->buflen) {
    h->buflen = len;
    h->end = h->buflen;
  }
  else if (len <= h->buflen) {
    h->buflen = len;
    kdt_heap_rewind (h);
    assert (h->end == len);
  }
  h->len = len;
}

static long heap_read (KdtHeap * h, long len)
{
  if (ftell (h->fp) != h->current)
    assert (fseek (h->fp, h->current, SEEK_SET) == 0);
  if (h->len > 0) {
    long maxlen = h->start + h->len - h->current/sizeof (KdtPoint);
    if (len > maxlen)
      len = maxlen;
  }
  long n = 0;
  if (len > 0) {
    n = fread (h->p, sizeof (KdtPoint), len, h->fp);
    h->current = ftell (h->fp);
  }
  return n;
}

static void heap_write (KdtHeap * h, long len)
{
  if (ftell (h->fp) != h->current)
    assert (fseek (h->fp, h->current, SEEK_SET) == 0);
  if (fwrite (h->p, sizeof (KdtPoint), len, h->fp) != len) {
    perror ("heap_write");
    exit (1);
  }
  h->current = ftell (h->fp);
}

void kdt_heap_create (KdtHeap * h, FILE * fp, long start, long len, long buflen)
{
  h->fp = fp;
  h->start = start;
  if (len > 0 && len < buflen)
    buflen = len;
  h->len = len;
  h->buflen = buflen;
  h->i = 0;
  h->buf = buffer_new (buflen);
  h->p = ((Buffer *) h->buf)->p;
  h->current = start*sizeof (KdtPoint);
  if (fp != NULL) {
    assert (fseek (fp, start*sizeof (KdtPoint), SEEK_SET) == 0);
    assert (ftell (fp) == h->current);
    h->end = heap_read (h, buflen);
    if (buflen == len)
      assert (h->end == len);
  }
  else
    h->end = 0;
}

void kdt_heap_rewind (KdtHeap * h)
{
  if (h->len == h->buflen) {
    h->i = 0;
    assert (h->end == h->buflen);
  }
  else {
    assert (fseek (h->fp, h->start*sizeof (KdtPoint), SEEK_SET) == 0);
    h->current = ftell (h->fp);
    h->end = heap_read (h, h->buflen);
    h->i = 0;
  }
}

int kdt_heap_get (KdtHeap * h, KdtPoint * p)
{
  if (h->len == h->buflen && h->i >= h->len)
    return 0;
  if (h->i < h->end) {
    *p = h->p[h->i++];
    return 1;
  }
  if (h->end < h->buflen)
    return 0;
  h->end = heap_read (h, h->buflen);
  h->i = 0;
  return kdt_heap_get (h, p);
}

void kdt_heap_split (KdtHeap * h1, long len1, KdtHeap * h2)
{
  assert (len1 < h1->len);
  if (h1->len == h1->buflen) {
    h2->fp = NULL;
    h2->start = 0;
    h2->len = h1->len - len1;
    h2->buflen = h2->len;
    h2->i = 0;
    h2->p = &h1->p[len1];
    h2->buf = buffer_ref (h1->buf);
    h2->end = h2->len;
    kdt_heap_resize (h1, len1);
  }
  else {
    kdt_heap_create (h2, h1->fp, h1->start + len1, h1->len - len1, h1->buflen);

    KdtHeap h;
    kdt_heap_create (&h, NULL, 0, len1, h1->buflen);
    if (len1 > h1->buflen)
      h.fp = kdt_tmpfile ();
    else
      h.end = h.len;

    kdt_heap_rewind (h1);
    long i;
    for (i = 0; i < len1; i++) {
      KdtPoint p;
      assert (kdt_heap_get (h1, &p));
      kdt_heap_put (&h, &p);
    }
    kdt_heap_flush (&h);
    h1->fp = NULL;
    kdt_heap_free (h1);
    *h1 = h;
  }
}

void kdt_heap_put (KdtHeap * h, KdtPoint * p)
{
  if (h->i == h->buflen) {
    heap_write (h, h->buflen);
    h->i = 0;
  }
  h->p[h->i++] = *p;
}

void kdt_heap_flush (KdtHeap * h)
{
  if (h->i > 0 && h->fp != NULL)
    heap_write (h, h->i);
}

void kdt_heap_free (KdtHeap * h)
{
  buffer_unref (h->buf);
  if (h->fp != NULL)
    assert (fclose (h->fp) == 0);
}

/* sort */

static int put (KdtHeap * h, KdtPoint * p, KdtHeap * merged)
{
  kdt_heap_put (merged, p);
  return kdt_heap_get (h, p);
}

void kdt_write (KdtHeap * h, FILE * fp)
{
  kdt_heap_rewind (h);
  long i = 0;
  KdtPoint p;
  while (kdt_heap_get (h, &p)) {
    fprintf (fp, "%ld %g %g\n", i, p.x, p.y);
    i++;
  }
}

static void merge (KdtHeap * h1, KdtHeap * h2, 
		   int (*compar) (const void *, const void *),
		   long buflen)
{
  KdtHeap hm;
  assert (h1->len + h2->len > buflen);
  kdt_heap_create (&hm, NULL, h2->start - h1->len, h1->len + h2->len, buflen);
  hm.fp = h2->fp;
  KdtPoint p1, p2;
  kdt_heap_rewind (h1);
  int r1 = kdt_heap_get (h1, &p1);
  kdt_heap_rewind (h2);
  int r2 = kdt_heap_get (h2, &p2);
  while (r1 && r2) {
    if ((* compar) (&p2, &p1))
      r1 = put (h1, &p1, &hm);
    else
      r2 = put (h2, &p2, &hm);
  }
  while (r1)
    r1 = put (h1, &p1, &hm);
  while (r2)
    r2 = put (h2, &p2, &hm);
  kdt_heap_free (h1);
  h2->fp = NULL;
  kdt_heap_free (h2);
  kdt_heap_flush (&hm);
  *h1 = hm;
}

#if TIMING
static double elapsed (const struct timeval * start, const struct timeval * end)
{
  return (double) (end->tv_usec - start->tv_usec) + 
    1e6*(double) (end->tv_sec - start->tv_sec);
}
#endif

static void kdt_heap_sort (KdtHeap * h,
			   int  (*compar)   (const void *, const void *),
			   void (*progress) (void *), void * data)
{
#if TIMING
  struct timeval start;
  gettimeofday (&start, NULL);
#endif
  if (h->len == h->buflen) {
    qsort (h->p, h->len, sizeof (KdtPoint), compar);
    if (progress)
      (* progress) (data);
  }
  else {
    KdtHeap h2;
    long buflen = h->buflen;
    kdt_heap_split (h, h->len/2, &h2);
    kdt_heap_sort (h, compar, progress, data);
    kdt_heap_sort (&h2, compar, progress, data);
    merge (h, &h2, compar, buflen);
  }
#if TIMING
  struct timeval end;
  gettimeofday (&end, NULL);
  fprintf (stderr, "kdt_heap_sort %ld %g\n", len, elapsed (&start, &end));
#endif
}

/* number of qsort() calls for a given kdt_heap_sort() */
static int kdt_heap_sort_cost (long len, long buflen)
{
  int m = 1;
  while (len > buflen) {
    m *= 2;
    len /= 2;
  }
  return m;
}

/* Kdt */

typedef struct {
  KdtRect bound1, bound2;
  long len1;
  int n1;
} Node;

typedef struct {
  KdtRect bound;
  long len;
  PADDING_32_BITS
  long np;
  int version;
} Header;

struct _Kdt {
  Header h;
  FILE * nodes, * sums, * leaves;
  KdtPoint * buffer;
  /* progress stuff */
  void (* progress) (float complete, void * data);
  void * data;
  int i, m;
};

static int check_32_bits (const Kdt * kdt)
{
#if (!defined (__LP64__) && !defined (__64BIT__) && \
     !defined (_LP64) && !(__WORDSIZE == 64))
  long maxlen = (1 << 31)/sizeof (KdtPoint);
  if (kdt->h.len > maxlen) {
    fprintf (stderr, "kdt: 32-bits systems are limited to %ld data points\n", 
	     maxlen);
    return 1;
  }
#endif
  return 0;
}

static void sizes (const Node * n, long * nodes, long * sums, long * leaves)
{
  *nodes = n->n1*sizeof (Node);
  *sums  = n->n1*sizeof (KdtSumCore);
  *leaves = n->len1*sizeof (KdtPoint);
}

static void relative (const KdtRect rect, double * o, double * h)
{
  o[0] = (((double)rect[0].l) + ((double)rect[0].h))/2.;
  o[1] = (((double)rect[1].l) + ((double)rect[1].h))/2.;
  *h = ((double)rect[0].h) - ((double)rect[0].l);
  if (((double)rect[1].h) - ((double)rect[1].l) > *h)
    *h = ((double)rect[1].h) - ((double)rect[1].l);
}

static void sum_add_point (const KdtRect parent, KdtSumCore * sum, 
			   const KdtPoint * a, double w)
{
  double p[3], o[2], h;

  relative (parent, o, &h);

  p[0] = (a->x - o[0])/h; p[1] = (a->y - o[1])/h; p[2] = a->z;
#if AVG_TERRAIN
  sum->H0 += p[2];
  sum->n++;
  if (p[2] < sum->Hmin) sum->Hmin = p[2];
  if (p[2] > sum->Hmax) sum->Hmax = p[2];
#else
  sum->m01 += w*p[0];
  sum->m02 += w*p[1];
  sum->m03 += w*p[0]*p[1];
  sum->m11 += w*p[0]*p[0];
  sum->m13 += w*p[0]*p[0]*p[1];
  sum->m22 += w*p[1]*p[1];
  sum->m23 += w*p[0]*p[1]*p[1];
  sum->m33 += w*p[0]*p[0]*p[1]*p[1];
  sum->m44 += w*p[0]*p[0]*p[0];
  sum->m55 += w*p[1]*p[1]*p[1];
  sum->m66 += w*p[0]*p[0]*p[0]*p[0];
  sum->m77 += w*p[1]*p[1]*p[1]*p[1];
  sum->m67 += w*p[0]*p[0]*p[0]*p[1];
  sum->m76 += w*p[1]*p[1]*p[1]*p[0];
  sum->H0 += w*p[2];
  sum->H1 += w*p[0]*p[2];
  sum->H2 += w*p[1]*p[2];
  sum->H3 += w*p[0]*p[1]*p[2];
  sum->H4 += w*p[2]*p[2];
  sum->H5 += w*p[0]*p[0]*p[2];
  sum->H6 += w*p[1]*p[1]*p[2];
  sum->n ++;
  if (p[2] < sum->Hmin) sum->Hmin = p[2];
  if (p[2] > sum->Hmax) sum->Hmax = p[2];
#endif
}

static float area (const KdtRect rect)
{
  return (rect[0].h - rect[0].l)*(rect[1].h - rect[1].l);
}

static float length (const KdtRect rect)
{
  float w = rect[0].h - rect[0].l, h = rect[1].h - rect[1].l;
  return w > h ? w : h;
}

void kdt_rect_write (const KdtRect rect, FILE * fp)
{
  fprintf (fp, "%f %f\n%f %f\n%f %f\n%f %f\n%f %f\n\n",
	   rect[0].l, rect[1].l, rect[0].h, rect[1].l,
	   rect[0].h, rect[1].h, rect[0].l, rect[1].h,
	   rect[0].l, rect[1].l);	   
}

static int sort_x (const void * p1, const void * p2)
{
  return ((KdtPoint *) p1)->x > ((KdtPoint *) p2)->x ? 1 : -1;
}

static int sort_y (const void * p1, const void * p2)
{
  return ((KdtPoint *) p1)->y > ((KdtPoint *) p2)->y ? 1 : -1;
}

static void kdt_sum_core_init (KdtSumCore * s)
{
  memset (s, 0, sizeof (KdtSumCore));
  s->Hmax = - 1e30;
  s->Hmin =   1e30;
}

#define GAP 0.2
#define MINLEN 6

static int update_sum (const KdtRect rect, KdtSumCore * n, KdtHeap * h, 
		       int index)
{
  kdt_sum_core_init (n);
  long i, imax = 0;
  double min, x, smax = 0;
  KdtPoint p;
  kdt_heap_rewind (h);
  assert (kdt_heap_get (h, &p));
  sum_add_point (rect, n, &p, 1.);
  min = x = (&p.x)[index];
  for (i = 1; i < h->len; i++) {
    assert (kdt_heap_get (h, &p));
    sum_add_point (rect, n, &p, 1.);
    double s = (&p.x)[index] - x;
    if (s > smax && i > MINLEN && i < h->len - MINLEN) {
      smax = s;
      imax = i;
    }
    x = (&p.x)[index];
  }
  return smax/(x - min) > GAP ? imax : -1;
}

static long update_bounds (KdtRect rect, KdtHeap * h)
{
  long len = 0;
  rect[0].h = rect[1].h = -1e30;
  rect[0].l = rect[1].l =  1e30;
  kdt_heap_rewind (h);
  KdtPoint p;
  while (kdt_heap_get (h, &p)) {
    if (p.x > rect[0].h) rect[0].h = p.x;
    if (p.x < rect[0].l) rect[0].l = p.x;
    if (p.y > rect[1].h) rect[1].h = p.y;
    if (p.y < rect[1].l) rect[1].l = p.y;
    len++;
  }
  return len;
}

static void progress (void * data)
{
  Kdt * kdt = data;
  if (kdt->progress && kdt->m > 0)
    (* kdt->progress) (++kdt->i/(float) kdt->m, kdt->data);
}

static void fwrite_check (const void * ptr, size_t size, size_t nmemb, 
			  FILE * stream)
{
  if (fwrite (ptr, size, nmemb, stream) != nmemb) {
    perror ("kdt_write");
    exit (1);
  }
}

static void union_bound (KdtRect b, const KdtRect b1, const KdtRect b2)
{
  b[0].l = MIN (b1[0].l, b2[0].l);
  b[1].l = MIN (b1[1].l, b2[1].l);
  b[0].h = MAX (b1[0].h, b2[0].h);
  b[1].h = MAX (b1[1].h, b2[1].h);
}

static int split (KdtHeap * h1, KdtRect bound, int index, Kdt * kdt, 
		  float * coverage)
{
#if TIMING
  struct timeval start;
  gettimeofday (&start, NULL);
#endif
  int ns = 0;
  if (h1->len > kdt->h.np) {
    //    fprintf (stderr, " splitting: %ld      \r", len);
    int nindex = (bound[0].h - bound[0].l < bound[1].h - bound[1].l);
    if (index != nindex) {
      kdt_heap_sort (h1, nindex ? sort_y : sort_x, progress, kdt);
      index = nindex;
    }
    else
      /* update cost estimate */
      kdt->m -= kdt_heap_sort_cost (h1->len, h1->buflen);
    KdtSumCore s;
    int imax = update_sum (bound, &s, h1, index);
    long spos = ftell (kdt->sums);
    fwrite_check (&s, sizeof (KdtSumCore), 1, kdt->sums);
    Node n;
    n.len1 = imax > 0 ? imax : h1->len/2;
#if TIMING
    struct timeval s1;
    gettimeofday (&s1, NULL);
#endif
    KdtHeap h2;
    kdt_heap_split (h1, n.len1, &h2);
#if TIMING
    struct timeval end;
    gettimeofday (&end, NULL);
    fprintf (stderr, "half %ld %f\n", len, elapsed (&s1, &end));
#endif
    update_bounds (n.bound1, h1);
    update_bounds (n.bound2, &h2);
    long pos = ftell (kdt->nodes);
    fwrite_check (&n, sizeof (Node), 1, kdt->nodes);
    float coverage1;
    n.n1 = split (h1, n.bound1, index, kdt, &coverage1);
    float coverage2;
    int n2 = split (&h2, n.bound2, index, kdt, &coverage2);
    ns = n.n1 + n2 + 1;

    /* update bound */
    union_bound (bound, n.bound1, n.bound2);

    /* update sums */
    double a = area (bound);
    if (a > 0.)
      s.coverage = (coverage1*area (n.bound1) + coverage2*area (n.bound2))/a;
    else
      s.coverage = 1.;
    assert (fseek (kdt->sums, spos + ((long) &s.coverage - (long) &s), 
		   SEEK_SET) == 0);
    fwrite_check (&s.coverage, sizeof (float), 1, kdt->sums);
    assert (fseek (kdt->sums, 0, SEEK_END) == 0);
    *coverage = s.coverage;

    /* update node */
    assert (fseek (kdt->nodes, pos, SEEK_SET) == 0);
    fwrite_check (&n, sizeof (Node), 1, kdt->nodes);
    assert (fseek (kdt->nodes, 0, SEEK_END) == 0);
  }
  else {
    assert (h1->len > 0);
    /* half the average distance between samples */
    double delta = length (bound)/sqrt (h1->len)/2.;
    bound[0].l -= delta;
    bound[1].l -= delta;
    bound[0].h += delta;
    bound[1].h += delta;
#if DEBUG
    kdt_rect_write (bound, stderr);
#endif
    assert (h1->len <= h1->buflen);
    fwrite_check (h1->p, sizeof (KdtPoint), h1->len, kdt->leaves);
    kdt_heap_free (h1);
    *coverage = 1.;
  }
#if TIMING
  struct timeval end;
  gettimeofday (&end, NULL);
  fprintf (stderr, "splitfile %ld %f\n", len, elapsed (&start, &end));
#endif
  return ns;
}

Kdt * kdt_new (void)
{
  Kdt * kdt = calloc (1, sizeof (Kdt));
  return kdt;
}

static FILE * open_ext (const char * name, const char * ext, const char * mode)
{
  int len = strlen (name), len1 = strlen (ext);
  char * fname = malloc (sizeof(char)*(len + len1 + 1));
  strcpy (fname, name);
  strcpy (&fname[len], ext);
  FILE * fp = fopen (fname, mode);
  free (fname);
#if BUFFERED_KDT
  if (fp && strchr (mode, 'r')) {
    fseek (fp, 0, SEEK_END);
    long fsize = ftell (fp);
    fseek (fp, 0, SEEK_SET);    
    char * buf = malloc (fsize);
    size_t len = fread (buf, 1, fsize, fp);
    fclose (fp);
    fp = fmemopen (buf, len, mode);
  }
#endif /* BUFFERED_KDT */
  return fp;
}

static int kdt_init (Kdt * kdt, const char * name, int npmax, long len)
{
  kdt->nodes  = open_ext (name, ".kdt", "w");
  if (!kdt->nodes)
    return -1;
  
  kdt->sums   = open_ext (name, ".sum", "w");
  if (!kdt->sums)
    return -1;

  kdt->leaves = open_ext (name, ".pts", "w");
  if (!kdt->leaves)
    return -1;

  kdt->h.version = VERSION;
  kdt->h.len = len;
  kdt->h.np = npmax;
  kdt->h.bound[0].l = kdt->h.bound[1].l =  1e30;
  kdt->h.bound[0].h = kdt->h.bound[1].h = -1e30;

  if (check_32_bits (kdt))
    return -1;
  
  return 0;
}

int kdt_create (Kdt * kdt, const char * name, int blksize,
		KdtHeap * h,
		void (* progress) (float complete, void * data),
		void * data)
{
  KdtRect bound;
  long len = update_bounds (bound, h);
  kdt_heap_resize (h, len);

  int npmax = blksize/sizeof (KdtPoint);
  if (kdt_init (kdt, name, npmax, len))
    return -1;
  kdt->h.bound[0] = bound[0];
  kdt->h.bound[1] = bound[1];

  fwrite_check (&kdt->h, sizeof (Header), 1, kdt->nodes);

  /* cost estimate kdt->m (number of qsort() calls) based on balanced
     binary tree */
  kdt->m = kdt->i = 0;
  int m2 = 1;
  while (len > kdt->h.np) {
    kdt->m += kdt_heap_sort_cost (len, h->buflen)*m2;
    len /= 2;
    m2 *= 2;
  }
  kdt->progress = progress;
  kdt->data = data;
  float coverage;
  split (h, kdt->h.bound, -1, kdt, &coverage);
  /* write updated header (bounds have been updated) */
  rewind (kdt->nodes);
  fwrite_check (&kdt->h, sizeof (Header), 1, kdt->nodes);

  return 0;
}

int kdt_open (Kdt * kdt, const char * name)
{
  kdt->nodes  = open_ext (name, ".kdt", "r");
  if (!kdt->nodes)
    return -1;
  
  kdt->sums   = open_ext (name, ".sum", "r");
  if (!kdt->sums)
    return -1;

  kdt->leaves = open_ext (name, ".pts", "r");
  if (!kdt->leaves)
    return -1;

  if (fread (&kdt->h, sizeof (Header), 1, kdt->nodes) != 1)
    return -1;

  if (kdt->h.version != VERSION) {
    fprintf (stderr, 
	     "kdt: incompatible version number. Use:\n"
	     "%% kdt2kdt -v %s\n"
	     "to convert to the new format.\n", 
	     name);
    return -1;
  }

  kdt->buffer = malloc (sizeof (KdtPoint)*kdt->h.np);

  if (check_32_bits (kdt))
    return -1;

  return 0;
}

void kdt_destroy (Kdt * kdt)
{
  if (kdt->nodes)
    fclose (kdt->nodes);
  if (kdt->sums)
    fclose (kdt->sums);
  if (kdt->leaves)
    fclose (kdt->leaves);
  if (kdt->buffer)
    free (kdt->buffer);
  free (kdt);
}

int kdt_intersects (const KdtRect rect, const KdtRect query)
{
  return (rect[0].l <= query[0].h && rect[1].l <= query[1].h &&
	  rect[0].h >= query[0].l && rect[1].h >= query[1].l);
}

int kdt_includes (const KdtRect rect, const KdtRect query)
{
  return (rect[0].h <= query[0].h && rect[1].h <= query[1].h &&
	  rect[0].l >= query[0].l && rect[1].l >= query[1].l);
}

static long query (const Kdt * kdt, const KdtRect rect, long len)
{
  if (len > kdt->h.np) {
    Node node;
    if (fread (&node, sizeof (Node), 1, kdt->nodes) != 1)
      return -1;
    long pos = ftell (kdt->nodes), lpos = ftell (kdt->leaves);
    if (pos < 0 || lpos < 0)
      return -1;
    long n = 0;
    if (kdt_intersects (node.bound1, rect)) {
#if DEBUG
      kdt_rect_write (node.bound1, stderr);
#endif
      long n1 = query (kdt, rect, node.len1);
      if (n1 < 0)
	return -1;
      n += n1;
    }
    if (kdt_intersects (node.bound2, rect)) {
#if DEBUG
      kdt_rect_write (node.bound2, stderr);
#endif
      long snodes, ssums, sleaves;
      sizes (&node, &snodes, &ssums, &sleaves);
      if (fseek (kdt->nodes, pos + snodes, SEEK_SET))
	return -1;
      if (fseek (kdt->leaves, lpos + sleaves, SEEK_SET))
	return -1;
      long n1 = query (kdt, rect, len - node.len1);
      if (n1 < 0)
	return -1;
      n += n1;
    }
    return n;
  }
  else if (len > 0) {
    if (fread (kdt->buffer, sizeof (KdtPoint), len, kdt->leaves) != len)
      return -1;
    int i, n = 0;
    for (i = 0; i < len; i++)
      if (kdt->buffer[i].x >= rect[0].l && kdt->buffer[i].x <= rect[0].h && 
	  kdt->buffer[i].y >= rect[1].l && kdt->buffer[i].y <= rect[1].h) {
       	printf ("%.8f %.8f %f\n", 
		kdt->buffer[i].x, kdt->buffer[i].y, kdt->buffer[i].z);
	n++;
      }
    return n;
  }
  return 0;
}

long kdt_query (const Kdt * kdt, const KdtRect rect)
{
  rewind (kdt->nodes);
  rewind (kdt->leaves);
  Header h;
  if (fread (&h, sizeof (Header), 1, kdt->nodes) != 1)
    return -1;
  if (!kdt_intersects (rect, h.bound))
    return 0;
  return query (kdt, rect, h.len);
}

static void intersection (const KdtRect rect1, const KdtRect rect2, 
			  KdtRect inter)
{
  inter[0].l = MAX (rect1[0].l, rect2[0].l);
  inter[0].h = MIN (rect1[0].h, rect2[0].h);
  inter[1].l = MAX (rect1[1].l, rect2[1].l);
  inter[1].h = MIN (rect1[1].h, rect2[1].h);
  assert (inter[0].h >= inter[0].l && inter[1].h >= inter[1].l);  
}

static float intersection_area (const KdtRect rect1, const KdtRect rect2)
{
  KdtRect inter;
  intersection (rect1, rect2, inter);
  return area (inter);
}

static void sum_add_sum (const KdtRect parent, KdtSum * sum, 
			 const KdtRect rect, const KdtSumCore * a)
{
  /* weigh by effective area per sample */
  double w = intersection_area (rect, parent)*a->coverage/a->n;
  if (w == 0.)
    return;

  double op[2], oa[2], hp, ha;

  relative (parent, op, &hp);
  relative (rect, oa, &ha);

  double oap0 = oa[0] - op[0], oap1 = oa[1] - op[1];
  double an = a->n;
  double ha2 = ha*ha, hp2 = hp*hp;
  sum->m01 += w*(an*oap0 + a->m01*ha)/hp;
  sum->m02 += w*(an*oap1 + a->m02*ha)/hp;
  sum->m03 += w*(oap0*(an*oap1 + a->m02*ha) + ha*(a->m01*oap1 + a->m03*ha))/hp2;
  double m11 = (oap0*(an*oap0 + 2.*a->m01*ha) + a->m11*ha2)/hp2;
  sum->m11 += w*m11;
  double m13 = ha*(oap0*(a->m02*oap0 + 2.*a->m03*ha) + a->m13*ha2)/hp2;
  sum->m13 += w*(oap1*m11 + m13)/hp;
  double m22 = (oap1*(an*oap1 + 2.*a->m02*ha) + a->m22*ha2)/hp2;
  sum->m22 += w*m22;
  sum->m23 += w*(oap0*m22 + ha*(oap1*(oap1*a->m01 + 2.*a->m03*ha) + 
				a->m23*ha2)/hp2)/hp;
  sum->m33 += w*(oap1*(oap1*m11 + 2.*m13) + 
		 ha2*(oap0*(oap0*a->m22 + 2.*a->m23*ha) + ha2*a->m33)/hp2)/hp2;
  double ha3 = ha2*ha, hp3 = hp2*hp;
  sum->m44 += w*(oap0*(oap0*(oap0*an + 3.*ha*a->m01) + 3.*ha2*a->m11) + 
		 ha3*a->m44)/hp3;
  sum->m55 += w*(oap1*(oap1*(oap1*an + 3.*ha*a->m02) + 3.*ha2*a->m22) + 
		 ha3*a->m55)/hp3;
  double ha4 = ha3*ha, hp4 = hp3*hp;
  sum->m66 += w*(oap0*(oap0*(oap0*(oap0*an + 4.*ha*a->m01) + 6.*ha2*a->m11) 
		       + 4.*ha3*a->m44) + ha4*a->m66)/hp4;
  sum->m77 += w*(oap1*(oap1*(oap1*(oap1*an + 4.*ha*a->m02) + 6.*ha2*a->m22)
		       + 4.*ha3*a->m55) + ha4*a->m77)/hp4;
  sum->m67 += w*(oap1*(oap0*(oap0*(oap0*an + 3.*ha*a->m01) + 3.*ha2*a->m11) + 
		       ha3*a->m44)
		 + oap0*(oap0*(ha*a->m02*oap0 + 3.*ha2*a->m03) + 3.*ha3*a->m13) 
		 + ha4*a->m67)/hp4;
  sum->m76 += w*(oap0*(oap1*(oap1*(oap1*an + 3.*ha*a->m02) + 3.*ha2*a->m22) + 
		       ha3*a->m55)
		 + oap1*(oap1*(ha*a->m01*oap1 + 3.*ha2*a->m03) + 3.*ha3*a->m23)
		 + ha4*a->m76)/hp4;
  sum->H0 += w*a->H0;
  sum->H1 += w*(a->H0*oap0 + a->H1*ha)/hp;
  sum->H2 += w*(a->H0*oap1 + a->H2*ha)/hp;
  sum->H3 += w*(ha*(ha*a->H3 + oap0*a->H2 + oap1*a->H1) + oap0*oap1*a->H0)/hp2;
  sum->H4 += w*a->H4;
  sum->H5 += w*(oap0*(2.*ha*a->H1 + oap0*a->H0) + ha2*a->H5)/hp2;
  sum->H6 += w*(oap1*(2.*ha*a->H2 + oap1*a->H0) + ha2*a->H6)/hp2;
  sum->n += w*a->n*a->n/area (rect);
  float aparent = area (parent);
  if (aparent > 0.)
    sum->coverage += w*a->n/aparent;
  else
    sum->coverage = 1.;
  sum->w += w*a->n;
  if (a->Hmin < sum->Hmin) sum->Hmin = a->Hmin;
  if (a->Hmax > sum->Hmax) sum->Hmax = a->Hmax;
}

typedef struct {
  long np, sp, lp;
} FilePointers;

static long query_sum (const Kdt * kdt,
		       KdtCheck includes, KdtCheck intersects, void * data, 
		       KdtRect bound, long len,
		       FilePointers * f,
		       const KdtRect query, KdtSum * sum)
{
  if (len > kdt->h.np) {
    if (length (bound) <= length (query) || (* includes) (bound, data)) {
      KdtSumCore s;
      if (fseek (kdt->sums, f->sp, SEEK_SET))
	return -1;
      if (fread (&s, sizeof (KdtSumCore), 1, kdt->sums) != 1)
	return -1;
#if DEBUG
      fprintf (stderr, "read 1 sum %ld\n", sizeof (KdtSumCore));
#endif
      f->sp += sizeof (KdtSumCore);
      sum_add_sum (query, sum, bound, &s);
      return len;
    }
    f->sp += sizeof (KdtSumCore);

    Node node;
    if (fseek (kdt->nodes, f->np, SEEK_SET))
      return -1;
    if (fread (&node, sizeof (Node), 1, kdt->nodes) != 1)
      return -1;
    f->np += sizeof (Node);
#if DEBUG
    fprintf (stderr, "read 1 node %ld\n", sizeof (Node));
#endif

    long pos = f->np, lpos = f->lp, spos = f->sp;
    long n = 0;

    if ((* intersects) (node.bound1, data)) {
      long n1 = query_sum (kdt, includes, intersects, data, 
			   node.bound1, node.len1, f, query, sum);
      if (n1 < 0)
	return -1;
      n += n1;
    }

    if ((* intersects) (node.bound2, data)) {
      long snodes, ssums, sleaves;
      sizes (&node, &snodes, &ssums, &sleaves);
      f->np = pos + snodes;
      f->sp = spos + ssums;
      f->lp = lpos + sleaves;
      long n1 = query_sum (kdt, includes, intersects, data, 
			   node.bound2, len - node.len1, f, query, sum);
      if (n1 < 0)
	return -1;
      n += n1;
    }
    return n;
  }
  else {
    float h = length (bound)/sqrt (len); /* average distance between samples */
    if (h <= length (query)) {
#if DEBUG
      fprintf (stderr, "# area: %f %f\n", area (query), area (bound)/len);
      kdt_rect_write (bound, stderr);
#endif
      if (fseek (kdt->leaves, f->lp, SEEK_SET))
	return -1;
      if (fread (kdt->buffer, sizeof (KdtPoint), len, kdt->leaves) != len)
	return -1;
#if DEBUG
      fprintf (stderr, "read %ld leaves %ld\n", len, sizeof (KdtPoint));
#endif
      KdtPoint * a = kdt->buffer;
      int i, n = 0;
      for (i = 0; i < len; i++, a++) {
	KdtRect boundp;
	boundp[0].l = a->x - h/2.;
	boundp[0].h = a->x + h/2.;
	boundp[1].l = a->y - h/2.;
	boundp[1].h = a->y + h/2.;
	if ((* intersects) (boundp, data)) {
	  double w = intersection_area (boundp, query);
	  sum_add_point (query, (KdtSumCore *) sum, a, w);
	  sum->w += w;
	  sum->coverage += w/area (query);
	  n++;
	}
      }
      return n;
    }
  }
  return 0;
}

long kdt_query_sum (const Kdt * kdt, 
		    KdtCheck includes, KdtCheck intersects, void * data, 
		    const KdtRect query, KdtSum * sum)
{
  rewind (kdt->nodes);
  rewind (kdt->leaves);
  Header h;
  if (fread (&h, sizeof (Header), 1, kdt->nodes) != 1)
    return -1;
  FilePointers f;
  f.np = sizeof (Header);
  f.sp = f.lp = 0;
  if (!(* intersects) (h.bound, data))
    return 0;
  return query_sum (kdt, includes, intersects, data, h.bound, h.len, &f, 
		    query, sum);
}

void kdt_sum_init (KdtSum * s)
{
  kdt_sum_core_init ((KdtSumCore *) s);
  s->w = 0.;
}