%dir;

% load camera results (for setting active images, n_ima,...)
load camera_results;


proj_name = 'proj'; %input('Basename projector calibration images (without number nor suffix): ','s');
camera_name = 'cam';

%i = 1;

xr_list = NaN*ones(1,n_ima);
yr_list = NaN*ones(1,n_ima);

ind_ima_proj = 1:n_ima;


DEBUG = 1;
%ind_ima_proj = [18];

for i = ind_ima_proj,
      
   if active_images(i),
   
   	%fprintf(1,'Loading image %d...\n',i);
   
   	if ~type_numbering,   
      	number_ext =  num2str(image_numbers(i));
   	else
      	number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i));
   	end;
   	
      ima_namep = [proj_name  number_ext 'p.' format_image];
      ima_namen = [proj_name  number_ext 'n.' format_image];
      ima_namer = [proj_name  number_ext 'r.' format_image];
      ima_nameb = [camera_name  number_ext '.' format_image];
      
      if i == ind_active(1),
         fprintf(1,'Loading image ');
      end;
         
         fprintf(1,'%d...',i);
         
         if format_image(1) == 'p',
            if format_image(2) == 'p',
               Ip = double(loadppm(ima_namep));
               In = double(loadppm(ima_namen));
               Ir = double(loadppm(ima_namer));
               Ib = double(loadppm(ima_nameb));
            else
               Ip = double(loadpgm(ima_namep));
               In = double(loadpgm(ima_namen));
               Ir = double(loadpgm(ima_namer));
               Ib = double(loadpgm(ima_nameb));
            end;
         else
            if format_image(1) == 'r',
               Ip = readras(ima_namep);
               In = readras(ima_namen);
               Ir = readras(ima_namer);
               Ib = readras(ima_nameb);
            else
               Ip = double(imread(ima_namep));
               In = double(imread(ima_namen));
               Ir = double(imread(ima_namer));
               Ib = double(imread(ima_nameb));
            end;
         end;

      	
			if size(Ip,3)>1,
            Ip = 0.299 * Ip(:,:,1) + 0.5870 * Ip(:,:,2) + 0.114 * Ip(:,:,3);
            In = 0.299 * In(:,:,1) + 0.5870 * In(:,:,2) + 0.114 * In(:,:,3);
            Ir = 0.299 * Ir(:,:,1) + 0.5870 * Ir(:,:,2) + 0.114 * Ir(:,:,3);
            Ib = 0.299 * Ib(:,:,1) + 0.5870 * Ib(:,:,2) + 0.114 * Ib(:,:,3);
   		end;
         
         %IIp = Ip - In;
         
         %Ip2 = Ip - Ib;
         %In2 = In - Ib;
         
         %imax = max(IIp(:));
         %imin = min(IIp(:));
         
         %IIp = 255*(IIp - imin)/(imax - imin);
         
         %indplus = find(IIp >= 255/2);
         %indminus = find(IIp < 255/2);
         
         %IIp(indplus) = 255*ones(length(indplus),1);
         %IIp(indminus) = zeros(length(indminus),1);
         
         delta_I = Ip - In;
         
         %IIp = 255*(1 + exp(-delta_I/2)).^(-1);
         
         
         IIp = (Ip >= In)*255;
         
         IIp = conv2(conv2(IIp,[1/4 1/2 1/4],'same'),[1/4 1/2 1/4]','same');
         
   		eval(['Ip_' num2str(i) ' = IIp;']);
   		eval(['In_' num2str(i) ' = 255 - IIp;']);
         
         I_marker2 = Ib-Ir;
         
         
         if DEBUG,
            
            Imax = max(I_marker2(:));
            Imin = min(I_marker2(:));
            
            I_marker_out = 255*(I_marker2 - Imin)/(Imax - Imin);
            
            figure(3);
            image(I_marker_out);
            colormap(gray(256));
            
            [xr,yr] = ginput(1);
            
            xr_list(i) = xr;
            yr_list(i) = yr;
            
         else
            
            I_marker = I_marker2 >50;
            I_marker = eliminate_boundary(I_marker);
            I_marker = eliminate_boundary(I_marker);
            [ymm,xmm] = find(I_marker);
            
            if length(xmm)<10,
               fprintf(1,'WARNING!! No marker in image %d!!!!\n',i);
            else
               xr = mean(xmm);
               yr = mean(ymm);
               xr_list(i) = xr;
               yr_list(i) = yr;
            end; 
         end;
         
         figure(2);
         image(IIp);
         colormap(gray(256));
         hold on;
         plot(xr,yr,'go');
         hold off;
         title(['Image ' num2str(i)]);
         drawnow;
         
         if ~DEBUG
            waitforbuttonpress;
         end;
         
         
         
   end;
   
   %i = i+1;   
   
end;


fprintf(1,'\nExtraction of the grid corners on the image\n');

if 0,
   disp('Window size for corner finder (wintx and winty):');
   wintx = input('wintx ([] = 8) = ');
   if isempty(wintx), wintx = 8; end;
   wintx = round(wintx);
   winty = input('winty ([] = 8) = ');
   if isempty(winty), winty = 8; end;
   winty = round(winty);
   fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1);
   
   
   disp('The projector you are using is the DLP or Intel');
   nx = 1024;
   ny = 768;
   
   dX = input('Size dX in x of the squares (in pixels) [32] = ');
   dY = input('Size dY in y of the squares (in pixels) [32] = ');
   
   if isempty(dX), dX=32; end;
   if isempty(dY), dY=32; end;
   
   dXoff = input('Position in x of your reference (in pixels) [511.5] = ');
   dYoff = input('Position in y of your reference (in pixels) [383.5] = ');
   
   if isempty(dXoff), dXoff=511.5; end;
   if isempty(dYoff), dYoff=383.5; end;
   
