#include "p1640.h"

void changedir(char * object, char * date);
void freestuff(int reads,int size);
void getmemory();
char * makecuberoot(char * object, char* date);
char ** makedatasetlist( char *root, int num, int reads);
void magnify1(float *image, int rows, int cols);
float *magnify2andshift(float *y, float * ms, int rows, int cols, float magx, float magy, float xs, float ys); 
FILE * makefilelist(char * object, char * date);
float  sd_of_difference(float *tmpimage,float* tmptarget,int rowstart,int rowend,int colstart,int colend);
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);
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);

float *x1, *x2;// **ys;
float *yy1, *ys1, *YS1, *cys1, *cyy1;
float *ms, *ml, *mtmp; 
float **yb1, **y2;
float *minarray;
float *slush;
char **file;
float *tmpshort, *tmptarget, *tmplong, *tmpimage;
float  *icube, *modshort; 
char ** datasetlist, *cuberoot;

int main (int argc, const char * argv[]) 
{ 
		int slice, rings, ring, rowstart, rowend, colstart, colend;
    int magx;
	int slicedelta, width, firstset, lastset,reads ;
	int i, j, quad, firstslice,lastslice;
    int innerradius, outerradius;
    
	float xshift, yshift, sd;
  	
	if (argc != 8)
	{
		printf("input object, date, ring separation, ring width, number of first and last dataset, and number of reads on the command line\n");
		return(0);
	}
	slicedelta = atoi(argv[3]);
	width = atoi(argv[4]);
	firstset = atoi(argv[5]);
	lastset = atoi(argv[6]);
	reads = atoi(argv[7]);
    
	firstslice = slicedelta;
	lastslice = waves - slicedelta;
    rings = (int)(reduced_size - 20)/(2*width);
    
    modshort = (float *) calloc(imagesize, sizeof(float));
    tmpshort = (float *) calloc(reduced_image, sizeof(float));
  //  for (i = 0; i < reduced_size; i ++ )
  //      tmpshort[i] = (float *) calloc(reduced_image, sizeof(float));
    tmptarget = (float *) calloc(imagesize, sizeof(float));
    tmplong = (float *) calloc(imagesize, sizeof(float));  
    if ( (file =(char **)calloc(reads - 1, sizeof(char*)))== NULL)
		printf("No memory for filenames\n");
        for( i = 0; i < reads; i++)
            if ( (file[i] =(char *)calloc(100, sizeof(char)))== NULL)
                printf("No memory for filenames\n");

    
    tmpimage = (float *) calloc(imagesize, sizeof(float));    
    cuberoot = makecuberoot((char*)argv[1],(char*)argv[2]); 

	changedir((char*)argv[1],(char*)argv[2]);	
    getmemory();
    for (row = 0; row < reduced_size; row += 1)
    {
        x2[row] = (float) row;
        x1[row] = (float) row;
    
    }

   //used as output
    system ( "rm -f magtest.fits");
	system ( "rm -f magtest2.fits");
	for (i = firstset; i <= lastset; i+=40 )
	{	 
        datasetlist = 	makedatasetlist(cuberoot, i, reads); 
        for (j = 0; j < reads - 1; j+=40)
        {
            printf("%s\n", datasetlist[j]);
            if ( (icube = load_simple_fits_float_data(datasetlist[j], &naxis, naxes, &ndatas) ) == NULL) 
            {
                (void) stderr, fprintf(stderr, "main: load_simple_fits_float_data %s failed\n", datasetlist[j]);
                return 0; 
            }
            naxis = 2;
            // GET FIRST SET OF SPLINE PARAMETERS FOR ALL SLICES magnify1 replaces splie2
            for (slice = 0; slice < waves; slice++)
            {
                memcpy(tmpimage, icube + slice*imagesize, sizeof(float)*imagesize);
                magnify1(tmpimage,reduced_size ,reduced_size);
                memcpy(cyy1 + slice*reduced_image, yy1, sizeof(float)* reduced_image); //images
                memcpy(cys1 + slice*reduced_image, YS1, sizeof(float)* reduced_image); //parameters
            }
            //    naxes[0] = naxes[1] = reduced_size;
            //  write_simple_fits_float_data("magtest.fits", naxis, naxes,cys1 + 3*reduced_image);

            
            //FOR SLICE THEN RING THEN QUAD THEN SHIFT THEN MAGNIFY THEN MULITPLY
            
            system ("date \"+TIME:%H:%M:%S\"");
            for (slice = firstslice+10; slice <= lastslice; slice +=40)
            {
                memcpy(tmpshort, cyy1 + (slice - slicedelta)*reduced_image, sizeof(float)*reduced_image);
                memcpy(tmptarget, cyy1 + slice*imagesize, sizeof(float)*reduced_image);
                memcpy(tmplong, cyy1 + (slice + slicedelta)*reduced_image, sizeof(float)*reduced_image);
                
                // for tmpshort
                for (ring = 0; ring < rings; ring +=15)
                {     
                   for (i = 0; i < 16; i++)
                        minarray[i] = 1e8;
                    
                    for (magx = reduced_size; magx <= reduced_size + 4; magx ++)
                         { 
                         xshift = yshift = 0;
                        //{ //  printf("%d %f %f\n", magx, xshift,yshift);
                        tmpimage = magnify2andshift(tmpshort, cys1 + slice*reduced_image, reduced_size ,reduced_size, (float) magx/reduced_size, (float) magx/reduced_size, xshift, yshift);  // magnified image
                                    //    naxes[0] = magx;
                                    //    naxes[1] = magx;
                                    //    write_simple_fits_float_data("magtest2.fits", naxis, naxes,tmpimage);
                                        innerradius = 15 + ring*width;
                                        outerradius = innerradius + width;
                                        for (quad = 0; quad < 4; quad++)
                                        {
                                            switch(quad)
                                            {
                                                case 0:  //upper right
                                                {
                                                    rowstart = colstart = innerradius + red_ctr;
                                                    rowend = colend = outerradius + red_ctr;
                                                    sd = sd_of_difference(tmpimage, tmptarget,rowstart, rowend, colstart, colend);
                                              
                                                    break;
                                                 }   
                                                case 1: //upper left
                                                {
                                                    rowstart = innerradius + red_ctr;
                                                    rowend = outerradius + red_ctr;
                                                    colstart = -outerradius + red_ctr;
                                                    colend = -innerradius + red_ctr;
                                                    sd = sd_of_difference(tmpimage, tmptarget,rowstart, rowend, colstart, colend);

                                                    break;
                                                 }  
                                                case 2: //lower left
                                                {
                                                    rowstart = colstart = -outerradius + red_ctr;
                                                    rowend = colend = -innerradius + red_ctr;
                                                    sd = sd_of_difference(tmpimage, tmptarget,rowstart, rowend, colstart, colend);

                                                    break;
                                                 }  
                                                case 3: // lower right
                                                {   
                                                    rowstart = -outerradius + red_ctr;
                                                    rowend = -innerradius + red_ctr;
                                                    colstart = innerradius + red_ctr;
                                                    colend = outerradius + red_ctr;
                                                    sd = sd_of_difference(tmpimage, tmptarget,rowstart, rowend, colstart, colend);

                                                    break;
                                                 }  
                                            }  // end switch
                                        if (sd < minarray[quad*4])
                                            {   minarray[quad*4] = sd;
                                                minarray[quad*4 + 1] = magx;
                                                minarray[quad*4 + 2] = xshift;
                                                minarray[quad*4 + 3] = yshift;

                                            } 
                                          
                                   
                                        } // end quad
                         //   printf("%d %d %d %f %f %f %f \n",slice, ring,  quad, minarray[1], minarray[5], minarray[9], minarray[13]);  
    
                            } //end mag  
                                       
                        for(xshift = -5.0; xshift <= 5.0; xshift += 0.25)
                            for(yshift = -5.; yshift <= 5.0; yshift += 0.25)  
                                { //printf("%d %f %f\n", magx, xshift, yshift);
                                    innerradius = 15 + ring*width;
                                    outerradius = innerradius + width;
                                    for (quad = 0; quad < 4; quad++)
                                        {
                                            switch(quad)
                                            {
                                                case 0:  //upper right
                                                {
                                                    magx = minarray[quad*4 + 1];
                                                    tmpimage = magnify2andshift(tmpshort, cys1 + slice*reduced_image, reduced_size ,reduced_size, (float) magx/reduced_size, (float) magx/reduced_size, xshift, yshift);  // magnified image
                                                    rowstart = colstart = innerradius + red_ctr;
                                                    rowend = colend = outerradius + red_ctr;
                                                    sd = sd_of_difference(tmpimage, tmptarget,rowstart, rowend, colstart, colend);
                                                
                                                    break;
                                                 }   
                                                case 1: //upper left
                                                {
                                                     magx = minarray[quad*4 + 1];
                                                    tmpimage = magnify2andshift(tmpshort, cys1 + slice*reduced_image, reduced_size ,reduced_size, (float) magx/reduced_size, (float) magx/reduced_size, xshift, yshift);  // magnified image
                                                   rowstart = innerradius + red_ctr;
                                                    rowend = outerradius + red_ctr;
                                                    colstart = -outerradius + red_ctr;
                                                    colend = -innerradius + red_ctr;
                                                    sd = sd_of_difference(tmpimage, tmptarget,rowstart, rowend, colstart, colend);

                                                    break;
                                                 }  
                                                case 2: //lower left
                                                {
                                                     magx = minarray[quad*4 + 1];
                                                    tmpimage = magnify2andshift(tmpshort, cys1 + slice*reduced_image, reduced_size ,reduced_size, (float) magx/reduced_size, (float) magx/reduced_size, xshift, yshift);  // magnified image
                                                   rowstart = colstart = -outerradius + red_ctr;
                                                    rowend = colend = -innerradius + red_ctr;
                                                    sd = sd_of_difference(tmpimage, tmptarget,rowstart, rowend, colstart, colend);

                                                    break;
                                                 }  
                                                case 3: // lower right
                                                {   
                                                     magx = minarray[quad*4 + 1];
                                                    tmpimage = magnify2andshift(tmpshort, cys1 + slice*reduced_image, reduced_size ,reduced_size, (float) magx/reduced_size, (float) magx/reduced_size, xshift, yshift);  // magnified image
                                                    rowstart = -outerradius + red_ctr;
                                                    rowend = -innerradius + red_ctr;
                                                    colstart = innerradius + red_ctr;
                                                    colend = outerradius + red_ctr;
                                                    sd = sd_of_difference(tmpimage, tmptarget,rowstart, rowend, colstart, colend);

                                                    break;
                                                 }  
                                            }  // end switch
                                        if (sd < minarray[quad*4])
                                            {   minarray[quad*4] = sd;
                                                minarray[quad*4 + 1] = magx;
                                                minarray[quad*4 + 2] = xshift;
                                                minarray[quad*4 + 3] = yshift;
                                            } 
                                          
                                   
                                                     
                                        } // end quad
                                        
                                   } // end  shift   
                                                   // for (quad = 0; quad < 4; quad++)
                                     //    printf(" %d %f %f %f %f \n", quad, minarray[quad*4], minarray[quad*4 + 1], minarray[quad*4 + 2], minarray[quad*4 + 3]);          
                // set ranges  //
                // naxes[0] = naxes[1] = 176;
                           //             write_simple_fits_float_data("magtest.fits", naxis, naxes,tmpimage);
             //  
                     
                } // end for ring short 
                                                          
                        
                //for templong
                } //end of slice
        } // end j
 

    } // end set
     for(quad = 0; quad < 4; quad ++)
       printf(" %d %f %f %f %f \n", quad, minarray[quad*4], minarray[quad*4 + 1], minarray[quad*4 + 2], minarray[quad*4 + 3]);  
    if (quad == 4)    
    system ("date \"+TIME:%H:%M:%S\"");

	freestuff(reads, reduced_size);
    return(0);
}


