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

#include "treematch.h"
#include "file.h"

#define TREE_MAX_STEP 15

void dematch_Leaf(int ncat, Leaf *cat){
   int i;
   Leaf p;
   for(i=0;i<ncat;i++){
      if((p=cat[i])==NULL) continue;
      p->nmatch=0;
      free(cat[i]->match);
      p->match=NULL;
      p->adopted=NULL;
   }
}

Leaf create_Leaf(double x, double y, double mag, void *data){
   Leaf p;
   p=malloc(sizeof(struct leaf));
   p->x=x; p->y=y; p->mag=mag;
   p->nmatch=0; p->match=NULL; p->up=NULL;
   p->data=data;
   p->adopted=NULL;
   return p;
}

Node create_Node(double x, double y, double size){
   int i;
   Node p;
   p=malloc(sizeof(struct node));
   p->step=0; p->x=x; p->y=y; p->size=size;
   for(i=0;i<FOUR;i++) p->next[i]=NULL;
   p->obj=NULL;
   return p;
}

void destroy_Node(Node p){
   int i;
   if(p->next[0]!=NULL) for(i=0;i<FOUR;i++) destroy_Node(p->next[i]);
   free(p); p=NULL;
}

int which_node(Node p, Leaf q){
   int pos;
   if(p->x>q->x){
      if(p->y>q->y) pos=0;
      else pos=1;
   } else {
      if(p->y>q->y) pos=2;
      else pos=3;
   }
   return pos;
}

void make_next_tree(Node tree){
   int sign_x[FOUR]={ -1, -1,  1,  1 };
   int sign_y[FOUR]={ -1,  1, -1,  1 };
   int i, step1;
   double xc, yc, size, x, y;;
   step1=tree->step+1;
   size=tree->size/2.0;
   xc=tree->x; yc=tree->y;
   for(i=0;i<FOUR;i++){
      x=xc+sign_x[i]*size;
      y=yc+sign_y[i]*size;
      tree->next[i]=create_Node(x,y,size);
      tree->next[i]->step=step1;
   }
}

void tree_match_pre(Node tree, Leaf obj, double del, double del2){
   int i;
   double lim, dx, dy;//, r2;
   Node p;
   Leaf q;
   if(tree==NULL||obj==NULL) return;
   if(tree->next[0]!=NULL){
      for(i=0;i<FOUR;i++){
         p=tree->next[i];
         lim=p->size+del;
         if(fabs(p->x-obj->x)>lim||fabs(p->y-obj->y)>lim) continue;
         tree_match(p,obj,del,del2);
      }
   } else {
      if(tree->obj!=NULL){
         q=tree->obj;
         if((dx=fabs(q->x-obj->x))>del||(dy=fabs(q->y-obj->y))>del) return;
         //r2=dx*dx+dy*dy;
         //if(r2>del2) return;
         add_obj_to_match(q,obj);
         add_obj_to_match(obj,q);
      }
   }
}

void tree_match(Node tree, Leaf obj, double del, double del2){
   int i;
   double lim, r2, dx, dy;
   Node p;
   Leaf q;
   if(tree==NULL||obj==NULL) return;
   if(tree->next[0]!=NULL){
      for(i=0;i<FOUR;i++){
         p=tree->next[i];
         lim=p->size+del;
         if(fabs(p->x-obj->x)>lim||fabs(p->y-obj->y)>lim) continue;
         tree_match(p,obj,del,del2);
      }
   } else {
      if(tree->obj!=NULL){
         q=tree->obj;
         if((dx=fabs(q->x-obj->x))>del||(dy=fabs(q->y-obj->y))>del) return;
         r2=dx*dx+dy*dy;
         if(r2>del2) return;
         add_obj_to_match(q,obj);
         add_obj_to_match(obj,q);
      }
   }
}

