#include <stdlib.h>


void splie2 (float x1a[], float x2a[], float **ya, int m, int n, float **y2a);
void splin2(float x1a[], float x2a[], float **ya, float **y2a, int m, int n, float x1, float x2, float *y);
void splint(float xa[], float ya[], float y2a[], int n, float x, float *y);
void spline(float x[], float y[], int n, float yp1, float ypn, float y2[]);

#define psf_size 9
void scale_psf(float ** psf)
{

	int  col, row, j, i, w; 
	float c, r;
	float *x1, *x2, **y, **ys;
	float *y1, *ys1;
	float result;
	float *Jpsf,*Hpsf;
	int ndata;
	long naxes = 2, axis_length[2];
	float lambda, magnify;
	float im, jm;
	
	int points = 7;
	
	
	
    if ((x1 = (float *)calloc(points,sizeof(float))) == NULL)
		printf("x1\n");
	if ((x2 = (float *)calloc(points,sizeof(float))) == NULL)
		printf("x2\n");
	if ((y = (float **)calloc(points,sizeof(float*))) == NULL)
		printf("y\n");
	if ((ys = (float **)calloc(points,sizeof(float*))) == NULL)
		printf("ys\n");
	if ((y1 = (float *)calloc(points,sizeof(float))) == NULL)
		printf("y1\n");
	if ((ys1 = (float *)calloc(points,sizeof(float))) == NULL)
		printf("ys1\n");
	for (i = 0; i < points; i ++)
		{
		if ((y[i] = (float *)calloc(points,sizeof(float))) == NULL)
			printf("y[i]\n");
		if ((ys[i] = (float *)calloc(points,sizeof(float))) == NULL)
			printf("ys[i]\n");
		}
	
			 
		if (  (Jpsf = load_simple_fits_float_data("/users/doug/pcxp/LIBRARY/2011-12/binned_dpgfit_jlaser_2011-12.fits", &naxes, axis_length, &ndata)) == NULL)
			printf("\n\n no J psf\n\n");
		
		if (  (Hpsf = load_simple_fits_float_data("/users/doug/pcxp/LIBRARY/2011-12/binned_dpgfit_hlaser_2011-12.fits", &naxes, axis_length, &ndata)) == NULL)
				printf("\n\n no H psf\n\n");
				
	
					
				
	for ( w = 0 ; w < NCHAN; w++)
	{
		lambda = LAMBDA_MIN + LAMBDA_INC*w;
		if (lambda <= LAMBDA_REF)
			{
			magnify = lambda/1350;	
			printf("%f, %f\n", lambda, magnify);	
            i = -1;
			for (row = (psf_size -1)/2 - (points - 1)/2; row< (psf_size -1)/2 + (points - 1)/2 +1; row++)
				{
				j = 0;
				i++;
				for (col = (psf_size -1)/2 - (points - 1)/2; col < (psf_size -1)/2 + (points - 1)/2 + 1; col++)	
					{
					x1[j] = Jpsf[psf_size*col  + row]; //if pixel[0] is min then im should equal start pixel to agree with icube dbuf[(crudeYcntr + j - 2)*FOCPLWIDTH + crudeXcntr + i - 2];
					y1[j++] = (float)(col  - ((psf_size -1)/2 - (points - 1)/2));
					printf("%d %d %e %f\n",col, row,  x1[j-1], y1[j-1]);											
					}
					
				spline( y1,x1,j - 1,0,0, ys1);  //	spline( x1,y1,j - 1,1e30,1e30, ys1);
				memcpy(ys[i], ys1, j*sizeof(float));
			// calculate spline fit for every row in the col
				for (col = 0; col < points; col++)
					{
					splint(y1, x1, ys1, points, (float) col, &result); 
					y[i][col] = result;
					}
				}	//end rows
			
			for (col = 0; col < points; col++)	
				x2[i++] = col;
			for (row = 0; row < points; row++)		
				x1[row] = row;
			
			for (row = 0; row < 3; row++)
				for(col = 0; col < 3; col++)
					
								  for (i = (points - 1)/2 - 1, row = 0; i < (points - 1)/2  + 2 ; i ++, row++)
								  for (j = (points - 1)/2 - 1, col = 0; j < (points - 1)/2  + 2 ; j ++, col++)
								  for(r = -0.45; r < 0.5; r += .1)
								  for(c = -0.45; c < 0.5;c += .1) 
								  {
								  printf("here\n");	
								  splin2(x1,x2, y, ys, points, points, j  + c , i  + r , &result);    //   printf("%f\n", result);
								

								  psf[w][(col) * 3 + row] += result*.01;
								  }

					
					
				/*	for (im = (points - magnify)/2 - magnify;   im < (points - magnify)/2  + 2*magnify ; im += magnify)
						for (jm = (points - magnify)/2 - magnify; jm < (points - magnify)/2  + 2*magnify; jm += magnify)
							for(r = -0.45; r < 0.5; r += .1)
								for(c = -0.45; c < 0.5;c += .1) 
									{
									printf("%f %f\n", jm + c,im + c);
									splin2(x1,x2, y, ys, points, points, jm + c , im + r , &result);    //   printf("%f\n", result);
									psf[w][(col) * 3 + row] += result*0.1;
									}	
				 */
	
			}
	
		/*else
			{
			magnify = lambda/1510;
			i = -1;
			for (row = 0; row < points; row++)
				{
					j = 0;
					i++;
					for (col = 0; col < points; col++)	
						{
						x1[j] = Hpsf[col*points + row]; 
						y1[j++] = (float) col;											
						}
					spline( y1,x1,j - 1,0,0, ys1);  //	spline( x1,y1,j - 1,1e30,1e30, ys1);
					memcpy(ys[i], ys1, j*sizeof(float));
				// calculate spline fit for every row in the col
					for (col = 0; col < points; col++)
						{
						splint(y1, x1, ys1, points, (float) col, &result); 
						y[i][col] = result;
						}
				}	//end rows
			for ( col = 0; col < points; col ++)
				x2[i++] = col;
			for (row = 0; row < points; row++)		
				x1[row] = row;
			
			for (row = 0; row < 3; row++)
				for(col = 0; col < 3; col++)
					for (im = (points - magnify)/2 - magnify;   im < (points - magnify)/2  + 2*magnify ; im += magnify)
						for (jm = (points - magnify)/2 - magnify; jm < (points - magnify)/2  + 2*magnify; jm += magnify)
							for(r = -0.45; r < 0.5; r += .1)
								for(c = -0.45; c < 0.5;c += .1) 
									{
									
									splin2(x1,x2, y, ys, points, points, j + c , i + r , &result);    //   printf("%f\n", result);
									psf[w][(col) * 3 + row] += result*0.1;
									}
			

			} //else
			*/
		
		
	}
		
		
				
					
							

}


