#include <stdlib.h>
#include "tree.h"
#include "ellipse.h"
#include "treelps.h"

/* These macros are unsafe and should be used with care since the 
   parameters occur in them more than once.  They are only used to 
   make the code more readable.  This is why they are placed here 
   in the \texttt{.c} rather than in the \texttt{.h} file. */
#define max(a,b) ((a>b)?a:b)
#define min(a,b) ((a<b)?a:b)

void calc_p(point *points,     /* Calculate p\_max and p\_min */
	    point *nextpoint,  /* for new point `nextpoint' */
	    double gamma1,     /* and existing points in `points' */
	    double gamma2,
	    double a1,
	    double a2,
	    double b1,
	    double b2,
	    double *p_max,
	    double *p_min
	    )
{
  int n_max=0;   /* Number of points in max process */
  int n_min=0;   /* Number of points in min process */
  int i=0;
  ellipse new;
  ellipse *old;
  double extra_max1; /* $m((x\oplus G_1)
			\setminus X_{\max}\oplus G_1)$ */
  double extra_max2; /* $m((x\oplus G_2)
			\setminus X_{\max}\oplus G_2)$ */
  double extra_min1; /* $m((x\oplus G_1)
			\setminus X_{\min}\oplus G_1)$ */
  double extra_min2; /* $m((x\oplus G_2)
			\setminus X_{\min}\oplus G_2)$ */

  applyinorder(points,count,&n_max);   /* Count number of points in */
				       /* the max process */

  applyinorder(points,count_min,&n_min);  /* Count number of */
					  /* points in the min */
                                          /* process */

/* Allocate space according to number of points in max process */  
  old=(ellipse *)calloc((n_max),sizeof(ellipse));

/* First deal with a1,b1 ellipse. */
  makeellipse(nextpoint,&new,a1,b1);

/* Max process first.  Make an array of ellipses 
   from the points in the max process for a1, b1 */
  makeellipseinorder(points,old,&i,a1,b1); 

/* Find extra\_max1 */
  extra_max1=extra(&new,old,n_max,a1,b1);

/* Now min process.  Make an array of ellipses from the 
   points in the min process for a1, b1 */
  i=0;
  makeellipseinorder_min(points,old,&i,a1,b1);

/* Find extra\_min1 */
  extra_min1=extra(&new,old,n_min,a1,b1);

/* Now deal with a2,b2 ellipse.  Algorithm same as for a1,b1 ellipse. */
  makeellipse(nextpoint,&new,a2,b2);

  i=0;
  makeellipseinorder(points,old,&i,a2,b2);
  extra_max2=extra(&new,old,n_max,a2,b2);

  i=0;
  makeellipseinorder_min(points,old,&i,a2,b2);
  extra_min2=extra(&new,old,n_min,a2,b2);

/* Don't forget to free allocated memory! */
  free(old);

  *p_max=pow(gamma1,-extra_max1*SCALE)*
    pow(gamma2,(measure(a2,b2)-extra_min2)*SCALE);
  *p_min=pow(gamma1,-extra_min1*SCALE)*
    pow(gamma2,(measure(a2,b2)-extra_max2)*SCALE);
}

/* Use with applyinorder to count points */
void count(point *p,void *arg)
{
  int *num;

  num=(int *) arg;
  (*num)++;
}

/* Use with applyinorder to count points in the minimum process */
void count_min(point *p,void *arg)
{
  int *num;

  num=(int *) arg;
  if(p->min_life)
    (*num)++;
}

void makeellipse(point *p, /* Point about which to centre ellipse */
		 ellipse *e, /* Address of ellipse to centre at p */
		 double a,   /* Length of semi-x axis */
		 double b)   /* Length of semi-y axis */
{
  double d; /* Semi-focal separation */

  if(a>b){
    d=sqrt(a*a-b*b);
    e->f1.x=p->pos.x-d;          /* Set e to be a1,b1 ellipse */
    e->f2.x=p->pos.x+d;          /* around new position */
    e->f1.y=e->f2.y=p->pos.y;
  }
  else{
    d=sqrt(b*b-a*a);
    e->f1.y=p->pos.y-d;          /* Set e to be a1,b1 ellipse */
    e->f2.y=p->pos.y+d;          /* around new position */
    e->f1.x=e->f2.x=p->pos.x;
  }
}

