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

#define cubePT_IGNORE    -1
#define cubePT_USE        0
#define cubePT_FIND       1

typedef struct xyzw_ {
  double x;
  double y;
  double z;
  double w;
} XYZW;

typedef struct dim_ {
  unsigned short x;
  unsigned short y;
  unsigned short z;
} Dim;

#define index(d,X,Y,Z)   X+Y*d.x+Z*d.x*d.y
#define cubsize(d)       (d.x*d.y*d.z)


int Bezier(XYZW *p,int n,double mu, double munk, XYZW *res);
void interpol2d(float *mycube,short *actioncube,Dim dim, unsigned short  nloops);

main()
{
    Dim dim;
    float *mycube;
    short *actioncube;
    unsigned short i,j,k;

    dim.x=5;dim.y=1;dim.z=1;
    mycube = (float*)calloc(cubsize(dim),sizeof(float));
    actioncube = (short*)calloc(cubsize(dim),sizeof(short));

    
    for (i=0;i<dim.x;i++)
	{
	for (j=0;j<dim.y;j++)
	    {
	    for (k=0;k<dim.z;k++)
		{
		actioncube[index(dim,i,j,k)] = cubePT_USE;
		mycube[index(dim,i,j,k)] = 1000.0;
		printf("mycube[%d,%d,%d]=%d,%lf\n",i,j,k,index(dim,i,j,k),mycube[index(dim,i,j,k)]);
		}
	    }
	}


    /*actioncube[index(dim,2,0,0)] = cubePT_FIND;*/
      mycube[index(dim,2,0,0)] = 1800.0;


    interpol2d(mycube,actioncube,dim,100);



}


void interpol2d(float *mycube,short *actioncube,Dim dim, unsigned short nloops)
{
    short pos;
    unsigned short i,j,k;
    XYZW  indata[1000],res,*resmat,selected;
    float distance,selected_distance;
    Dim point;
    double stepsize,cumstep,munk;

    resmat = (XYZW*)calloc(nloops,sizeof(XYZW));

    memset(indata,0x00,1000*sizeof(XYZW));


    pos=0;
    for(i=0;i<dim.x;i++)
	{
	for(j=0;j<dim.y;j++)
	    {
	    for(k=0;k<dim.z;k++)
		{
		if ( !actioncube[index(dim,i,j,k)] || actioncube[index(dim,i,j,k)] == cubePT_FIND ) 
		    {
#ifdef DEBUG
		    printf("Used/Find mycube[%d,%d,%d]=%d,%lf\n",i,j,k,index(dim,i,j,k),mycube[index(dim,i,j,k)]);
#endif
		    indata[pos].x = i;
		    indata[pos].y = j;
		    indata[pos].z = k;
		    indata[pos].w = mycube[index(dim,i,j,k)];
		    pos++;    
		    }
		else
		    {
#ifdef DEBUG
			printf("Ignored mycube[%d,%d,%d]=%d,%lf\n",i,j,k,index(dim,i,j,k),mycube[index(dim,i,j,k)]);
#endif
		    }
		}	
	    }	
	}

    /**/


    stepsize = 1.0/(double)nloops;
    cumstep =0.00000000;
#ifdef DEBUG
    printf("\n");
    printf("\"bezier interpolated data\n");
#endif
    munk = pow(1.0-cumstep,(double)pos-1);
    for (i=0;i<nloops && munk != 0.0; i++)
	{
	memset(&res,0x00,sizeof(XYZW));
	Bezier(indata,pos-1,cumstep,munk,&res);

	resmat[i]=res;


#ifdef DEBUG
	printf("%d %d,%f : %lf %lf %lf %lf %lf\n",pos-1,i,cumstep,res.x,res.y,res.z,res.w,distance);
#endif
	cumstep = cumstep+stepsize;
	munk = pow(1.0-cumstep,(double)pos-1);
	}
    
    for(i=0;i<pos;i++)
	{
#ifdef DEBUG
	printf("looking for x=%lf y=%lf z=%lf= %lf\n",
	       indata[i].x,indata[i].y,indata[pos].z,indata[i].w);	
#endif
	selected_distance=1000;
	for(j=0;j<nloops;j++)
	    {
	    distance = sqrt(pow((indata[i].x-resmat[j].x),2)+pow((indata[i].y-resmat[j].y),2)+pow((indata[i].z-resmat[j].z),2));
	    if ( distance < selected_distance )
		{
		selected_distance = distance;
		selected.x = resmat[j].x;
		selected.y = resmat[j].y;
		selected.z = resmat[j].z;
		selected.w = resmat[j].w;
		}
	    }
#ifdef DEBUG
	printf("Selected %lf %lf %lf %lf with distance=%lf\n",
	       indata[i].x,indata[i].y,indata[i].z,selected.w,selected_distance);	
#endif
	mycube[index(dim,(unsigned short )indata[i].x,(unsigned short )indata[i].y,(unsigned short )indata[i].z)] = selected.w;
	}
    free(resmat);
}

 











int Bezier(XYZW *p,int n,double mu, double munk, XYZW *res)
{
   int k,kn,nn,nkn;
   double blend,muk;
   short i;

   muk = 1;
   /*munk = pow(1.0-mu,(double)n);*/
       
   for (k=0;k<=n;k++) {
      nn = n;
      kn = k;
      nkn = n - k;
      blend = muk * munk;
      muk *= mu;
      munk /= (1.0-mu);
      while (nn >= 1) {
         blend *= (double)nn;
         nn--;
         if (kn > 1) {
            blend /= (double)kn;
            kn--;
         }
         if (nkn > 1) {
            blend /= (double)nkn;
            nkn--;
         }
      }
      res->x += p[k].x * blend;
      res->y += p[k].y * blend;
      res->z += p[k].z * blend;
      res->w += p[k].w * blend;
   }

   return(0);
}