void  shift(float *shiftedarray, float *arraytobeshifted, float xoff, float yoff) {
  
    int  col, row, j, i; 
	float c, r;
	float *x1, *x2, **y, **ys;
	float *y1, *ys1;
	float result;
	int points= 5;
	  
	
	
    
	
	

    if ((x1 = (float *)calloc(points,sizeof(float))) == NULL)
		printf("x1\n");
	if ((x2 = (float *)calloc(points,sizeof(float))) == NULL)
		printf("x2\n");
	if ((y = (float **)calloc(points,sizeof(float*))) == NULL)
		printf("y\n");
	if ((ys = (float **)calloc(points,sizeof(float*))) == NULL)
		printf("ys\n");
	if ((y1 = (float *)calloc(points,sizeof(float))) == NULL)
		printf("y1\n");
	if ((ys1 = (float *)calloc(points,sizeof(float))) == NULL)
		printf("ys1\n");
	for (i = 0; i < points; i ++)
		{
		if ((y[i] = (float *)calloc(points,sizeof(float))) == NULL)
			printf("y[i]\n");
		if ((ys[i] = (float *)calloc(points,sizeof(float))) == NULL)
			printf("ys[i]\n");
		}
	

//for each data file
 

    
  for(i = 0; i < 9; i++)
	shiftedarray[i] = 0;
	        

            i = -1;
        for (row = 0; row< points; row++)
            {
            j = 0;
			i++;
            for (col = 0; col < points; col++)	
            	{
                x1[j] = arraytobeshifted[col*points + row]; //if pixel[0] is min then im should equal start pixel to agree with icube										
                y1[j++] = (float) col;											
                }
            spline( y1,x1,j - 1,0,0, ys1);  //	spline( x1,y1,j - 1,1e30,1e30, ys1);
            memcpy(ys[i], ys1, j*sizeof(float));
                    // calculate spline fit for every row in the col
            for (col = 0; col < points; col++)
                {
                splint(y1, x1, ys1, points, (float) col, &result); 
                y[i][col] = result;
                }
            }	//end rows

    i = 0;
    // use interpolated rows to get complete spline. 
    for ( col = 0; col < points; col ++)
        x2[i++] = col;
    for (row = 0; row < points; row++)		
        x1[row] = row;
   
     for (i = (points - 1)/2 - 1, row = 0;   i < (points - 1)/2  + 2 ; i ++, row++)
        for (j = (points - 1)/2 -1, col = 0; j < (points - 1)/2  + 2 ; j ++, col++)
         for(r = -0.45; r < 0.5; r += .1)
                for(c = -0.45; c < 0.5;c += .1) 
                {
				splin2(x1,x2, y, ys, points, points, j + xoff + c , i - yoff + r , &result);    //   printf("%f\n", result);
                shiftedarray[(col) * 3 + row] += result*.01;
                }
        



	free(x1);
	free(x2);
	for (i = 0; i < points; i ++)
		{
		free(y[i]);
		free(ys[i]);
		}	

	free(y);
	free(ys);
    free(y1);
    free(ys1);

	
}


