#ifdef WIN
#pragma warning (disable: 4244) // disable warning for conversion from double to float, possible loss of data
#endif /* WIN */

#include "fitsio.h"

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

#include "cube.h"

/*
main()
{
    Dim dim;
    float *mycube;
    short *actioncube;

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

    mycube[index(dim,3,3,3)] = 333.09;
    mycube[index(dim,1,2,1)] = 121.09;
    mycube[index(dim,0,1,2)] = 12.09;
    mycube[index(dim,2,1,0)] = 210.09;

    actioncube[index(dim,2,3,3)] = cubePT_IGNORE;
    actioncube[index(dim,1,3,1)] = cubePT_IGNORE;
    actioncube[index(dim,1,1,2)] = cubePT_IGNORE;
    actioncube[index(dim,0,1,0)] = cubePT_IGNORE;

    actioncube[index(dim,3,3,2)] = cubePT_FIND;


    interpol(mycube,actioncube,dim);



}

*/
float interpol(float *mycube,short *actioncube,Dim dim)
{
    short pos;
    unsigned short i,j,k;
    XYZW  indata[1000],res,selected;
    float step,cumstep,distance,selected_distance;
    Dim point;
    double munk;

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

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

    /**/
    step = (float)0.01;
    cumstep  =0.0;
//    printf("\n");
//    printf("\"bezier interpolated data\n");

    selected_distance=1000;
	munk = pow(1.0-cumstep,(double)pos-1);
    for (i=0;(i<100) && (munk != 0.0);i++)
	{
		memset(&res,0x00,sizeof(XYZW));
		Bezier(indata,pos-1,cumstep,munk,&res);
		distance = sqrt(pow((point.x-res.x),2)+pow((point.y-res.y),2)+pow((point.z-res.z),2));
//		printf("%lf %lf %lf %lf %lf\n",res.x,res.y,res.z,res.w,distance);
		if ( distance < selected_distance )
			{
			selected_distance = distance;
			selected.x = res.x;
			selected.y = res.y;
			selected.z = res.z;
			selected.w = res.w;
			}
		cumstep = cumstep+step;
		munk = pow(1.0-cumstep,(double)pos-1);
	}

//    printf("Selected %lf %lf %lf %lf with distance=%lf\n",selected.x,selected.y,selected.z,selected.w,selected_distance);
	mycube[index(dim,point.x, point.y, point.z)] = selected.w;

	return selected.w;
}

 



void interpol1d(float *mycube,short *actioncube,Dim dim, unsigned short nloops)
{
    short pos,ignpos;
    unsigned short i,j,k;
    XYZW  indata[1000],igndata[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));
	memset(igndata,0x00,1000*sizeof(XYZW));


    pos=0;ignpos=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
		    {
		    igndata[ignpos].x = i;
		    igndata[ignpos].y = j;
		    igndata[ignpos].z = k;
		    igndata[ignpos].w = mycube[index(dim,i,j,k)];
		    ignpos++;    
			
#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;
	}

//Ignored Data    
	for(i=0;i<ignpos;i++)
	{
#ifdef DEBUG
	printf("looking for x=%lf y=%lf z=%lf= %lf\n",
	       igndata[i].x,igndata[i].y,igndata[pos].z,igndata[i].w);	
#endif
	selected_distance=1000;
	for(j=0;j<nloops;j++)
	    {
	    distance = sqrt(pow((igndata[i].x-resmat[j].x),2)+pow((igndata[i].y-resmat[j].y),2)+pow((igndata[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",
	       igndata[i].x,igndata[i].y,igndata[i].z,selected.w,selected_distance);	
#endif
	mycube[index(dim,(unsigned short )igndata[i].x,(unsigned short )igndata[i].y,(unsigned short )igndata[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;

   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);
}

