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


/* macro */
static double dsqrarg;
#define DSQR(a) ((dsqrarg=(a)) == 0.0 ? 0.0 : dsqrarg*dsqrarg)

#define FREE_ARG char*

/* function declarations */
double getVrgrm(double neighDis,int n,int D,int maxIter,int vecLen,
   double *loc1,double *loc2,double *y,double *vd,double tol,double *neigh1,   
   double *neigh2,double *diff,double *nwVrgrmN,double *nwVrgrmO,
   double *nwtNd,double *nwt);
double *dvector(long nl,long nh);
void free_dvector(double *v, long nl, long nh);
void nrerror(char error_text[]);
void nwVrgram(int *nn,int *noBins,int *dDim,double *deltaLo,double *deltaHi,double *loc1In,
   double *loc2In,double *yIn,double *vgram,double *nwVgram,double *mxDis); 

void nwVrgram(int *nn,int *noBins,int *dDim,double *deltaLo,double *deltaHi,double *loc1In,
   double *loc2In,double *yIn,double *vgram,double *nwVgram,double *mxDis) 
{   
   /* maxIter determines the maximum number of iterations for each bin */
   int i,j,k,k1,maxIter=20;
   int D=noBins[0];
   int dGrdDim=dDim[0];
   int n=nn[0];
   int vecLen=(n/D)*(n/2);
   /* tol controls convergence tolerance */
   double tol=1.0e-5,dLo=deltaLo[0],dHi=deltaHi[0],maxDis=mxDis[0];
   double val,minVal,neighDis,wt,dis,temp,*nwt,*loc1,*loc2,*y,*nwVrgrmN,*nwVrgrmO,*nwtNd,*vd, 
      *neigh1,*neigh2,*diff;
   nwt=dvector(1,n);
   loc1=dvector(1,n);
   loc2=dvector(1,n);
   y=dvector(1,n);
   nwVrgrmN=dvector(1,D);
   nwVrgrmO=dvector(1,D);
   nwtNd=dvector(1,D);
   vd=dvector(1,D+1);
   neigh1=dvector(1,vecLen);
   neigh2=dvector(1,vecLen);
   diff=dvector(1,vecLen);
   for(i=1;i<=n;i++){
      loc1[i]=loc1In[i-1];
      loc2[i]=loc2In[i-1];
      y[i]=yIn[i-1];
   }
   /* set up bins */
   for(i=1;i<=D+1;i++) vd[i]=(i-1.0)*maxDis/D;
   /* find estimate of delta */
   minVal=1.0e10;
   for(i=1;i<=dGrdDim;i++){
      val=getVrgrm(dLo+(i-1.0)*dHi/(dGrdDim-1.0),n,D,maxIter,vecLen,
         loc1,loc2,y,vd,tol,neigh1,neigh2,diff,nwVrgrmN,nwVrgrmO,
          nwtNd,nwt);
      if(val<minVal){
         minVal=val;
         neighDis=dLo+(i-1.0)*dHi/(dGrdDim-1.0);
      }
   }
   /* compute weights based on number of neighbors */
   for(i=1;i<=n;i++){
      nwt[i]=0.0;
      for(j=1;j<=n;j++){
         if(sqrt(DSQR(loc1[i]-loc1[j])+DSQR(loc2[i]-loc2[j]))
            <=neighDis)
            nwt[i]+=1.0; /* so a point with no neighbors gets 1.0 */
      }
   }
   /* initialize */
   for(i=1;i<=D;i++){
      nwVrgrmN[i]=0.0;
      nwVrgrmO[i]=0.0;
      nwtNd[i]=0.0;
   }
   /* first get estimate at distance of "0"-use nwVrgrmO[1] to get
   unweighted estimate since this memory slot not used anywhere */
   k1=0;
   for(i=1;i<=n;i++){
      for(j=i+1;j<=n;j++){
         if(sqrt(DSQR(loc1[i]-loc1[j])+DSQR(loc2[i]-loc2[j]))<=vd[2]){
            wt=1.0/(nwt[i]*nwt[j]);
            nwVrgrmN[1]+=wt*DSQR(y[i]-y[j]);
            nwVrgrmO[1]+=DSQR(y[i]-y[j]);
            nwtNd[1]+=wt;
            k1+=1;
         }
      }
   }
   if(nwtNd[1]==0.0) nrerror("no contrasts in smallest bin");
   nwVrgrmN[1]/=nwtNd[1];
   nwVgram[0]=0.5*nwVrgrmN[1];
   /* get unweighted estimate in lowest bin */
   vgram[0]=0.5*nwVrgrmO[1]/k1;
   /* now get estimate at other distances iteratively */
   for(i=2;i<=D;i++){
      /* go through and find all of the relevant squared
      differences for current bin and their number of neighbors */
      for(j=1;j<=vecLen;j++) neigh1[j]=neigh2[j]=0.0;
      k1=1;
      for(j=1;j<=n;j++){
         for(k=j+1;k<=n;k++){
            dis=sqrt(DSQR(loc1[j]-loc1[k])+DSQR(loc2[j]-loc2[k]));
            if(k1==vecLen) nrerror("neigh1 and neigh2 are too short");
            if((dis>vd[i])&&(dis<=vd[i+1])){
               neigh1[k1]=nwt[j];
               neigh2[k1]=nwt[k];
               diff[k1]=DSQR(y[j]-y[k]);
               k1+=1;
            }
         }
      }
      /* get initial value for iterative scheme */
      for(j=1;j<=vecLen;j++){
         if(neigh1[j]>0.0){
            nwVrgrmO[i]+=diff[j];
            nwtNd[i]+=1.0;
         }
      }
      /* if no contrast variogram is zero */
      if(nwtNd[i]==0.0){
         nwVrgrmN[i]=0.0;
         nwVgram[i-1]=vgram[i-1]=0.0;
      }
      else{
         nwVrgrmO[i]/=nwtNd[i];
         temp=nwVrgrmO[i];
         /* get unweighted estimate */
         vgram[i-1]=0.5*nwVrgrmO[i];
         /* for small distances use unweighted estimate */
         if(0.5*(vd[i]+vd[i-1])<=neighDis)
            nwVgram[i-1]=0.5*nwVrgrmO[i];  
         else{  
            /* this next command ensures at least one iteration */
            nwVrgrmN[i]=2.0*tol+nwVrgrmO[i];
            k1=1;
            while(fabs(nwVrgrmN[i]-nwVrgrmO[i])>tol){
               if(k1>1) nwVrgrmO[i]=nwVrgrmN[i];
               nwVrgrmN[i]=nwtNd[i]=0.0;
               for(j=1;j<=vecLen;j++){
                  if(neigh1[j]>0.0){
                     wt=1.0/((nwVrgrmN[1]+fabs(nwVrgrmO[i]-nwVrgrmN[1])*neigh1[j])*
                        (nwVrgrmN[1]+fabs(nwVrgrmO[i]-nwVrgrmN[1])*neigh2[j]));
                     nwVrgrmN[i]+=diff[j]*wt;
                     nwtNd[i]+=wt;
                  }
               }
               nwVrgrmN[i]/=nwtNd[i];
               k1+=1;
               if(k1>=maxIter){
                  fprintf(stderr,"%s","failed to converge for bin with lower bound ");
                  fprintf(stderr,"%f\n",vd[i-1]); 
                  /* if fails to converge just set to unweighted value */
                  nwVrgrmN[i]=temp;
                  break;
               }
            }
            nwVgram[i-1]=0.5*nwVrgrmN[i];
         }
      }
   }
   free_dvector(nwt,1,n);
   free_dvector(loc1,1,n);
   free_dvector(loc2,1,n);
   free_dvector(y,1,n);
   free_dvector(nwVrgrmN,1,D);
   free_dvector(nwVrgrmO,1,D);
   free_dvector(nwtNd,1,D);
   free_dvector(vd,1,D+1);
   free_dvector(neigh1,1,vecLen);
   free_dvector(neigh2,1,vecLen);
   free_dvector(diff,1,vecLen);
}

