%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Canon G1
%
%    1 2 3 4 5 6 
%  1 G M G M G M ...
%  2 Y C Y C Y C
%  3 G M G M G M ...
%  4 Y C Y C Y C
%    .
%    .
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
clear; %close all;
run('./getraw4.m');
[rsize,csize] = size(g);
G = g; M = m; C = c; Y = y;
gamma = 2.2;


%------- COLOR BALANCING MATRIX -----------%
color_flag = 3;
if color_flag == 1;
	% Jong and Ulrich try 1
	color_corr = [
		1   1   -1   0
		1   0    0    0
		1   1    0   -1];
elseif color_flag == 2;
	% Jong and Ulrich try 2
	rgb2gmcy = [
	 0 1 0
	 1 0 1
	 0 1 1
	 1 1 0 ];
elseif color_flag == 3;
	% From the c code
	rgb2gmcy =     [
			 0.11, 0.86, 0.08
			 0.50, 0.29, 0.51
			 0.11, 0.92, 0.75
			 0.81, 0.98, 0.08 ];
end;

color_corr = pinv(rgb2gmcy)
%------- The END of COLOR BALANCING MATRIX -----------%



dms_flag = 3;
corr_flag = 2;



%------- COLOR DEMOSAICING -----------%
%
%


if dms_flag == 1;

	rG = G(1:2:rsize,1:2:csize);
	rM = M(1:2:rsize,2:2:csize);
	rC = C(2:2:rsize,2:2:csize);
	rY = Y(2:2:rsize,1:2:csize);
	G = rG; M = rM; C = rC; Y = rY; 

	[rsize,csize] = size(G);

elseif dms_flag == 2;

	G(2:2:rsize,1:2:csize) = G(1:2:rsize,1:2:csize); 
	G(:,2:2:csize) = G(:,1:2:csize);

	M(2:2:rsize,2:2:csize) = M(1:2:rsize,2:2:csize); 
	M(:,1:2:csize) = M(:,2:2:csize);

	C(1:2:rsize,2:2:csize) = C(2:2:rsize,2:2:csize);
	C(:,1:2:csize) = C(:,2:2:csize);

	Y(1:2:rsize,1:2:csize) = Y(2:2:rsize,1:2:csize);
	Y(:,2:2:csize) = Y(:,1:2:csize);

elseif dms_flag == 3;

	G(1:2:rsize,2:2:csize-2) = ...
						(G(1:2:rsize,1:2:csize-2) + G(1:2:rsize,3:2:csize))/2;
	M(1:2:rsize,3:2:csize) = ...
						(M(1:2:rsize,2:2:csize-2) + M(1:2:rsize,4:2:csize))/2;

	C(2:2:rsize,3:2:csize) = ...
						(C(2:2:rsize,2:2:csize-2) + C(2:2:rsize,4:2:csize))/2;
	Y(2:2:rsize,2:2:csize-2) = ...
						(Y(2:2:rsize,1:2:csize-2) + Y(2:2:rsize,3:2:csize))/2;

	G(2:2:rsize-2,:) = (G(1:2:rsize-2,:) + G(3:2:rsize,:))/2;
	M(2:2:rsize-2,:) = (M(1:2:rsize-2,:) + M(3:2:rsize,:))/2;
	C(3:2:rsize,:) = (C(2:2:rsize-2,:) + C(4:2:rsize,:))/2;
	Y(3:2:rsize,:) = (Y(2:2:rsize-2,:) + Y(4:2:rsize,:))/2;

