////////////////////////////////////////////////////////////////
//
//			Green Equilibration via directional average
//
//	copyright (c) 2008-2010  Emil Martinec <ejmartin@uchicago.edu>
//
//
// code dated: February 12, 2011
//
//	green_equil_RT.cc is free software: you can redistribute it and/or modify
//	it under the terms of the GNU General Public License as published by
//	the Free Software Foundation, either version 3 of the License, or
//	(at your option) any later version.
//
//	This program is distributed in the hope that it will be useful,
//	but WITHOUT ANY WARRANTY; without even the implied warranty of
//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//	GNU General Public License for more details.
//
//	You should have received a copy of the GNU General Public License
//	along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
////////////////////////////////////////////////////////////////
#define TS 256	 // Tile size

#include <math.h>
#include <stdlib.h>
#include <time.h>


#define SQR(x) ((x)*(x))


//void green_equilibrate()//for dcraw implementation
void RawImageSource::green_equilibrate(float thresh)
{  
	// thresh = threshold for performing green equilibration; max percentage difference of G1 vs G2  
	// G1-G2 differences larger than this will be assumed to be Nyquist texture, and left untouched
	
	int height=H, width=W; 

	// local variables
	array2D<float> cfa (width,height,rawData);
	array2D<int> checker (width,height,ARRAY2D_CLEAR_DATA);
	
	
	//int verbose=1;
	
	static const float eps=1.0;	//tolerance to avoid dividing by zero
	
	static const float diffthresh=0.25; //threshold for texture, not to be equilibrated
	
	
	// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	
	// Fill G interpolated values with border interpolation and input values
	
	int rr, cc;
	int vote1, vote2;
	int counter, vtest;
	
	float gin, gse, gsw, gne, gnw, wtse, wtsw, wtne, wtnw;
	float ginterp;
	
	float d1,d2,c1,c2;  
	float o1_1,o1_2,o1_3,o1_4;  
	float o2_1,o2_2,o2_3,o2_4;
	
	
	//The green equilibration algorithm starts here
#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (rr=1; rr < height-1; rr++)
		for (cc=3-(FC(rr,2)&1); cc < width-2; cc+=2) {
			
			float pcorr = (cfa[rr+1][cc+1]-cfa[rr][cc])*(cfa[rr-1][cc-1]-cfa[rr][cc]);
			float mcorr = (cfa[rr-1][cc+1]-cfa[rr][cc])*(cfa[rr+1][cc-1]-cfa[rr][cc]);
			
			if (pcorr>0 && mcorr>0) {checker[rr][cc]=1;} else {checker[rr][cc]=0;}
			
			checker[rr][cc]=1;//test what happens if we always interpolate
		}
	
	counter=vtest=0;
	
	//now smooth the cfa data
#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (rr=4; rr < height-4; rr++)
		for (cc=5-(FC(rr,2)&1); cc < width-6; cc+=2) {
			if (checker[rr][cc]) {
				//%%%%%%%%%%%%%%%%%%%%%%
				//neighbor checking code from Manuel Llorens Garcia
				o1_1=cfa[(rr-1)][cc-1]; 
				o1_2=cfa[(rr-1)][cc+1];  
				o1_3=cfa[(rr+1)][cc-1]; 
				o1_4=cfa[(rr+1)][cc+1];  
				o2_1=cfa[(rr-2)][cc];  
				o2_2=cfa[(rr+2)][cc];  
				o2_3=cfa[(rr)][cc-2];  
				o2_4=cfa[(rr)][cc+2];  
				
				d1=(o1_1+o1_2+o1_3+o1_4)/4.0;  
				d2=(o2_1+o2_2+o2_3+o2_4)/4.0;  
				
				c1=(fabs(o1_1-o1_2)+fabs(o1_1-o1_3)+fabs(o1_1-o1_4)+fabs(o1_2-o1_3)+fabs(o1_3-o1_4)+fabs(o1_2-o1_4))/6.0;  
				c2=(fabs(o2_1-o2_2)+fabs(o2_1-o2_3)+fabs(o2_1-o2_4)+fabs(o2_2-o2_3)+fabs(o2_3-o2_4)+fabs(o2_2-o2_4))/6.0;
				//%%%%%%%%%%%%%%%%%%%%%%
				
				vote1=(checker[rr-2][cc]+checker[rr][cc-2]+checker[rr][cc+2]+checker[rr+2][cc]);
				vote2=(checker[rr+1][cc-1]+checker[rr+1][cc+1]+checker[rr-1][cc-1]+checker[rr-1][cc+1]);
				//if ((vote1==0 || vote2==0) && (c1+c2)<2*thresh*fabs(d1-d2)) vtest++;
				if (vote1>0 && vote2>0 && (c1+c2)<4*thresh*fabs(d1-d2)) {
					//pixel interpolation
					gin=cfa[rr][cc];
					
					gse=(cfa[rr+1][cc+1])+0.5*(cfa[rr][cc]-cfa[rr+2][cc+2]);
					gnw=(cfa[rr-1][cc-1])+0.5*(cfa[rr][cc]-cfa[rr-2][cc-2]);
					gne=(cfa[rr-1][cc+1])+0.5*(cfa[rr][cc]-cfa[rr-2][cc+2]);
					gsw=(cfa[rr+1][cc-1])+0.5*(cfa[rr][cc]-cfa[rr+2][cc-2]);
					
					
					
					wtse=1/(eps+SQR(cfa[rr+2][cc+2]-cfa[rr][cc])+SQR(cfa[rr+3][cc+3]-cfa[rr+1][cc+1]));
					wtnw=1/(eps+SQR(cfa[rr-2][cc-2]-cfa[rr][cc])+SQR(cfa[rr-3][cc-3]-cfa[rr-1][cc-1]));
					wtne=1/(eps+SQR(cfa[rr-2][cc+2]-cfa[rr][cc])+SQR(cfa[rr-3][cc+3]-cfa[rr-1][cc+1]));
					wtsw=1/(eps+SQR(cfa[rr+2][cc-2]-cfa[rr][cc])+SQR(cfa[rr+3][cc-3]-cfa[rr+1][cc-1]));
					
					ginterp=(gse*wtse+gnw*wtnw+gne*wtne+gsw*wtsw)/(wtse+wtnw+wtne+wtsw);
					
					if ( ((ginterp-gin) < thresh*(ginterp+gin)) ) {
						rawData[rr][cc]=0.5*(ginterp+gin);
						counter++;
					}
					
				}
			}
		}
	//printf("pixfix count= %d; vtest= %d \n",counter,vtest);
	// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	
	
	
	
	// done
	/*t2 = clock();
	 dt = ((double)(t2-t1)) / CLOCKS_PER_SEC;
	 if (verbose) {
	 fprintf(stderr,_("elapsed time = %5.3fs\n"),dt);
	 }*/
	
	
}
#undef TS