else
   
   wintx = 8;
   winty = 8;
   nx = 1024;
   ny = 768;
   dX = 32;
   dY = 32;
   dXoff=511.5;
   dYoff=383.5;
   
end;

qqq = input('Indices of the images to extract ([]=all) ');

if isempty(qqq),
   ind_proc = ind_active;
else
   ind_proc = qqq;
end;



recompute_corner = 1;

if recompute_corner & ~exist(['xproj_' num2str(ind_proc(1))]),
   if exist('projector_data'),
      load projector_data;
   else
      recompute_corner = 0;
      disp('WARNING: Cannot recompute corners. Data need to be extracted at least once');
   end;   
end;

if ~recompute_corner,
   disp('Manual extraction mode');
else
   disp('Automatic recomputation of the corners');
end;

% extract the projector corners:

for kk = ind_proc,
   
   eval(['Ip = Ip_' num2str(kk) ';']);
   eval(['In = In_' num2str(kk) ';']);
   
   xr = xr_list(kk);
   yr = yr_list(kk);
   
   if ~recompute_corner,
		[x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(Ip,wintx,winty,fc_save,cc_save,kc_save,dX,dY,xr,yr,1);
   	xproj = x;
   else
      eval(['xproj = xproj_' num2str(kk) ';']);
      x = cornerfinder(xproj+1,Ip,winty,wintx);
      xproj = x - 1;
   end;
   
   Np_proj = size(x,2);
   
	figure(2);
	image(Ip);
	hold on;
	plot(xproj(1,:)+1,xproj(2,:)+1,'r+');
	%title('Click on your reference point');
	xlabel('Xc (in camera frame)');
	ylabel('Yc (in camera frame)');
	hold off;
	
	%disp('Click on your reference point...');
	
   %[xr,yr] = ginput2(1);
   
   xr = xr_list(kk);
   yr = yr_list(kk);
   
	err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2));
	ind_ref = find(err == min(err));
	
	ref_pt = xproj(:,ind_ref);
	
	figure(2);
	hold on;
   plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off;
   drawnow;
   
   
   if ~recompute_corner,
      
   	off_x = mod(ind_ref-1,n_sq_x+1);
   	off_y = n_sq_y - floor((ind_ref-1)/(n_sq_x+1));
   	
   	x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj));
   	
   	eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the projector image
   	
	end;

   eval(['xproj_' num2str(kk) ' = xproj;']); % coordinates of the points in the camera image

   
end;


string_save = 'save projector_data ';

for kk = ind_active,
   string_save = [string_save ' xproj_' num2str(kk) ' x_proj_' num2str(kk)];
end;
eval(string_save);