#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <float.h>

#include "rtree.h"

static node_t RTreeNewNode (void);
static void RTreeDestroyNode (node_t);
static void RTreeNodeInit (node_t);

static int RTreeSearchNode (node_t, rect_t, SearchHitCallback, void *);
static int RTreeInsertNode (node_t, int, rect_t,void *,node_t *);

static int RTreePickBranch (rect_t, node_t);
static int RTreeAddBranch(node_t, branch_t, node_t *);
static void RTreeSplitNode (node_t, branch_t, node_t *);

static void RTreePickSeeds(partition_t *, node_t, node_t);
static void RTreeNodeAddBranch(rect_t *, node_t, branch_t);
static void RTreePickNext(partition_t *, node_t, node_t);

static rect_t RTreeNodeCover(node_t);

static double RectArea (rect_t);
static rect_t RectCombine (rect_t, rect_t);
static int RectOverlap (rect_t, rect_t);
static void RectPrint (rect_t);

static partition_t PartitionNew (void);
static void PartitionPush (partition_t *, branch_t);
static branch_t PartitionPop (partition_t *);
static branch_t PartitionGet (partition_t *, int);

rtree_t RTreeNew (void)
{
  rtree_t t;
  t = RTreeNewNode();
  t->level = 0; /*leaf*/
  return t;
}

void RTreeDestroy (rtree_t t)
{
  if (t)
    RTreeDestroyNode (t);
}

static node_t RTreeNewNode (void)
{
  node_t n;

  n = (node_t) malloc (sizeof(*n));
  assert(n);
  RTreeNodeInit(n);
  return n;
}

static void RTreeDestroyNode (node_t node)
{
  int i;
  
  if (node->level == 0) /* leaf level*/
    {
      for (i = 0; i < MAXCARD; i++)
        if (node->branch[i].child)
          ;/* allow user free data*/
        else
          break;
    }
  else
    {
      for (i = 0; i < MAXCARD; i++)
        if (node->branch[i].child)
          RTreeDestroyNode (node->branch[i].child);
        else
          break;
    }
  free (node);
}

static void RTreeNodeInit (node_t n)
{
  memset((void *) n,0, sizeof(*n));
  n->level = -1;
}

int RTreeSearch (rtree_t t, rect_t s, SearchHitCallback f, void *arg)
{
  assert(t);
  return RTreeSearchNode(t,s,f,arg);
}

static int RTreeSearchNode (node_t n, rect_t s, SearchHitCallback f, void *arg)
{
  int i;
  int c = 0;

  if (n->level > 0)
    {
      for (i = 0; i < MAXCARD; i++)
        if (n->branch[i].child &&
            RectOverlap (s,n->branch[i].mbr))
          c += RTreeSearchNode ((node_t) n->branch[i].child, s, f, arg);
    }
  else
    {
      for (i = 0; i < MAXCARD; i++)
        if (n->branch[i].child &&
            RectOverlap (s,n->branch[i].mbr))
          {
            c ++;
            if (f)
              if ( !f(n->branch[i].mbr,n->branch[i].child,arg))
                return c;
          }
    }
  return c;
}

void RTreeInsert (rtree_t *t, rect_t r, void *data)
{
  node_t n2;
  node_t new_root;
  branch_t b;
  assert(t && *t);

  if (RTreeInsertNode(*t, 0, r, data, &n2))
    /* deal with root split */
    {
      new_root = RTreeNewNode();
      new_root->level = (*t)->level + 1;
      b.mbr = RTreeNodeCover(*t);
      b.child = (void *) *t;
      RTreeAddBranch(new_root, b, NULL);
      b.mbr = RTreeNodeCover(n2);
      b.child = (void *) n2;
      RTreeAddBranch(new_root, b, NULL);
      *t = new_root;
    }
}

static int RTreeInsertNode (node_t n, int level,
                            rect_t r, void *data,
                            node_t *new_node)
{
  int i;
  node_t n2;
  branch_t b;

  assert(n && new_node);
  assert(level >= 0 && level <= n->level);
  
  if (n->level > level)
    {
      i = RTreePickBranch(r,n);
      if (!RTreeInsertNode((node_t) n->branch[i].child, level,
                           r, data,&n2)) /* not split */
        {
          n->branch[i].mbr = RectCombine(r,n->branch[i].mbr);
          return FALSE;
        }
      else /* node split */
        {
           n->branch[i].mbr = RTreeNodeCover(n->branch[i].child);
           b.child = n2;
           b.mbr = RTreeNodeCover(n2);
           return RTreeAddBranch(n, b, new_node);
        }
    }
  else /*insert level*/
    {
      b.mbr = r;
      b.child = data;
      return RTreeAddBranch(n, b, new_node);
    }
}