/* Returns the amount of extra area resulting from the addition of 
   a new point */
double extra(ellipse *new,  /* Ellipse centred at new point */
	     ellipse old[], /* Array of ellipses centred at */
			    /* existing points */
	     int n_old,
	     double a,      /* Length of semi-x axis */
	     double b)      /* Length of semi-y axis */
{
  int i,j;
  int k=0;
  int n_olap=0;         /* Number of ellipses from old which */
			/* overlap with new */
  int extra_boxes=0;    /* Number of extra 2a/RESOLUTION by */
			/* 2b/RESOLUTION boxes added by new */
                        /* point */
  ellipse *over;        /* Array of ellipses overlapping with new */
  coord box;            /* The notional coordinates of the box we */
			/* are working on */
  short morearea;       /* Inicator of whether box is additional area */

  coord increment;      /* For the purposes of common */
  double diff;		/* sub-expression elimination. */

  if(n_old == 0)
    return acos(-1.0)*a*b;
  over=(ellipse *)calloc(n_old,sizeof(ellipse));

  for(i=n_old;i>=1;--i){
/* If new and old[i] overlap store overlap-ellipse in over[n\_olap-1] */
    if(overlap(new,&(old[i-1]),&(over[n_olap]),a,b) != 0)
      ++n_olap;
  }
  if(n_olap==0){
    free(over);
    return acos(-1.0)*a*b;
  }

/* For those rectangles which overlap with new's rectangle, 
   calculate the amount of extra area added by new.  First do 
   some common sub-expression elimination and initialize box. */
  increment.x=2*a/RESOLUTION;
  increment.y=2*b/RESOLUTION;
  box.x=(new->f1.x+new->f2.x)/2 + a + increment.x;

/* If a==b, f1==f2 so we can eliminate many calculations. */
  if(a==b){
    for(i=RESOLUTION; i>=0; --i){
      box.x-=increment.x;
      box.y=(new->f1.y+new->f2.y)/2 + b + increment.y;
      for(j=RESOLUTION; j>=0; --j){
	morearea=1;
	box.y-=increment.y;

/* If distance from f1 to point plus distance from f2 to 
   point is greater than 2a, point is outside new ellipse 
   so we needn't worry about it. */
	if(sqrt((new->f1.x-box.x)*(new->f1.x-box.x) + 
		(new->f1.y-box.y)*(new->f1.y-box.y)) > a)
	  morearea=0;

	else{
	  for(k=n_olap; k>0; --k){
/* If distance from f1 to point plus distance from f2 
   to point is less than 2a, point is inside ellipse.  The
   repeats take care of the toroidal boundary conditions. */
	    if(sqrt((over[k-1].f1.x-box.x) * 
		    (over[k-1].f1.x-box.x) + 
		    (over[k-1].f1.y-box.y) * 
		    (over[k-1].f1.y-box.y)) < a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x+1-box.x) * 
		    (over[k-1].f1.x+1-box.x) + 
		    (over[k-1].f1.y-box.y) * 
		    (over[k-1].f1.y-box.y)) < a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x-1-box.x) * 
		    (over[k-1].f1.x-1-box.x) + 
		    (over[k-1].f1.y-box.y) * 
		    (over[k-1].f1.y-box.y)) < a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x-box.x) * 
		    (over[k-1].f1.x-box.x) + 
		    (over[k-1].f1.y+1-box.y) * 
		    (over[k-1].f1.y+1-box.y)) < a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x-box.x) * 
		    (over[k-1].f1.x-box.x) + 
		    (over[k-1].f1.y-1-box.y) * 
		    (over[k-1].f1.y-1-box.y)) < a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x+1-box.x) * 
		    (over[k-1].f1.x+1-box.x) + 
		    (over[k-1].f1.y+1-box.y) * 
		    (over[k-1].f1.y+1-box.y)) < a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x+1-box.x) * 
		    (over[k-1].f1.x+1-box.x) + 
		    (over[k-1].f1.y-1-box.y) * 
		    (over[k-1].f1.y-1-box.y)) < a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x-1-box.x) * 
		    (over[k-1].f1.x-1-box.x) + 
		    (over[k-1].f1.y+1-box.y) * 
		    (over[k-1].f1.y+1-box.y)) < a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x-1-box.x) * 
		    (over[k-1].f1.x-1-box.x) + 
		    (over[k-1].f1.y-1-box.y) * 
		    (over[k-1].f1.y-1-box.y)) < a){
	      morearea=0;
	      break;
	    }
	  }
	}