elseif dms_flag == 4;



	f_width = .7 
	% Low-pass filter width with respect to FOV ( = 1/delta_x) of spectrum.
	% delta_x = pixel size in image-domain.

	N = 151; % window length. 
	F = zeros(N);
	for rind = 1:N;
		for cind = 1:N;
			F(rind,cind) = sinc(f_width*(rind-(N+1)/2)) * ...
											sinc(f_width*(cind-(N+1)/2));
		end;
	end;


	kaiser_N = kaiser(N,8);
	F = F .* (kaiser_N*kaiser_N');
	%F = F .* (hann(N)*hann(N)');

	figure; Ffft = fft2c(F); pa(Ffft((N-1)/2,:));

		% #1 - Rect windowed FIR low-pass filter 

	G = conv2(G,F,'same');
	M = conv2(M,F,'same');
	C = conv2(C,F,'same');
	Y = conv2(Y,F,'same');

		% #2 - Bartlett windowed FIR low-pass filter 
		% #3 - Hanning windowed FIR low-pass filter 
		% #4 - Hamming windowed FIR low-pass filter 
		% #5 - Blackman windowed FIR low-pass filter 
		% #6 - Kaiser windowed FIR low-pass filter 
		%		(all #1 - #5 can be implemented with Kaiser window.)

end;
%
%------- The END of COLOR DEMOSAICING -----------%






%------------ Color CORRECTION -----------------%
GMCY_arr = [G(:), M(:), C(:), Y(:)]';


if corr_flag == 1; %%% Changing SATURATION %%%
	load xyz_phos_tutorial;
	rgb2xyz = XYZ'*phosphors;
	xyz2rgb = inv(rgb2xyz);

	%color_corr(3,:) = 0.9*color_corr(3,:);

	gmcy2xyz = rgb2xyz*color_corr;

	if 0;
		whiteGMCY = zeros(4,1);
		whiteGMCY(1) = mean2(G(220:260, 680:720));
		whiteGMCY(2) = mean2(M(220:260, 680:720));
		whiteGMCY(3) = mean2(C(220:260, 680:720));
		whiteGMCY(4) = mean2(Y(220:260, 680:720));
		whiteXYZ = gmcy2xyz*whiteGMCY;
		whiteX0Z = [whiteXYZ(1);0;whiteXYZ(3)];
		save whiteX0Z whiteX0Z whiteXYZ
	else
		load whiteX0Z
	end;

	sensorXYZ = gmcy2xyz*GMCY_arr;
	

	if 0;
		newSensorXYZ = diag([1.03,1,1.03]) * ...
						(sensorXYZ - whiteX0Z*ones(1,size(sensorXYZ,2))) + ...
												whiteX0Z*ones(1,size(sensorXYZ,2));
	else
		sensorx = sensorXYZ(1,:)./sum(sensorXYZ);
		sensory = sensorXYZ(2,:)./sum(sensorXYZ);
		whitex = whiteXYZ(1)/sum(whiteXYZ);
		whitey = whiteXYZ(2)/sum(whiteXYZ);
		newSensorx = 1.50*(sensorx - whitex*ones(1,length(sensorx))) + ...
											whitex*ones(1,length(sensorx));
		newSensory = 1.50*(sensory - whitey*ones(1,length(sensory))) + ...
											whitey*ones(1,length(sensory));
		newSensorY = sensorXYZ(2,:);
		newSensorX = newSensorY./newSensory.*newSensorx;
		newSensorZ = newSensorY./newSensory - newSensorY - newSensorX;

		newSensorXYZ = [newSensorX;newSensorY;newSensorZ];
		% not linear operation ok because we can assume that the gmcy2rgb
		% was wrong in the sense that A = A_correct + A_wrong;
	end;


	RGB_arr = xyz2rgb*newSensorXYZ;

	max_rgb = max(RGB_arr(:));
	RGB_arr = RGB_arr.*(RGB_arr>0);
	RGBarr = RGB_arr/max_rgb;

elseif corr_flag == 2;
	load new_color_corr;
	RGB_arr = color_corr*GMCY_arr;

	max_rgb = max(RGB_arr(:));
	RGB_arr = RGB_arr.*(RGB_arr>0);
	RGBarr = RGB_arr/max_rgb; % Now, RGBarr is from exactly 0 to 1.

else
	color_corr(:,3) = 0.9*color_corr(:,3);
	RGB_arr = color_corr*GMCY_arr;

	max_rgb = max(RGB_arr(:));
	RGB_arr = RGB_arr.*(RGB_arr>0);
	%RGBarr = RGB_arr/max_rgb/1.5; 
	RGBarr = RGB_arr/max_rgb; 
end;




gcRGBarr = (RGBarr).^(1/gamma); % Gamma Correction

imRGB = zeros(rsize,csize,3);
imRGB(:,:,1) = reshape(gcRGBarr(1,:),rsize,csize);
imRGB(:,:,2) = reshape(gcRGBarr(2,:),rsize,csize);
imRGB(:,:,3) = reshape(gcRGBarr(3,:),rsize,csize);
figure; image(imRGB); axis equal;
%------------ The END of Color CORRECTION -----------------%