void splie2 (float x1a[], float x2a[], float **ya, int m, int n, float **y2a)
{
	void spline (float x[], float y[], int n, float yp1, float ypn, float y2[]);
	int j;
	
	for ( j= 0; j < m; j++)
		spline(x2a, ya[j], n, 1.0e30, 1.0e30, y2a[j]);
	
		
}

void splin2(float x1a[], float x2a[], float **ya, float **y2a, int m, int n, float x1, float x2, float *y)
{

	void spline (float x[], float y[], int n, float yp1, float ypn, float y2[]);
	void splint(float xa[], float ya[], float y2a[], int n, float x, float *y);
	int j;
	float *ytmp, * yytmp;
	
	ytmp = (float *) calloc(m, sizeof(float));
	yytmp = (float *) calloc(m, sizeof(float)); 
	
	for ( j = 0; j < m; j++){
		splint(x2a,ya[j],y2a[j],n,x2,&yytmp[j]);
	//	printf("%f\n", yytmp[j]);
		}
	spline(x1a, yytmp,m,1.0e30, 1.0e30, ytmp);
			
	for ( j = 0; j < m; j++)
	//	printf("%f %f \n",x1a[m], ytmp[m]);
		
	splint(x1a,yytmp,ytmp,m,x1,y);
	free(ytmp);
	free(yytmp);
	
	
	}



