#include <stdlib.h>
#include <math.h>
#include <Rmath.h>
#include <R.h>

struct inter_comp {
   int i;
   int c;
};


void computeCardinals(int *source,int n,int* result) {
   int sn;

   for(sn=0;sn<n;sn++) {
      if(source[sn]<0) result[sn] = 1;
      else result[sn] = result[source[sn]-1];
      if(source[sn+n]<0) result[sn]++;
      else result[sn] += result[source[sn+n]-1];
   }
}



void computeStability(int *source,int *target,int *number,double* result,int *nbleaves) {
   int n,sl,tl,sn,tn;
   int **LL;
   struct inter_comp **NL,**NN;
   double max,tmp;

   n = *number;

   /* memory allocation */
   LL = (int**) calloc(n+1,sizeof(int*));
   for(sl=0;sl<n+1;sl++) LL[sl] = (int*) calloc(n+1,sizeof(int));
   NL = (struct inter_comp**) calloc(n,sizeof(struct inter_comp*));
   for(sn=0;sn<n;sn++) NL[sn] = (struct inter_comp*) calloc(n+1,sizeof(struct inter_comp));
   NN = (struct inter_comp**) calloc(n,sizeof(struct inter_comp*));
   for(sn=0;sn<n;sn++) NN[sn] = (struct inter_comp*) calloc(n,sizeof(struct inter_comp));

   computeCardinals(source,n,nbleaves);
   
   
   /* LL is a diagonal matrix */ 
   for(sl=0;sl<n+1;sl++)
      for(tl=0;tl<n+1;tl++)
	 if(tl==sl) LL[sl][tl] = 1;
	 else LL[sl][tl] = 0;
   

   /* NL computing */
   for(sn=0;sn<n;sn++) {
      for(tl=0;tl<n+1;tl++) {
      	 if(source[sn]<0)
	    if(source[sn+n]<0) NL[sn][tl].i = LL[-source[sn]-1][tl] + LL[-source[sn+n]-1][tl];
	    else NL[sn][tl].i = LL[-source[sn]-1][tl] + NL[source[sn+n]-1][tl].i;
	 else
	    if(source[sn+n]<0) NL[sn][tl].i = NL[source[sn]-1][tl].i + LL[-source[sn+n]-1][tl];
	    else NL[sn][tl].i = NL[source[sn]-1][tl].i + NL[source[sn+n]-1][tl].i;

	 if(NL[sn][tl].i>0) {
	    NL[sn][tl].i = 1;
	    NL[sn][tl].c = 0;
	 }
	 else NL[sn][tl].c = 1;
      }
   }

   /* NN computing */
   for(sn=0;sn<n;sn++) {
      for(tn=0;tn<n;tn++) {
	 if(target[tn]<0)
	    if(target[tn+n]<0) {
	       NN[sn][tn].i = NL[sn][-target[tn]-1].i + NL[sn][-target[tn+n]-1].i;
	       NN[sn][tn].c = NL[sn][-target[tn]-1].c + NL[sn][-target[tn+n]-1].c;
	    }
	    else {
	       NN[sn][tn].i = NL[sn][-target[tn]-1].i + NN[sn][target[tn+n]-1].i;
	       NN[sn][tn].c = NL[sn][-target[tn]-1].c + NN[sn][target[tn+n]-1].c;
	    }
	 else 
	    if(target[tn+n]<0) {
	       NN[sn][tn].i = NN[sn][target[tn]-1].i + NL[sn][-target[tn+n]-1].i;
	       NN[sn][tn].c = NN[sn][target[tn]-1].c + NL[sn][-target[tn+n]-1].c;
	    }
	    else {
	       NN[sn][tn].i = NN[sn][target[tn]-1].i + NN[sn][target[tn+n]-1].i;
	       NN[sn][tn].c = NN[sn][target[tn]-1].c + NN[sn][target[tn+n]-1].c;
	    }
      }
   }


   for(sn=0;sn<n;sn++) {
      max = 0.;
      for(tn=0;tn<n;tn++) {
	 tmp = ((double)NN[sn][tn].i)/(nbleaves[sn]+NN[sn][tn].c);
	 if(tmp>max) max = tmp;
      }
      result[sn] = max;
   }
   
   /* memory deallocation */
   for(sl=0;sl<n+1;sl++) free(LL[sl]);
   free(LL);
   for(sn=0;sn<n;sn++) free(NL[sn]);
   free(NL);
   for(sn=0;sn<n;sn++) free(NN[sn]);
   free(NN);
}