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

#include "rtree.h"

#define  METHODS  1

typedef struct _RTREEPARTITION
{
 int   partition[MAXCARD+1];
 int   total;
 int   minfill;
 int   taken[MAXCARD+1];
 int   count[2];
 RTREEMBR cover[2];
 REALTYPE area[2];
} RTREEPARTITION;

typedef struct _RTREENODE
{
 int count;
 int level;
 RTREEBRANCH  branch[MAXCARD];
}RTREENODE;

typedef struct _RTREELISTNODE
{
     struct _RTREELISTNODE *next;
     RTREENODE  *node;
}RTREELISTNODE;

typedef struct _RTREEROOT
{
 RTREENODE*  root_node;
 
 RTREEBRANCH  BranchBuf[MAXCARD+1];
 int    BranchCount;
 RTREEMBR  CoverSplit;
 REALTYPE  CoverSplitArea;
 RTREEPARTITION Partitions[METHODS];

 PFNRTreeSearchCallback  pfnCallback;
} RTREEROOT;

#define BIG_NUM (FLT_MAX/4.0)

#define INVALID_RECT(x) ((x)->bound[0] > (x)->bound[DIMS_NUMB])

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

int NODECARD = MAXCARD;
int LEAFCARD = MAXCARD;

#define MINNODEFILL (NODECARD / 2)
#define MINLEAFFILL (LEAFCARD / 2)

#define MAXKIDS(n) ((n)->level > 0 ? NODECARD : LEAFCARD)
#define MINFILL(n) ((n)->level > 0 ? MINNODEFILL : MINLEAFFILL)

static int set_max(int *which, int new_max){
 if(2 > new_max || new_max > MAXCARD)
  return 0;
 *which = new_max;
 return 1;
}
static int RTreeSetNodeMax(int new_max) { return set_max(&NODECARD, new_max); }
static int RTreeSetLeafMax(int new_max) { return set_max(&LEAFCARD, new_max); }
static int RTreeGetNodeMax(void) { return NODECARD; }
static int RTreeGetLeafMax(void) { return LEAFCARD; }

static void _RTreeGetBranches(HRTREEROOT root, RTREENODE *node, RTREEBRANCH *br)
{
 int i;

 assert(node && br);
 
 for (i=0; i<MAXKIDS(node); i++)
 {
  assert(node->branch[i].child);
  root->BranchBuf[i] = node->branch[i];
 }

 root->BranchBuf[MAXKIDS(node)] = *br;
 root->BranchCount = MAXKIDS(node) + 1;

 root->CoverSplit = root->BranchBuf[0].mbr;

 for (i=1; i<MAXKIDS(node)+1; i++)
 {
  root->CoverSplit = RTreeCombineRect(&root->CoverSplit, &root->BranchBuf[i].mbr);
 }

 root->CoverSplitArea = RTreeRectSphericalVolume(&root->CoverSplit);
 RTreeInitNode(node);
}

static void _RTreeClassify(HRTREEROOT root, int i, int group, RTREEPARTITION *p)
{
 assert(p);
 assert(!p->taken[i]);

 p->partition[i] = group;
 p->taken[i] = TRUE;

 if (p->count[group] == 0)
  p->cover[group] = root->BranchBuf[i].mbr;
 else
  p->cover[group] = RTreeCombineRect(&root->BranchBuf[i].mbr, &p->cover[group]);
 
 p->area[group] = RTreeRectSphericalVolume(&p->cover[group]);
 p->count[group]++;
}

static void _RTreePickSeeds(HRTREEROOT root, RTREEPARTITION *p)
{
 int i, j, seed0=0, seed1=0;
 REALTYPE worst, waste, area[MAXCARD+1];

 for (i=0; i<p->total; i++)
  area[i] = RTreeRectSphericalVolume(&root->BranchBuf[i].mbr);
 
 worst = -root->CoverSplitArea - 1;
 
 for (i=0; i<p->total-1; i++)
 {
  for (j=i+1; j<p->total; j++)
  {
   RTREEMBR one_rect;
   one_rect = RTreeCombineRect(&root->BranchBuf[i].mbr, &root->BranchBuf[j].mbr);
   waste = RTreeRectSphericalVolume(&one_rect) - area[i] - area[j];
   if (waste > worst)
   {
    worst = waste;
    seed0 = i;
    seed1 = j;
   }
  }
 }
 _RTreeClassify(root, seed0, 0, p);
 _RTreeClassify(root, seed1, 1, p);
}
static void _RTreeLoadNodes(HRTREEROOT root, RTREENODE *n, RTREENODE *q, RTREEPARTITION *p)
{
 int i;
 assert(n && q && p);

 for (i=0; i<p->total; i++)
 {
  assert(p->partition[i] == 0 || p->partition[i] == 1);
  if (p->partition[i] == 0)
   RTreeAddBranch(root, &root->BranchBuf[i], n, NULL);
  else if (p->partition[i] == 1)
   RTreeAddBranch(root, &root->BranchBuf[i], q, NULL);
 }
}