void spline(float x[], float y[], int n, float yp1, float ypn, float y2[])
/*Given arrays x[1..n] and y[1..n] containing a tabulated function, i.e., yi = f(xi), with
x1 <x2 < :: : < xN, and given values yp1 and ypn for the first derivative of the interpolating
function at points 1 and n, respectively, this routine returns an array y2[1..n] that contains
the second derivatives of the interpolating function at the tabulated points xi. If yp1 and/or
ypn are equal to 1 x 10^30 or larger, the routine is signaled to set the corresponding boundary
condition for a natural spline, with zero second derivative on that boundary. */
{
int i,k;
float p,qn,sig,un,*u;


if ( !( u = (float *)calloc (2*n,sizeof (float)) ))
   printf( "calloc failed\n");

//printf("here\n");
if (yp1 > 0.99e30) //The lower boundary condition is set either to be "natural"
y2[0]=u[0]=0.0; 
else {				//or else to have a specified first derivative.
y2[0] = -0.5;
u[0]=(3.0/(x[1]-x[0]))*((y[1]-y[0])/(x[1]-x[0])-yp1);
}
for (i=1;i<=n-2;i++) {		//This is the decomposition loop of the tridiagonal algorithm.	y2 and u are used for temporary
							//storage of the decomposed factors.
sig=(x[i]-x[i-1])/(x[i+1]-x[i-1]);
p=sig*y2[i-1]+2.0;
y2[i]=(sig-1.0)/p;
u[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
u[i]=(6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
}

n = n-1;
if (ypn > 0.99e30)     //The upper boundary condition is set either to be "natural"
qn=un=0.0;				//or else to have a specified first derivative.
else {					

qn=0.5;
un=(3.0/(x[n]-x[n-1]))*(ypn-(y[n]-y[n-1])/(x[n]-x[n-1]));
}
y2[n]=(un-qn*u[n-1])/(qn*y2[n-1]+1.0);
for (k=n;k>=0;k--)		//This is the backsubstitution loop of the tridiagonal algorithm.
y2[k]=y2[k]*y2[k+1]+u[k]; 

//printf("here\n");

free (u);
//printf("here\n");

}


void splint(float xa[], float ya[], float y2a[], int n, float x, float *y)
 /*Given the arrays xa[1..n] and ya[1..n], which tabulate a function (with the xai's in order),
and given the array y2a[1..n], which is the output from spline above, and given a value of
x, this routine returns a cubic-spline interpolated value y. */
{
int klo,khi,k;
float h,b,a;
klo=1 - 1;   /*We will find the right place in the table by means of
		bisection. This is optimal if sequential calls to this
		routine are at random values of x. If sequential calls
		are in order, and closely spaced, one would do better
		to store previous values of klo and khi and test if
		they remain appropriate on the next call. */
khi=n - 1;
while (khi-klo > 1) {
k=(khi+klo) >> 1;
if (xa[k] > x) khi=k;
else klo=k;
}							//klo and khi now bracket the input value of x.
h=xa[khi]-xa[klo];
if (h == 0.0) printf("Bad xa input to routine splint %d %f %d %f\n", khi, xa[khi], klo, xa[klo]);   //The xa's must be distinct. 
a=(xa[khi]-x)/h; 
b=(x-xa[klo])/h;			//Cubic spline polynomial is now evaluated.
*y=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)/6.0;

}


/*	&& (pixel[row*points + col]  < pixel[(row - 1)*points + col + 1] ) &&  (pixel[row*points + col]  < pixel[(row - 1)*points + col + 2] )
					&&	(pixel[row*points + col]  < pixel[(row - 1)*points + col - 1] ) &&  (pixel[row*points + col]  < pixel[(row - 1)*points + col - 2] ) 	
				
					&& (pixel[row*points + col]  < pixel[(row - 2)*points + col + 1] ) &&  (pixel[row*points + col]  < pixel[(row - 2)*points + col + 2] )
					&&	(pixel[row*points + col]  < pixel[(row - 2)*points + col - 1]) &&  (pixel[row*points + col]  < pixel[(row - 2)*points + col - 2]  ) 	
				
					&& (pixel[row*points + col]  < pixel[(row + 1)*points + col + 1] ) &&  (pixel[row*points + col]  < pixel[(row + 1)*points + col + 2] )
					&&	(pixel[row*points + col]  < pixel[(row + 1)*points + col - 1] ) &&  (pixel[row*points + col]  < pixel[(row + 1)*points + col - 2]) 	
					
					&& (pixel[row*points + col]  < pixel[(row + 2)*points + col + 1] ) &&  (pixel[row*points + col]  < pixel[(row + 2)*points + col + 2] )
					&&	(pixel[row*points + col]  < pixel[(row + 2)*points + col - 1] ) &&  (pixel[row*points + col]  < pixel[(row + 2)*points + col - 2]  ) 
					
					&& (pixel[row*points + col]  < pixel[(row + 1)*points + col] ) &&  (pixel[row*points + col]  < pixel[(row + 2)*points + col] )
					&&	(pixel[row*points + col]  < pixel[(row - 1 )*points + col] ) &&  (pixel[row*points + col]  < pixel[(row + 2)*points + col]  ) )				
					*/