static int RTreeAddBranch(node_t n, branch_t b, node_t *new_node)
{
  int i;

  assert(n);

  if (n->count < MAXCARD) /*split not necessary*/
    {
      for (i = 0; i < MAXCARD; i++)
        if (n->branch[i].child == NULL)
          {
            n->branch[i] = b;
            n->count ++;
            break;
          }
      return FALSE;
    }
  else /*needs to split*/
    {
      assert(new_node);
      RTreeSplitNode (n, b, new_node);
      return TRUE;
    }
}

static int RTreePickBranch (rect_t r, node_t n)
{
  int i;
  double area;
  double inc_area;
  rect_t tmp;
  int best_i;
  double best_inc;
  double best_i_area;

  best_i = 0;
  best_inc = DBL_MAX; /* double Max value */
  best_i_area = DBL_MAX;

  for (i = 0; i < MAXCARD; i++)
    if (n->branch[i].child)
      {
        area = RectArea (n->branch[i].mbr);
        tmp = RectCombine (r, n->branch[i].mbr);
        inc_area = RectArea (tmp) - area; 

        if (inc_area < best_inc)
          {
            best_inc = inc_area;
            best_i = i;
            best_i_area = area;
          }
        else if (inc_area == best_inc && best_i_area > area)
          {
            best_inc = inc_area;
            best_i = i;
            best_i_area = area;
          }
      }
    else
      break;
  return best_i;
}

static void RTreeSplitNode (node_t n, branch_t b, node_t *new_node)
{
  partition_t p;
  int level;
  int i;

  assert(n);
  assert(new_node);

  p = PartitionNew();

  for (i = 0; i < MAXCARD; i ++)
    PartitionPush(&p,n->branch[i]);
  PartitionPush(&p,b);

  level = n->level;
  RTreeNodeInit(n);
  n->level = level;
  *new_node = RTreeNewNode();
  (*new_node)->level = level;

  RTreePickSeeds(&p, n, *new_node);

  while (p.n)
    if (n->count + p.n <= MINCARD)
      /* first group (n) needs all entries */
      RTreeNodeAddBranch(&(p.cover[0]), n, PartitionPop(&p));
    else if ((*new_node)->count + p.n <= MINCARD)
      /* second group (new_node) needs all entries */
      RTreeNodeAddBranch(&(p.cover[1]), *new_node, PartitionPop(&p));
    else
      RTreePickNext(&p, n, *new_node);
}

static void RTreePickNext(partition_t *p, node_t n1, node_t n2)
/* linear version */
{
  branch_t b;
  double area[2], inc_area[2];
  rect_t tmp;

  b = PartitionPop(p);

  area[0] = RectArea (p->cover[0]);
  tmp = RectCombine (p->cover[0], b.mbr);
  inc_area[0] = RectArea (tmp) - area[0];

  area[1] = RectArea (p->cover[1]);
  tmp = RectCombine (p->cover[1], b.mbr);
  inc_area[1] = RectArea (tmp) - area[1]; 

  if (inc_area[0] < inc_area[1] ||
      (inc_area[0] == inc_area[1] && area[0] < area[1]))
    RTreeNodeAddBranch(&(p->cover[0]),n1,b);
  else
    RTreeNodeAddBranch(&(p->cover[1]),n2,b);
}

static void RTreePickSeeds(partition_t *p, node_t n1, node_t n2)
/* puts in index 0 of each node the resulting entry, forming the two
   groups
   This is the linear version
*/
{
  int dim,high, i;
  int highestLow[NUMDIMS], lowestHigh[NUMDIMS];
  double width[NUMDIMS];
  int seed0, seed1;
  double sep, best_sep;

  assert(p->n == MAXCARD + 1);

  for (dim = 0; dim < NUMDIMS; dim++)
    {
      high = dim + NUMDIMS;
      highestLow[dim] = lowestHigh[dim] = 0;
      for (i = 1; i < MAXCARD +1; i++)
        {
          if (p->buffer[i].mbr.coords[dim] >
              p->buffer[highestLow[dim]].mbr.coords[dim])
            highestLow[dim] = i;
          if (p->buffer[i].mbr.coords[high] < 
              p->buffer[lowestHigh[dim]].mbr.coords[high])
            lowestHigh[dim] = i;
        }
      width[dim] = p->cover_all.coords[high] - p->cover_all.coords[dim];
      assert(width[dim] >= 0);
    }

  seed0 = lowestHigh[0];
  seed1 = highestLow[0];
  best_sep = 0;
  for (dim = 0; dim < NUMDIMS; dim ++)
    {
      high = dim + NUMDIMS;
      
      sep = (p->buffer[highestLow[dim]].mbr.coords[dim] -
             p->buffer[lowestHigh[dim]].mbr.coords[high]) / width[dim];
      if (sep > best_sep)
        {
          seed0 = lowestHigh[dim];
          seed1 = highestLow[dim];
          best_sep = sep;
        }
    }
/*   assert (seed0 != seed1); */
  if (seed0 > seed1)
    {
      RTreeNodeAddBranch(&(p->cover[0]),n1,PartitionGet(p,seed0));
      RTreeNodeAddBranch(&(p->cover[1]),n2,PartitionGet(p,seed1));
    }
  else if (seed0 < seed1)
    {
      RTreeNodeAddBranch(&(p->cover[0]),n1,PartitionGet(p,seed1));
      RTreeNodeAddBranch(&(p->cover[1]),n2,PartitionGet(p,seed0));
    }
}