void add_obj_to_match(Leaf obj, Leaf target){
   int i;
   Leaf *new;
   new=malloc((target->nmatch+1)*sizeof(Leaf));
   for(i=0;i<target->nmatch;i++) new[i]=target->match[i];
   new[target->nmatch]=obj;
   target->nmatch+=1;
   free(target->match);
   target->match=new;
}

void add_obj_to_tree(Node tree, Leaf obj){
   int next_node;
   if(fabs(tree->x-obj->x)>tree->size||fabs(tree->x-obj->x)>tree->size) return;
   if(tree->next[0]!=NULL){
      next_node=which_node(tree,obj);
      add_obj_to_tree(tree->next[next_node],obj);
   } else {
      if(tree->obj!=NULL){
         if(tree->step==TREE_MAX_STEP){
            if(tree->obj->mag>obj->mag){
               tree->obj->up=NULL; obj->up=tree; tree->obj=obj;
            }
         } else {
            make_next_tree(tree);
            next_node=which_node(tree,tree->obj);
            add_obj_to_tree(tree->next[next_node],tree->obj);
            tree->obj=NULL;
            next_node=which_node(tree,obj);
            add_obj_to_tree(tree->next[next_node],obj);
         }
      } else {
         tree->obj=obj; obj->up=tree;
      }
   }
}

void trans_Leaf_xy(int ncat, Leaf *cat, double *coef){
   int i;
   Leaf p;
   double x, y;
   for(i=0;i<ncat;i++){
      p=cat[i];
      x=coef[0]+coef[1]*p->x+coef[2]*p->y;
      y=coef[3]+coef[4]*p->x+coef[5]*p->y;
      p->x=x; p->y=y;
   }
}

void make_treematch(int nobj, Leaf *obj, int nref, Leaf *ref, double x, double y, double size, double tol){
   Node tree=NULL;
   int i, nmatch;
   double tol2;
   Leaf match;
   dematch_Leaf(nobj,obj); dematch_Leaf(nref,ref);
   tree=create_Node(x,y,size);
   tol2=tol*tol;
   for(i=0;i<nref;i++) add_obj_to_tree(tree,ref[i]);
   for(i=0,nmatch=0;i<nobj;i++){
      tree_match(tree,obj[i],tol,tol2);
      if((match=get_brightest_unadopted(obj[i]))==NULL) continue;
      obj[i]->adopted=match;
      match->adopted=obj[i];
      ++nmatch;
   }
   destroy_Node(tree);
}

void make_treematch_pre(int nobj, Leaf *obj, int nref, Leaf *ref, double x, double y, double size, double tol){
   Node tree=NULL;
   int i;
   double tol2;
   Leaf match;
   dematch_Leaf(nobj,obj); dematch_Leaf(nref,ref);
   tree=create_Node(x,y,size);
   tol2=tol*tol;
   for(i=0;i<nref;i++) add_obj_to_tree(tree,ref[i]);
   for(i=0;i<nobj;i++){
      tree_match_pre(tree,obj[i],tol,tol2);
      if((match=get_brightest_unadopted(obj[i]))==NULL) continue;
      obj[i]->adopted=match;
      match->adopted=obj[i];
   }
   destroy_Node(tree);
}

int count_adopted(int ncat, Leaf *cat){
   int num, i;
   for(i=0,num=0;i<ncat;i++) if(cat[i]->adopted!=NULL) ++num;
   return num;
}

Leaf get_brightest_unadopted(Leaf p){
   Leaf tmp, q;
   int i, nmatch;
   double mag, bright;
   nmatch=p->nmatch;
   if(nmatch<=0) return NULL;
   q=NULL;
   for(i=0;i<nmatch;i++){
      tmp=p->match[i];
      if(tmp->adopted==NULL){
         q=tmp; bright=tmp->mag; ++i; break;
      }
   }
   for(;i<nmatch;i++){
      tmp=p->match[i];
      if(tmp->adopted!=NULL) continue;
      if((mag=tmp->mag)<bright){
         bright=mag; q=tmp;
      }
   }
   return q;
}