float  sd_of_difference(float *tmpimage,float* tmptarget,int rowstart,int rowend,int colstart,int colend)
{
    float sum = 0, sum2 = 0, var, diff;
  
    int irad, orad ,radius, r2, i = 0;
    irad = rowstart*rowstart;
    orad = rowend*rowend;
    
      for (row = 0; row <= rowend; row++)
        {
            r2 = row*row;
            for (col = 0; col <= colend; col++)
                {   i++;
                   radius = r2 + col*col;
                    if  ( ( radius > irad)  && (radius <= orad))
                        {
                        diff = (tmptarget[row*reduced_size + col]  -  tmpimage[row*reduced_size + col]);
                        sum  += diff;
                        sum2 += diff*diff;
                        }
                }
        }
        
  var = sum2/i - sum*sum/(i*i);      
         
return (var);

}



float * magnify2andshift (float *y, float * ms, int rows, int cols, float magx, float magy, float xs, float ys)
{
   int r,c;
    float frow, fcol, result;
    float xmag, ymag;
    
    xmag = (float)1/magx;
    ymag = (float)1/magy;

       
   for ( row = 0; row < rows; row++)
        for (col = 0; col < cols; col++)
            {
            yb1[row][col] = y[row*cols +col];
            y2[row][col] = ms[row*cols +col];
            }
    
    
    //for ( row = 0; row < rows; row++)
    //{
    //    memcpy(&yb1[row], &y[row*rows], sizeof(float)*rows);
    //    memcpy(&y2[row], &ms[row*rows], sizeof(float)*rows);
    //}
    
    
    r = 0;
    for (frow = 0; frow < rows; frow += ymag, r++)
    { 
        c = 0;
        for (fcol = 0; fcol < cols; fcol += xmag, c++)  
        {   
            splin2(x1,x2,yb1,y2,rows,cols,frow+ys,fcol + xs, &result); // x1 not used
            mtmp[(int)(r*rows*magy) + c] = result; 
        }
    }
       	
	return(mtmp);
}