/* If after all this, point is in new ellipse and not in 
   any old ellipse so constitutes extra area */
	if(morearea != 0)
	  ++extra_boxes;
      }
    }
  }
/* If a$<$b, f1.y=f2.y so we can eliminate some calculations. */
  else if(a<b){
    for(i=RESOLUTION; i>=0; --i){
      box.x-=increment.x;
      box.y=(new->f1.y+new->f2.y)/2 + b + increment.y;
      for(j=RESOLUTION; j>=0; --j){
	morearea=0;
	box.y-=increment.y;

	diff=new->f1.y-box.y;/* Common sub-expression elimination */
/* If distance from f1 to point plus distance from f2 to 
   point is greater than 2a, point is outside new ellipse 
   so we needn't worry about it. */
	if(sqrt((new->f1.x-box.x)*(new->f1.x-box.x) + 
		(diff)*(diff)) + 
	   sqrt((new->f2.x-box.x)*(new->f2.x-box.x) + 
		(diff)*(diff)) > 2*a)
	  morearea=0;

	else{
	  for(k=n_olap; k>0; --k){
	    diff=over[k-1].f1.y-box.y;  /* Common sub-expression */
					/* elimination */
/* If distance from f1 to point plus distance from f2 
   to point is less than 2a, point is inside ellipse.  The repeats 
   deal with the toroidal boundary conditions. */
	    if(sqrt((over[k-1].f1.x-box.x) * 
		    (over[k-1].f1.x-box.x) + 
		    (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x-box.x) * 
		    (over[k-1].f2.x-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x+1-box.x)*
		    (over[k-1].f1.x+1-box.x) + (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x+1-box.x)*
		    (over[k-1].f2.x+1-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x-1-box.x)*
		    (over[k-1].f1.x-1-box.x) + (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x-1-box.x)*
		    (over[k-1].f2.x-1-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }

	    diff=over[k-1].f1.y+1-box.y;/* Common sub-expression */
					/* elimination */
	    if(sqrt((over[k-1].f1.x-box.x) * 
		    (over[k-1].f1.x-box.x) + 
		    (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x-box.x) * 
		    (over[k-1].f2.x-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x+1-box.x)*
		    (over[k-1].f1.x+1-box.x) + (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x+1-box.x)*
		    (over[k-1].f2.x+1-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x-1-box.x)*
		    (over[k-1].f1.x-1-box.x) + (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x-1-box.x)*
		    (over[k-1].f2.x-1-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }

	    diff=over[k-1].f1.y-1-box.y;/* Common sub-expression */
					/* elimination */

	    if(sqrt((over[k-1].f1.x-box.x) * 
		    (over[k-1].f1.x-box.x) + 
		    (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x-box.x) * 
		    (over[k-1].f2.x-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x+1-box.x)*
		    (over[k-1].f1.x+1-box.x) + (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x+1-box.x)*
		    (over[k-1].f2.x+1-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((over[k-1].f1.x-1-box.x)*
		    (over[k-1].f1.x-1-box.x) + (diff)*(diff)) + 
	       sqrt((over[k-1].f2.x-1-box.x)*
		    (over[k-1].f2.x-1-box.x) + 
		    (diff)*(diff)) < 2*a){
	      morearea=0;
	      break;
	    }
	  }
	}

/* If after all this, point is in new ellipse and not in 
   any old ellipse so constitutes extra area */
	if(morearea != 0)
	  ++extra_boxes;
      }
    }
  }
/* else f1.x=f2.x, so we eliminate different calculations */
  else{
    for(i=RESOLUTION; i>=0; --i){
      box.x-=increment.x;
      box.y=(new->f1.y+new->f2.y)/2 + b + increment.y;
      for(j=RESOLUTION; j>=0; --j){
	morearea=0;
	box.y-=increment.y;

	diff=new->f1.x-box.x;/* Common sub-expression elimination */
/* If distance from f1 to point plus distance from f2 to 
   point is greater than 2a, point is outside new ellipse 
   so we needn't worry about it. */
	if(sqrt((diff)*(diff) + 
		(new->f1.y-box.y)*(new->f1.y-box.y)) + 
	   sqrt((diff)*(diff) + 
		(new->f2.y-box.y)*(new->f2.y-box.y)) > 2*a)
	  morearea=0;

	else{
	  for(k=n_olap; k>0; --k){
	    diff=over[k-1].f1.x-box.x;  /* Common sub-expression */
					/* elimination */
/* If distance from f1 to point plus distance from f2 
   to point is less than 2a, point is inside ellipse the repeats 
   deal with the toroidal boundary conditions. */
	    if(sqrt((diff)*(diff) + (over[k-1].f1.y-box.y)*
		    (over[k-1].f1.y-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y-box.y)*
		    (over[k-1].f2.y-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((diff)*(diff) + (over[k-1].f1.y+1-box.y)*
		    (over[k-1].f1.y+1-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y+1-box.y)*
		    (over[k-1].f2.y+1-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((diff)*(diff) + (over[k-1].f1.y-1-box.y)*
		    (over[k-1].f1.y-1-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y-1-box.y)*
		    (over[k-1].f2.y-1-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }

	    diff=over[k-1].f1.x+1-box.x;/* Common sub-expression */
					/* elimination */
	    if(sqrt((diff)*(diff) + (over[k-1].f1.y-box.y)*
		    (over[k-1].f1.y-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y-box.y)*
		    (over[k-1].f2.y-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((diff)*(diff) + (over[k-1].f1.y+1-box.y)*
		    (over[k-1].f1.y+1-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y+1-box.y)*
		    (over[k-1].f2.y+1-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((diff)*(diff) + (over[k-1].f1.y-1-box.y)*
		    (over[k-1].f1.y-1-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y-1-box.y)*
		    (over[k-1].f2.y-1-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }

	    diff=over[k-1].f1.x-1-box.x;/* Common sub-expression */
					/* elimination */
	    if(sqrt((diff)*(diff) + (over[k-1].f1.y-box.y)*
		    (over[k-1].f1.y-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y-box.y)*
		    (over[k-1].f2.y-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((diff)*(diff) + (over[k-1].f1.y+1-box.y)*
		    (over[k-1].f1.y+1-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y+1-box.y)*
		    (over[k-1].f2.y+1-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }

	    if(sqrt((diff)*(diff) + (over[k-1].f1.y-1-box.y)*
		    (over[k-1].f1.y-1-box.y)) + 
	       sqrt((diff)*(diff) + (over[k-1].f2.y-1-box.y)*
		    (over[k-1].f2.y-1-box.y)) < 2*a){
	      morearea=0;
	      break;
	    }
	  }
	}

/* If after all this, point is in new ellipse and not in 
   any old ellipse so constitutes extra area */
	if(morearea != 0)
	  ++extra_boxes;
      }
    }
  }

  free(over);
  return (double )extra_boxes*(4.0*a*b)/(RESOLUTION*RESOLUTION);
}

/* Return value of 0 means that the outer rectangles of the 
   ellipses passed were non-overlapping */
int overlap(ellipse *A,
	    ellipse *B,
	    ellipse *OL,
	    double a,
	    double b)
{
  coord centreA;   /* Centre of ellipse A */
  coord centreB;   /* Centre of ellipse B */

  centreA.x=(A->f1.x+A->f2.x)/2;
  centreA.y=(A->f1.y+A->f2.y)/2;
  centreB.x=(B->f1.x+B->f2.x)/2;
  centreB.y=(B->f1.y+B->f2.y)/2;
  if(((centreA.x-a <= centreB.x+a) &&   /* If A and B overlap in */
      (centreA.x+a >= centreB.x-a)) &&  /* the x direction */
     ((centreA.y-b <= centreB.y+b) &&   /* And A and B overlap in */
      (centreA.y+b >= centreB.y-b))) {  /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

/* The next eight `if' statements deal with the possibility that 
   the toroidal nature of the box we are simulating on might 
   cause the two rectangles to overlap we deal first with simple 
   cases, then with toroidal-in-two-directions cases. */
  if(((centreA.x+1-a <= centreB.x+a) && /* If A and B overlap in */
      (centreA.x+1+a >= centreB.x-a)) &&   /* the x direction */
     ((centreA.y-b <= centreB.y+b) &&   /* And A and B overlap in */
      (centreA.y+b >= centreB.y-b))) {   /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

  if(((centreA.x-1-a <= centreB.x+a) && /* If A and B overlap in */
      (centreA.x-1+a >= centreB.x-a)) &&   /* the x direction */
     ((centreA.y-b <= centreB.y+b) &&   /* And A and B overlap in */
      (centreA.y+b >= centreB.y-b))) {   /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

  if(((centreA.x-a <= centreB.x+a) &&   /* If A and B overlap in */
      (centreA.x+a >= centreB.x-a)) &&   /* the x direction */
     ((centreA.y+1-b <= centreB.y+b) && /* And A and B overlap in */
      (centreA.y+1+b >= centreB.y-b))) {   /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

  if(((centreA.x-a <= centreB.x+a) &&   /* If A and B overlap in */
      (centreA.x+a >= centreB.x-a)) &&   /* the x direction */
     ((centreA.y-1-b <= centreB.y+b) && /* And A and B overlap in */
      (centreA.y-1+b >= centreB.y-b))) {   /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

/* Toroidal in two directions... Do +1's first.  */
  if(((centreA.x+1-a <= centreB.x+a) && /* If A and B overlap in */
      (centreA.x+1+a >= centreB.x-a)) &&   /* the x direction */
     ((centreA.y+1-b <= centreB.y+b) && /* And A and B overlap in */
      (centreA.y+1+b >= centreB.y-b))) {   /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

  if(((centreA.x-1-a <= centreB.x+a) && /* If A and B overlap in */
      (centreA.x-1+a >= centreB.x-a)) &&   /* the x direction */
     ((centreA.y+1-b <= centreB.y+b) && /* And A and B overlap in */
      (centreA.y+1+b >= centreB.y-b))) {   /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

  if(((centreA.x+1-a <= centreB.x+a) && /* If A and B overlap in */
      (centreA.x+1+a >= centreB.x-a)) &&   /* the x direction */
     ((centreA.y-1-b <= centreB.y+b) && /* And A and B overlap in */
      (centreA.y-1+b >= centreB.y-b))) {   /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

  if(((centreA.x-1-a <= centreB.x+a) && /* If A and B overlap in */
      (centreA.x-1+a >= centreB.x-a)) &&   /* the x direction */
     ((centreA.y-1-b <= centreB.y+b) && /* And A and B overlap in */
      (centreA.y-1+b >= centreB.y-b))) {   /* the y direction */
    OL->f1.x=B->f1.x;
    OL->f1.y=B->f1.y;       /* Copy B into OL */
    OL->f2.x=B->f2.x;
    OL->f2.y=B->f2.y;
    return 1;         /* They do indeed overlap */
  }

  return 0;     /* If no overlap on toroid, return 0 */
}