static void _RTreeInitPart( RTREEPARTITION *p, int maxrects, int minfill)
{
 int i;
 assert(p);

 p->count[0] = p->count[1] = 0;
 p->cover[0] = p->cover[1] = RTreeNullRect();
 p->area[0] = p->area[1] = (REALTYPE)0;
 p->total = maxrects;
 p->minfill = minfill;
 for (i=0; i<maxrects; i++)
 {
  p->taken[i] = FALSE;
  p->partition[i] = -1;
 }
}

static void _RTreePrintPart(HRTREEROOT root, RTREEPARTITION *p)
{
 int i;
 assert(p);
 
 fprintf (stdout, "\npartition:\n");
 for (i=0; i<p->total; i++)
 {
  fprintf (stdout, "%3d\t", i);
 }
 fprintf (stdout, "\n");
 for (i=0; i<p->total; i++)
 {
  if (p->taken[i])
   fprintf (stdout, "  t\t");
  else
   fprintf (stdout, "\t");
 }
 fprintf (stdout, "\n");
 for (i=0; i<p->total; i++)
 {
  fprintf (stdout, "%3d\t", p->partition[i]);
 }
 fprintf (stdout, "\n");

 fprintf (stdout, "count[0] = %d  area = %f\n", p->count[0], p->area[0]);
 fprintf (stdout, "count[1] = %d  area = %f\n", p->count[1], p->area[1]);
 if (p->area[0] + p->area[1] > 0)
 {
  fprintf (stdout, "total area = %f  effectiveness = %3.2f\n", 
   p->area[0] + p->area[1], (float)root->CoverSplitArea / (p->area[0] + p->area[1]));
 }
 fprintf (stdout, "cover[0]:\n");
 RTreePrintRect(&p->cover[0], 0);

 fprintf (stdout, "cover[1]:\n");
 RTreePrintRect(&p->cover[1], 0);
}
static void _RTreeMethodZero(HRTREEROOT root, RTREEPARTITION *p, int minfill )
{
 int i;
 REALTYPE biggestDiff;
 int group, chosen=0, betterGroup=0;
 assert(p);

 _RTreeInitPart(p, root->BranchCount, minfill);
 _RTreePickSeeds(root, p);

 while (p->count[0] + p->count[1] < p->total && 
  p->count[0] < p->total - p->minfill && 
  p->count[1] < p->total - p->minfill)
 {
  biggestDiff = (REALTYPE)-1.;
  for (i=0; i<p->total; i++)
  {
   if (!p->taken[i])
   {
    RTREEMBR *r, rect_0, rect_1;
    REALTYPE growth0, growth1, diff;
    r = &root->BranchBuf[i].mbr;
    rect_0 = RTreeCombineRect(r, &p->cover[0]);
    rect_1 = RTreeCombineRect(r, &p->cover[1]);
    growth0 = RTreeRectSphericalVolume(&rect_0) - p->area[0]; 
    growth1 = RTreeRectSphericalVolume(&rect_1) - p->area[1];
    diff = growth1 - growth0;
    if (diff >= 0)
     group = 0;
    else
    {
     group = 1;
     diff = -diff;
    }
    if (diff > biggestDiff)
    {
     biggestDiff = diff;
     chosen = i;
     betterGroup = group;
    }
    else if (diff==biggestDiff && p->count[group]<p->count[betterGroup])
    {
     chosen = i;
     betterGroup = group;
    }
   }
  }
  _RTreeClassify(root, chosen, betterGroup, p);
 }
 if (p->count[0] + p->count[1] < p->total)
 {
  if (p->count[0] >= p->total - p->minfill)
   group = 1;
  else
   group = 0;
  
  for (i=0; i<p->total; i++)
  {
   if (!p->taken[i])
    _RTreeClassify(root, i, group, p);
  }
 }
 assert(p->count[0] + p->count[1] == p->total);
 assert(p->count[0] >= p->minfill && p->count[1] >= p->minfill);
}