void magnify1(float *image, int rows, int cols)
// make reduced cube and calculate spline parameters

{
    
//calloc for 1 D
//splie2 produces an arr of 2 der.	    
for (row = 0; row < rows; row++)
    x1[row] = (float) row;
    
    for (row = 0; row < rows; row++)
    {
        memcpy(yy1 + row*reduced_size, image + (row + 37)*lenslets_size + 37, reduced_size*sizeof(float));	
            {
                spline( x1, yy1 + row*reduced_size,rows,0,0, ys1);
            }                   //make the ys1 array 
    memcpy(YS1+row*reduced_size, ys1, sizeof(float)*reduced_size);
    }
            
   
}






void getmemory()
{   
  int i; 
    
      if ((slush = (float *) calloc((19),sizeof(float))) == NULL)
        {
            printf("no slush memory\n");
            exit(0);
        }

      
      
      if ((x1 = (float *)calloc(reduced_size,sizeof(float))) == NULL)
        {
            printf("no x1 memory\n");
            exit(0);
        }
    if ((x2 = (float *)calloc(reduced_size,sizeof(float))) == NULL)
        {
            printf("no x2 memory\n");
            exit(0);
        }
	if ((yy1 = (float *)calloc(reduced_image*2,sizeof(float))) == NULL)
        {
            printf("no yy1 memory\n");
            exit(0);
        }
    if ((ys1 = (float *)calloc(reduced_size*2 ,sizeof(float))) == NULL)
        {
            printf("no ys1 memory\n");
            exit(0);
        }
    if ((YS1 = (float *)calloc(reduced_image *2,sizeof(float))) == NULL)
        {
            printf("no YS1 memory\n");
            exit(0);
        }
    if ((cys1 = (float *)calloc(waves*reduced_image ,sizeof(float))) == NULL)
        {
            printf("no cys1 memory\n");
            exit(0);
        }

    if ((cyy1 = (float *)calloc(waves*reduced_image,sizeof(float))) == NULL)
        {
            printf("no cys1 memory\n");
            exit(0);
        }
        
     if ((ms = (float*) calloc(reduced_image,sizeof(float))) == NULL)
        if ((ml = (float*) calloc(reduced_image,sizeof(float))) == NULL)
        {
          
            printf("no ml memory\n");
            exit(0);
        }
    if ((mtmp = (float*) calloc(reduced_image* 2,sizeof(float))) == NULL)
        {
            printf("no mtmp memory\n");
            exit(0);
                    }

    if ((yb1 = (float**) calloc(reduced_size,sizeof(float*))) == NULL)
        {
                printf("no yb1 memory\n");
                exit(0);
            }

      if ((y2 = (float**) calloc(reduced_size,sizeof(float*))) == NULL)
            {
                printf("no y2a memory\n");
                exit(0);
            }

       for (i = 0; i < reduced_size; i++)
        {
            if ((y2[i] = (float*) calloc(reduced_size,sizeof(float))) == NULL)
            {
                printf("no y2i memory\n");
                exit(0);
            }
            if ((yb1[i] = (float*) calloc(reduced_size,sizeof(float))) == NULL)
            {
                printf("no yb1i memory\n");
                exit(0);
            }

        }
         
         minarray = (float*) calloc(16,sizeof(float));
         
         
       

    

}
    
