/*!\file:  HoleFillerx.cpp
 * \brief  plug missing values of a double* array, using interpolation techniques. This 
 * is an adaptation of Dr. Crippen's routine (Jet Propulsion Lab, see HoleFillerCrippen.c
 * for more details.
 */ 

#include "./HoleFillerx.h"

#include <stdio.h>
#include <time.h>
#include "../shared/shared.h"

int HoleFillerx(double** pimageout,double* image, int lines,int samps,int smooth){
	
	FILE 			*fp1;
	unsigned long	filesize;
	long			infinit;
	double          temp;
	long			i, j, k, l, ii, jj, bytes;
	long			iii, jjj;
	long			count, test, prevlines, prevpix, test1, test2, thresh,counter;
	long			bell, cycles, leftovers;
	float			howlong;
	float			nsteps, ssteps, wsteps, esteps;
	float			nwsteps, nesteps, swsteps, sesteps;
	float			north, south, east, west;
	float			nw, sw, ne, se, elev, range;
	float			nnesteps, enesteps, esesteps, ssesteps;
	float			sswsteps, wswsteps, wnwsteps, nnwsteps;
	float			nne, ene, ese, sse;
	float			ssw, wsw, wnw, nnw;	
	float			sum;
	
	time_t			t1, t2;

	/*output: */
	double*         imageout=NULL;

	double*         image2=NULL;
	double*         image3=NULL;
	double*         image4=NULL;

	/*Added to original routine: */
	int             imageoutsize;

	#ifdef _ISSM_DEBUG_
		if ( smooth == 1 ){
			printf("Data patches will be SMOOTHED. \n");
		}
		if ( smooth == 0 ){ 
			printf("Data patches will NOT be smoothed. \n");
		}
	#endif
	
	 /*^^^^^^^^^^^^^  Remove pixels close to the holes ^^^^^^^^^^^^^*/
	image2 = (double*) xmalloc( lines*samps*sizeof(double));
	memcpy(image2,image,lines*samps*sizeof(double));
	
  	for ( i = 0; i < lines; i++ ){
  		for ( j = 0; j < samps; j++ ){
 			if ( *(image+i*samps+j) == 0 ){ 
  				if ( (j > 3) && (j < samps-4) && (i > 3) && (i < lines-4)){
					for ( k = 0; k < 4; k++ ){
						for ( l = 0; l < 4; l++ ){
							*(image2+samps*(i+k)+j+l)=0;
							*(image2+samps*(i-k)+j+l)=0;
							*(image2+samps*(i+k)+j-l)=0;
							*(image2+samps*(i-k)+j-l)=0;

						}
					}
				}
			}
		}
	}

	image3 = (double*) xmalloc( lines*samps*sizeof(double));
	memcpy(image3,image2,lines*samps*sizeof(double));

	count += 2;
	#ifdef _DEBUG2_
		counter=0;
		for ( i = 0; i < lines; i++ ) {
			for ( j = 0; j < samps; j++ ){
				if ( *(image2+i*samps+j) != 0 ) { 
					counter++;
				}
			}
		}
		printf( "\n" );
		printf("Number of zeroes remaining: %10ld",lines*samps-counter);
		fflush( stdout );
	#endif

	#ifdef _ISSM_DEBUG_
		printf( "\n" );
		printf( "Iterations %5ld   and %5ld.", count-1, count );
		fflush( stdout );
	#endif
	goto afterfirst2;

	again2:	
	
	count += 2;
	
	#ifdef _DEBUG2_
		counter=0;
		for ( i = 0; i < lines; i++ ) {
			for ( j = 0; j < samps; j++ ){
				if ( *(image2+i*samps+j) != 0 ) { 
					counter++;
				}
			}
		}
		//        n u m b e r   o f   z e r o e s   r e m a i n i n g :   1 2 3 4 5 6 7 8 9 10        
		printf( "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b" );
		printf("Number of zeroes remaining: %10ld",lines*samps-counter);
		fflush( stdout );
	#endif

	#ifdef _ISSM_DEBUG_
		printf( "\b\b\b\b\b\b\b\b\b\b\b\b\b\b" );
		printf( "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b" );
		printf( "Iterations %5ld   and %5ld.", count-1, count );
		fflush( stdout );
	#endif
	
	afterfirst2:

/***************** FIRST RUN *********************/
/*
fprintf ( stdout, "First  Application:  " ); time(&t2); printf( ctime(&t2) );
*/
	for ( i = 0; i < lines; i++ ){
		for ( j = 0; j < samps; j++ ){
			if ( *(image2+i*samps+j) != 0 ){ 
				*(image3+samps*i+j) = *(image2+i*samps+j); 
				continue; 
			} 

	/* Count All Neighbors Test */
			test = 0;		
			for ( ii = i-1; ii < i+2; ii++ ){
				for ( jj = j-1; jj < j+2; jj++ ){
					if (( ii < 0 ) || ( ii >= lines )) continue;
					if (( jj < 0 ) || ( jj >= samps )) continue;
					if ( *(image2+ii*samps+jj) != 0 ) test += 1;			
				}
			}
			if ( test < 3 ){ 
				*(image3+samps*i+j) = 0; 
				continue; 
			} 

			/* For void edge pixels: */			
			
			nsteps = 0.0; ssteps = 0.0; esteps = 0.0; wsteps = 0.0;
			nwsteps = 0.0; nesteps = 0.0; swsteps = 0.0; sesteps = 0.0;
			north = 0.0; south = 0.0; east = 0.0; west = 0.0;
			nw = 0.0; ne = 0.0; sw = 0.0; se = 0.0;

			nnesteps = 0.0; enesteps = 0.0; esesteps = 0.0; ssesteps = 0.0;
			sswsteps = 0.0; wswsteps = 0.0; wnwsteps = 0.0; nnwsteps = 0.0;
			nne = 0.0; ene = 0.0; ese = 0.0; sse = 0.0;
			ssw = 0.0; wsw = 0.0; wnw = 0.0; nnw = 0.0;


/** NSEW **/
			for ( ii = i - 1; ii >= 0; ii-- ){   /* North */
				if ( ii <= 0 ) { north = 0; nsteps = 0; break;}
				if ( *(image2+ii*samps+j) != 0 ){ north = *(image2+ii*samps+j); nsteps = i - ii; break;}
			}

			for ( ii = i + 1; ii < lines; ii++ ){ /* South */ 
				if ( ii >= lines-1 ){ south = 0; ssteps = 0; break;}
				if ( *(image2+ii*samps+j) != 0 ){ south = *(image2+ii*samps+j); ssteps = ii - i; break;}
			}
				
			for ( jj = j - 1; jj >= 0; jj-- ){   /* West */
				if ( jj <= 0 ) { west = 0; wsteps = 0; break;}
				if ( *(image2+i*samps+jj) != 0 ){ west = *(image2+i*samps+jj); wsteps = j - jj; break;}
			}
				
			for ( jj = j + 1; jj < samps; jj++ ){ /* East */
				if ( jj >= samps-1 ){ east = 0; esteps = 0; break;}
				if ( *(image2+i*samps+jj) != 0 ){ east = *(image2+i*samps+jj); esteps = jj - j; break;}
			}

				
/** Diagonals  **/						
			/* Southeast */
			for ( k = 1; k < infinit; k++ ){
				ii = i + k; jj = j + k;
				if (( ii == lines ) || ( jj == samps )){ se = 0; sesteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ se = *(image2+ii*samps+jj); sesteps = 1.4142 * k; break;}
			}
			
			/* Northeast */
			for ( k = 1; k < infinit; k++ ){
				ii = i - k; jj = j + k;
				if (( ii == -1 ) || ( jj == samps )){ ne = 0; nesteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ ne = *(image2+ii*samps+jj); nesteps = 1.4142 * k; break;}
			}
			
			/* Northwest */
			for ( k = 1; k < infinit; k++ ){
				ii = i - k; jj = j - k;
				if (( ii == -1 ) || ( jj == -1 )){ nw = 0; nwsteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ nw = *(image2+ii*samps+jj); nwsteps = 1.4142 * k; break;}
			}
			
			/* Southwest */
			for ( k = 1; k < infinit; k++ ){
				ii = i + k; jj = j - k;
				if (( ii == lines ) || ( jj == -1 )){ sw = 0; swsteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ sw = *(image2+ii*samps+jj); swsteps = 1.4142 * k; break;}
			}

/** Other 8 **/
			/* NNE */
			for ( k = 1; k < infinit; k++ ){
				ii = i - (k*2); jj = j + k;
				if (( ii <= -1 ) || ( jj >= samps )){ nne = 0; nnesteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ nne = *(image2+ii*samps+jj); nnesteps = 2.2361 * k; break;}
			}
			
			/* ENE */
			for ( k = 1; k < infinit; k++ ){
				ii = i - k; jj = j + (k*2);
				if (( ii <= -1 ) || ( jj >= samps )){ ene = 0; enesteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ ene = *(image2+ii*samps+jj); enesteps = 2.2361 * k; break;}
			}
			
			/* ESE */
			for ( k = 1; k < infinit; k++ ){
				ii = i + k; jj = j + (k*2);
				if (( ii >= lines ) || ( jj >= samps )){ ese = 0; esesteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ ese = *(image2+ii*samps+jj); esesteps = 2.2361 * k; break;}
			}
			
			/* SSE */
			for ( k = 1; k < infinit; k++ ){
				ii = i + (k*2); jj = j + k;
				if (( ii >= lines ) || ( jj >= samps )){ sse = 0; ssesteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ sse = *(image2+ii*samps+jj); ssesteps = 2.2361 * k; break;}
			}

			/* SSW */
			for ( k = 1; k < infinit; k++ ){
				ii = i + (k*2); jj = j - k;
				if (( ii >= lines ) || ( jj <= -1 )){ ssw = 0; sswsteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ ssw = *(image2+ii*samps+jj); sswsteps = 2.2361 * k; break;}
			}
			
			/* WSW */
			for ( k = 1; k < infinit; k++ ){
				ii = i + k; jj = j - (k*2);
				if (( ii >= lines ) || ( jj <= -1 )){ wsw = 0; wswsteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ wsw = *(image2+ii*samps+jj); wswsteps = 2.2361 * k; break;}
			}
			
			/* WNW */
			for ( k = 1; k < infinit; k++ ){
				ii = i - k; jj = j - (k*2);
				if (( ii <= -1 ) || ( jj <= -1 )){ wnw = 0; wnwsteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ wnw = *(image2+ii*samps+jj); wnwsteps = 2.2361 * k; break;}
			}
			
			/* NNW */
			for ( k = 1; k < infinit; k++ ){
				ii = i - (k*2); jj = j - k;
				if (( ii <= -1 ) || ( jj <= -1 )){ nnw = 0; nnwsteps = 0; break;}
				if ( *(image2+ii*samps+jj) != 0 ){ nnw = *(image2+ii*samps+jj); nnwsteps = 2.2361 * k; break;}
			}
			
			elev = 0; range = 0;
/*NSEW*/			
			if ( nsteps > 0.5 ){ elev += north /  nsteps; range +=  1.00 /  nsteps;}
			if ( ssteps > 0.5 ){ elev += south /  ssteps; range +=  1.00 /  ssteps;}
			if ( esteps > 0.5 ){ elev +=  east /  esteps; range +=  1.00 /  esteps;}
			if ( wsteps > 0.5 ){ elev +=  west /  wsteps; range +=  1.00 /  wsteps;}
/*Diag*/
			if ( nwsteps > 0.5 ){ elev +=    nw / nwsteps; range +=  1.00 / nwsteps;}
			if ( nesteps > 0.5 ){ elev +=    ne / nesteps; range +=  1.00 / nesteps;}
			if ( swsteps > 0.5 ){ elev +=    sw / swsteps; range +=  1.00 / swsteps;}
			if ( sesteps > 0.5 ){ elev +=    se / sesteps; range +=  1.00 / sesteps;}
			
/*Other 8*/
			if ( nnesteps > 0.5 ){ elev += nne / nnesteps; range +=  1.00 / nnesteps;}
			if ( enesteps > 0.5 ){ elev += ene / enesteps; range +=  1.00 / enesteps;}
			if ( esesteps > 0.5 ){ elev += ese / esesteps; range +=  1.00 / esesteps;}
			if ( ssesteps > 0.5 ){ elev += sse / ssesteps; range +=  1.00 / ssesteps;}
			if ( sswsteps > 0.5 ){ elev += ssw / sswsteps; range +=  1.00 / sswsteps;}
			if ( wswsteps > 0.5 ){ elev += wsw / wswsteps; range +=  1.00 / wswsteps;}
			if ( wnwsteps > 0.5 ){ elev += wnw / wnwsteps; range +=  1.00 / wnwsteps;}
			if ( nnwsteps > 0.5 ){ elev += nnw / nnwsteps; range +=  1.00 / nnwsteps;}
									
			//temp = ( elev / range ) + 0.5 ;
			temp = ( elev / range );
			//+ 0.5 ;
			//if ( temp > 10000 ) temp = 10000;
			//if ( temp <     0 ) temp =     0;
			
			#ifdef _DEBUG2_
				//printf("%g %g %g \n",temp,elev,range);
			#endif

			*(image3+samps*i+j) = temp ;			
		}
	}



	for ( i = 0; i < lines; i++ ){
		for ( j = 0; j < samps; j++ ){
			*(image2+samps*i+j)= *(image3+samps*i+j);
		}
	}


	for ( i = 0; i < lines; i++ ){
		for ( j = 0; j < samps; j++ ){
			if ( *(image2+samps*i+j) == 0 ) goto again2;
		}
	}
	
	if ( smooth == 0 ) goto there2;		
		
		
/************************ SMOOTH THE RESULT ***********************/		
		
	image4 = (double*) xmalloc( lines*samps*sizeof(double));
	memcpy(image4,image3,lines*samps*sizeof(double));


	for ( i = 0; i < lines; i++ ) {
		for ( j = 0; j < samps; j++ ) {
			if ( *(image4+i*samps+j) != 0 ) { *(image3+i*samps+j) = *(image2+i*samps+j) ; continue; }


			for ( k = 1; k < infinit; k++ ) { /* Find the smallest box size with data */ 
				for ( ii = i-k; ii <= i+k; ii++ ){
					if ( ii <      0 ) continue;
					if ( ii >= lines ) break;
					for ( jj = j-k; jj <= j+k; jj++ ) {
						if ( jj <      0 ) continue;
						if ( jj >= samps ) continue;
						if ( *(image4+ii*samps+jj) != 0 ) goto k_nowset;
					}
				}
			}
		
			k_nowset:
			k = k / 4; if ( k < 1 ) k = 1; /* Errrrr.  Make it fourth size */
			sum = 0;
			for ( ii = i-k; ii <= i+k; ii++ ) {
				if ( ii <      0 ) iii = -1 - ii; else iii = ii;
				if ( ii >= lines ) iii = lines - (ii - lines + 1);
				for ( jj = j-k; jj <= j+k; jj++ ) {
					if ( jj <      0 ) jjj = -1 - jj; else jjj = jj;
					if ( jj >= samps ) jjj = samps - (jj - samps + 1);
					sum += *(image2+samps*iii+jjj) ;
				}
			}
			k = (k*2)+1;
			*(image3+samps*i+j) = ( sum / k / k ) ;//+ 0.5;
		}

		#ifdef _ISSM_DEBUG_
			if ( !(i%100) ){
			fprintf(stdout, "\b\b\b\b\b\b%5ld ", i );   /* Count lines on output screen */
			fflush(stdout);	
			}
		#endif
	}

	#ifdef _ISSM_DEBUG_
		fprintf(stdout, "\b\b\b\b\b\b%5ld ", lines );
		fflush(stdout);	
	#endif
	
		
there2:		


	/*Allocate output image: */
	imageout=(double*)xmalloc(samps*lines*sizeof(double));
	memcpy(imageout,image3,lines*samps*sizeof(double));

	time(&t2);	
	#ifdef _ISSM_DEBUG_
		printf ( "\n\nEnd                  "); printf( ctime(&t2) );
		printf( "\n                           Total time:        ");
		printf( "%4.2f minutes. \n", difftime(t2,t1)/60 );		
	#endif
	
	#ifdef _DEBUG2_
		printf( "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b" );
		printf("Number of zeroes remaining:          0\n\n");
		printf ( "\n");
	#endif
	
	end:

	#ifdef _ISSM_DEBUG_
		printf( "Done.\a\n" );
	#endif

	/*Assign output pointers: */
	*pimageout=imageout;

	return 1;
	
}