/* this function used to get deltaHat, repeats a lof of the above code */
double getVrgrm(double neighDis,int n,int D,int maxIter,int vecLen,
   double *loc1,double *loc2,double *y,double *vd,double tol,double *neigh1,
   double *neigh2,double *diff,double *nwVrgrmN,double *nwVrgrmO,
   double *nwtNd,double *nwt)
{
   int i,j,k,k1;
   /* errVal is a number that is returned if all bins are empty */
   double wt,dis,estVar=0.0,errVal=100.0,temp;
/* compute weights based on number of neighbors */
   for(i=1;i<=n;i++){
      nwt[i]=0.0;
      for(j=1;j<=n;j++){
        if(sqrt(DSQR(loc1[i]-loc1[j])+DSQR(loc2[i]-loc2[j]))
            <=neighDis)
            nwt[i]+=1.0; /* so a point with no neighbors gets 1.0 */
      }
   }
/* initialize */
   for(i=1;i<=D;i++){
      nwVrgrmN[i]=0.0;
      nwVrgrmO[i]=0.0;
      nwtNd[i]=0.0;
   }
   /* first get estimate at distance of "0" */
   k1=0;
   for(i=1;i<=n;i++){
      for(j=i+1;j<=n;j++){
         if(sqrt(DSQR(loc1[i]-loc1[j])+DSQR(loc2[i]-loc2[j]))<=vd[2]){
            wt=1.0/(nwt[i]*nwt[j]);
            nwVrgrmN[1]+=wt*DSQR(y[i]-y[j]);
            nwVrgrmO[1]+=DSQR(y[i]-y[j]);
            nwtNd[1]+=wt;
            k1+=1;
         }
      }
   }
   if(nwtNd[1]==0.0){
       nrerror("no contrasts in smallest bin");
   }
   nwVrgrmN[1]/=nwtNd[1];
   /* now get estimate at other distances iteratively */
   for(i=2;i<=D;i++){
/* go through and find all of the relevant squared
   differences and their number of neighbors */
      for(j=1;j<=vecLen;j++) neigh1[j]=neigh2[j]=0.0;
      k1=1;
      for(j=1;j<=n;j++){
         for(k=j+1;k<=n;k++){
            dis=sqrt(DSQR(loc1[j]-loc1[k])+DSQR(loc2[j]-loc2[k]));
            if(k1==n*n) nrerror("neigh1 and neigh2 are too short");
            if((dis>vd[i])&&(dis<=vd[i+1])){
               neigh1[k1]=nwt[j];
               neigh2[k1]=nwt[k];
               diff[k1]=DSQR(y[j]-y[k]);
               k1+=1;
            }
         }
      }
      /* get initial value for iterative scheme */
      for(j=1;j<=vecLen;j++){
         if(neigh1[j]>0.0){
            nwVrgrmO[i]+=diff[j];
            nwtNd[i]+=1.0;
         }
      }
      /* if no contrast variogram is zero and don't use this bin to get delta */
      if(nwtNd[i]==0.0){
         nwVrgrmN[i]=0.0;
      }
      else{
         nwVrgrmO[i]/=nwtNd[i];
         temp=nwVrgrmO[i];
         /* this next command ensures at least one iteration */
         nwVrgrmN[i]=2.0*tol+nwVrgrmO[i];
         k1=1;
         while(fabs(nwVrgrmN[i]-nwVrgrmO[i])>tol){
            if(k1>1) nwVrgrmO[i]=nwVrgrmN[i];
            nwVrgrmN[i]=nwtNd[i]=0.0;
            for(j=1;j<=vecLen;j++){
               if(neigh1[j]>0.0){
                  wt=1.0/((nwVrgrmN[1]+fabs(nwVrgrmO[i]-nwVrgrmN[1])*neigh1[j])*
                     (nwVrgrmN[1]+fabs(nwVrgrmO[i]-nwVrgrmN[1])*neigh2[j]));
                  nwVrgrmN[i]+=diff[j]*wt;
                  nwtNd[i]+=wt;
               }
            }
            nwVrgrmN[i]/=nwtNd[i];
            k1+=1;
            if(k1>=maxIter){
               /* if no convergence just set to zero and don't use to get delta */
               nwVrgrmN[i]=0.0;
               break;
            }
         }
      }
   }
   k1=0;
   for(i=2;i<=D;i++){
      if(nwVrgrmN[i]>0 && nwVrgrmN[i-1]>0){
         k1+=1;
         estVar+=(i-1.0)*DSQR(nwVrgrmN[i]-nwVrgrmN[i-1]);
      }
   }
   if(k1>0) estVar/=k1;
   else estVar=errVal;
   return estVar;
}

double *dvector(long nl,long nh)
/* allocate a vector with index range v[nl...nh] */
{
   void nrerror(char error_text[]);
   double *v;
   v=(double *)malloc((size_t) ((nh-nl+2)*sizeof(double)));
   if (!v) nrerror("allocation failure in dvector()");
   return v-nl+1;
}

void free_dvector(double *v, long nl, long nh)
{
   free((FREE_ARG) (v+nl-1));
}

void nrerror(char error_text[])
/* Numerical Recipes standard error handler */
{
   fprintf(stderr,"Run-time error...\n");
   fprintf(stderr,"%s\n",error_text);
   fprintf(stderr,"...now exiting to system...\n");
   exit(1);
}