void splint(float xa[], float ya[], float y2a[], int n, float x, float *y)
{
	int klo,khi,k;
	float h,b,a;
	klo=1 - 1;  
    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.
 //   printf("%d %d %f\n", klo, khi, y2a[klo]);
	*y=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)/6.0;
   
	
}

void spline(float x[], float y[], int n, float yp1, float ypn, float y2[])
{
	int i,k;
	float p,qn,sig,un,*u;

	if ( !( u = (float *)calloc (2*n,sizeof (float)) ))
		printf( "calloc failed\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);
	
	
}

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]);
        }
	spline(x1a, yytmp,m,1.0e30, 1.0e30, ytmp);
	
    splint(x1a,yytmp,ytmp,m,x1,y);
	free(ytmp);
	free(yytmp);
	
	
}



 char *cube;
char ** makedatasetlist( char *root, int num, int reads)
{ 
    char  snum[5];
    
    int i;
    
    if ( (cube =(char *)calloc(200, sizeof(char)))== NULL)
		printf("No memory for cube\n");

        strcpy(cube,root);
        sprintf(snum,"%03d", num);
        strcat(cube,snum);
        strcat(cube,"_f");
    
    for (i = 0; i < reads - 1; i ++)
    {   
        strcpy(file[i],cube);
        sprintf(snum,"%03d", i+1);
        
        strcat(file[i],snum);
        strcat(file[i],".fits");
       // printf("%s\n", file[i]);
    }
    
    return file;
}    

										
char * cuberoot;										
										