static void RTreeNodeAddBranch(rect_t *r, node_t n, branch_t b)
{
  int i;

  assert(n);
  assert(n->count < MAXCARD);

  for (i = 0; i < MAXCARD; i++)
    if (n->branch[i].child == NULL)
      {
        n->branch[i] = b;
        n->count ++;
        break;
      }
  *r = RectCombine(*r,b.mbr);
}


void RTreePrint(node_t t)
{
  int i;

  /*  printf("rtree([_,_,_,_,_]).\n"); */
  printf("rtree(%p,%d,[",t,t->level);
  for (i = 0; i < MAXCARD; i++)
    {
      if (t->branch[i].child != NULL)
        {
          printf("(%p,",t->branch[i].child);
                   RectPrint(t->branch[i].mbr);
          printf(")");
        }
      else
        {
          printf("nil");
        }
      if (i < MAXCARD-1)
        printf(",");
    }
  printf("]).\n");

  if (t->level != 0)
    for (i = 0; i < MAXCARD; i++)
      if (t->branch[i].child != NULL)
        RTreePrint((node_t) t->branch[i].child);
      else
        break;
}

/*
 * Partition related
 */

static partition_t PartitionNew (void)
{
  partition_t p;
  memset((void *) &p,0, sizeof(p));
  p.cover[0] = p.cover[1] = p.cover_all = RectInit();
  return p;
}

static void PartitionPush (partition_t *p, branch_t b)
{
  assert(p->n < MAXCARD + 1);
  p->buffer[p->n] = b;
  p->n ++;
  p->cover_all = RectCombine(p->cover_all,b.mbr);
}

static branch_t PartitionPop (partition_t *p)
{
  assert(p->n > 0);
  p->n --;
  return p->buffer[p->n];
}

static branch_t PartitionGet (partition_t *p, int n)
{
  branch_t b;
  assert (p->n > n);
  b = p->buffer[n];
  p->buffer[n] = PartitionPop(p);
  return b;
}

/*
 * Rect related
 */

rect_t RectInit (void)
{
  rect_t r = {{DBL_MAX, DBL_MAX, DBL_MIN, DBL_MIN}};
  return (r);
}

static double RectArea (rect_t r)
{
  int i;
  double area;

  for (i = 0,area = 1; i < NUMDIMS; i++)
    area *= r.coords[i+NUMDIMS] - r.coords[i];

/*   area = (r.coords[1] - r.coords[0]) *  */
/*     (r.coords[3] - r.coords[2]); */

  return area;
}

static rect_t RectCombine (rect_t r, rect_t s)
{
  int i;
  rect_t new_rect;

  for (i = 0; i < NUMDIMS; i++)
    {
      new_rect.coords[i] = MIN(r.coords[i],s.coords[i]);
      new_rect.coords[i+NUMDIMS] = MAX(r.coords[i+NUMDIMS],s.coords[i+NUMDIMS]);
    }
  
  return new_rect;
}

static int RectOverlap (rect_t r, rect_t s)
{
  int i;
  
  for (i = 0; i < NUMDIMS; i++)
    if (r.coords[i] > s.coords[i + NUMDIMS] ||
        s.coords[i] > r.coords[i + NUMDIMS])
      return FALSE;
  return TRUE;
}

static rect_t RTreeNodeCover(node_t n)
{
  int i;
  rect_t r = RectInit();

  for (i = 0; i < MAXCARD; i++)
    if (n->branch[i].child)
      {
        r = RectCombine (r, n->branch[i].mbr);
      }
    else
      break;

  return r;
}

static void RectPrint (rect_t r)
{
  int i;

  printf("[");
  for (i = 0; i < 2*NUMDIMS; i++)
    {
      printf("%f",r.coords[i]);
      if ( i < 2*NUMDIMS - 1)
        printf(",");
    }
  printf("]");
}