static void _RTreeInitBranch( RTREEBRANCH *br )
{
 RTreeInitRect(&(br->mbr));
 br->child = NULL;
}

static void _RTreePrintBranch( RTREEBRANCH *br, int depth )
{
 RTreePrintRect(&(br->mbr), depth);
 RTreePrintNode(br->child, depth);
}

static int _RTreeInsertRect(HRTREEROOT root, RTREEMBR *mbr, void* tid,  RTREENODE *node, RTREENODE **new_node, int level)
{
 int i;
 RTREEBRANCH b;
 RTREENODE *n2;

 assert(mbr && node && new_node);
 assert(level >= 0 && level <= node->level);

 if (node->level > level)
 {
  i = RTreePickBranch(mbr, node);
  if (!_RTreeInsertRect(root, mbr, tid, node->branch[i].child, &n2, level))
  {
   node->branch[i].mbr = RTreeCombineRect(mbr, &(node->branch[i].mbr));
   return 0;
  }
  
  node->branch[i].mbr = RTreeNodeCover(node->branch[i].child);
  b.child = n2;
  b.mbr = RTreeNodeCover(n2);

  return RTreeAddBranch(root, &b, node, new_node);
 } 
 else if (node->level == level) 
 {
  b.mbr = *mbr;

#pragma warning(push) 
#pragma warning( disable : 4312 )
  b.child = ( RTREENODE *) tid;
#pragma warning(pop)

  return RTreeAddBranch(root, &b, node, new_node);
 }
 assert (FALSE);
 return 0;
}

static RTREELISTNODE * _RTreeNewListNode(void)
{
 return (RTREELISTNODE *) malloc(sizeof(RTREELISTNODE));
}

static void _RTreeFreeListNode(RTREELISTNODE *p)
{
 free(p);
}

static void _RTreeReInsert(RTREENODE *node, RTREELISTNODE **ne)
{
 RTREELISTNODE *ln = _RTreeNewListNode();
 ln->node = node;
 ln->next = *ne;
 *ne = ln;
}

static int _RTreeDeleteRect( RTREEMBR *mbr, void* tid, RTREENODE *node, RTREELISTNODE **ee)
{
 int i;

 assert(mbr && node && ee);
 assert(node->level >= 0);

 if (node->level > 0)  
 {
  for (i = 0; i < NODECARD; i++)
  {
   if (node->branch[i].child && RTreeOverlap( mbr, &(node->branch[i].mbr )))
   {
    if (!_RTreeDeleteRect( mbr, tid, node->branch[i].child, ee ))
    {
     if (node->branch[i].child->count >= MINNODEFILL) 
      node->branch[i].mbr = RTreeNodeCover( node->branch[i].child );
     else{ 
      _RTreeReInsert(node->branch[i].child, ee);
      RTreeDisconnectBranch(node, i);
     }
     return 0;
    }
   }
  }
  return 1;
 }

#pragma warning(push)
#pragma warning( disable : 4312 )

 for (i = 0; i < LEAFCARD; i++)
 {
  if ( node->branch[i].child && node->branch[i].child == (RTREENODE *) tid )
  {
   RTreeDisconnectBranch( node, i );
   return 0;
  }

 }
#pragma warning(pop)

 return 1;
}

static void _RTreeTabIn(int depth)
{
 int i;
 for(i=0; i<depth; i++){
 }
}


static int _RTreeSearchRect( RTREENODE *node, RTREEMBR *mbr, PFNRTreeSearchCallback pfnSHCB, void* pfnParam)
{
 int hitCount = 0;
 int i;

 assert(node && mbr);
 assert(node->level >= 0);
 
 if (node->level > 0)
 {
  for (i=0; i<NODECARD; i++){
   if (node->branch[i].child && RTreeOverlap(mbr, &node->branch[i].mbr))
    hitCount += _RTreeSearchRect(node->branch[i].child, mbr, pfnSHCB, pfnParam);
  }
 }