char * makecuberoot(char * object, char* date)
{
	char * cuberoot;
	
	if ( (cuberoot =(char *)calloc(200, sizeof(char)))== NULL)
		printf("No memory for cube\n");
	
	strcpy(cuberoot, object);
	strcat(cuberoot,"_O_");
	strcat(cuberoot,date);
	strcat(cuberoot,"_");
	
	return cuberoot;

	
}



 char *datadir;

void changedir(char * object, char * date)
{
   
	printf("changeing directory\n");

	if ( (datadir =(char *)calloc(200, sizeof(char)))== NULL)
		printf("No memory for datadir\n");
		
	
	strcpy(datadir, processed_data);
	strcat(datadir,object);
	strcat(datadir,"/");
	strcat(datadir,date);
	strcat(datadir,"/");
	strcat(datadir,"Series");
	
    chdir(datadir);
    getwd(datadir);
	printf("%s\n", datadir);


}


FILE * makefilelist(char * object, char * date)
{
	char *datadir, *cmd;
	FILE *fp;
	
	if ( (datadir =(char *)calloc(200, sizeof(char)))== NULL)
		printf("No memory for datadir\n");
	
	if ( (cmd =(char *)calloc(200, sizeof(char)))== NULL)
		printf("No memory for cmd\n");
	
	
	strcpy(datadir, processed_data);
	strcat(datadir,object);
	strcat(datadir,"/");
	strcat(datadir,date);
	strcat(datadir,"/");
	strcat(datadir,"Series");
	
    chdir(datadir);
	//getwd(datadir);
	//printf("%s\n", datadir);
	
	strcpy(cmd, "ls ");
	//strcat(cmd, datadir);
	strcat(cmd, "*.fits ");
	strcat(cmd, "> filelist.lis");
	system(cmd); 
		
	//strcpy(datadir,"filelist.lis");
	

    if ( (fp = fopen("filelist.lis", "r")) == NULL)
		printf("Couldn't open filelist.lis\n");
			
    free(datadir);
	free(cmd);
	
	return (fp);

}
						
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 freestuff(int reads, int size)
{
    int i;
    
    free(icube);
    free(ms);
    free(minarray);
    free(modshort);
    free(tmpshort);
    free(tmptarget);
    free(tmplong);  
    free(tmpimage); 
    free(x1);
    free(x2);
    free(yy1);
    free(ys1);
    free(YS1);
    free(cys1); 
    free(cyy1);
    free(ms); 
    free(ml);
    free(mtmp);
    free(minarray);
    free(slush); 
    for ( i = 0; i < reads; i++)
        free (file[i]);
      free (file);  
    
     for (i = 0; i < reduced_size; i++)
        {
          free(y2[i]); 
          free(yb1[i]);
        }
    free(yb1);
    free(y2);
free (cube);
  free (datadir);
  free(cuberoot);
}
