function [cam_image]=Project_Record_Frame(theprojector,thecamera,objects,options)
% Proejct_Record_Frame
%   Project_Record_Frame(theprojector,thecamera,objects,options);
%   Project the image of the projector on the to objects and images by the thecamera
%
%   'options' is a structure that may contains:
%       - 'compute_flag' : a (nvertx x 3) array specifying the normals at each vertex.
%       - 'edge_color' : a float specifying the color of the edges.
%       - 'face_color' : a float specifying the color of the faces.
%       - 'face_vertex_color' : a color per vertex or face.
%       - 'vertex'
%
%   See also: mesh_previewer.
%
%   Copyright (c) 2004 Gabriel Peyr?

if nargin<4,
    %It will print some debugging and checking information if it is nonzero
    options.verbose = 1;
    %This specifies the threshold for checking plane displacement
    options.d_thresh = 0.0000001;
    options.line_width = 1;
    options.cstyle=['bgrcmyk'];
    options.len=1;
end
if isfield(options,'verbose')==0,
    options.verbose = 0;
end
if isfield(options,'d_thresh'),
    options.d_thresh = 0.0000001;
end
if isfield(options,'style')==0,
    options.style = ['-'];
end
if isfield(options,'line_width')==0,
    options.line_width = 1;
end
if isfield(options,'bklight')==0,
    options.bklight=0.2;
end
%We need to know the relative pose and position of the camera relative to the projector
Rc2p = theprojector.Rw'*thecamera.Rw;
Tc2p = theprojector.Rw'*(thecamera.Tw-theprojector.Tw);
if length(size(theprojector.image))==3,
    %It is a color image in this case
    cam_image=zeros([thecamera.im_height thecamera.im_width 3]);
    for ii=1:3,
        cam_image(:,:,ii)=thecamera.bkcolor(ii);
    end
    thecamera.nband=3;
else
    cam_image=zeros([thecamera.im_height thecamera.im_width]);
    cam_image=cam_image+thecamera.bkcolor(1);
    thecamera.nband=1;
end
%XV and YV are the triangle in the camera
XV=zeros([3 1]);
YV=zeros([3 1]);
thecamera.boundXV=[1;thecamera.im_width;thecamera.im_width;1];
thecamera.boundYV=[1;1;thecamera.im_height;thecamera.im_height];

theprojector.boundXV=[1;theprojector.im_width;theprojector.im_width;1];
theprojector.boundYV=[1;1;theprojector.im_height;theprojector.im_height];

%for cobj=1:size(objects,1),
for cobj=1:1,
    %We assume the vertices are given in the world coordinate
    %We need to convert that into first camera
    v_cam=thecamera.world2cam*objects{cobj}.v;
    for v_ind=1:size(v_cam,2),
        v_cam(:,v_ind)=v_cam(:,v_ind)/v_cam(end,v_ind);
    end
    v_cam=v_cam(1:3,:);
    %We project the pixels on the image
    v_image=thecamera.P*objects{cobj}.v;
    for v_ind=1:size(v_image,2),
        v_image(:,v_ind)=v_image(:,v_ind)/v_image(end,v_ind);
    end


    norms_v=thecamera.Rw'*objects{cobj}.norms;
    triangles_d=zeros([1 size(norms_v,2)]);
    for f_ind=1:size(objects{cobj}.f,2),
    %for f_ind=1:1,
        triangles_d(f_ind)=dot(norms_v(:,f_ind),v_cam(:,objects{cobj}.f(1,f_ind)));
        if options.verbose>0,
            fprintf(1,'For object %d, triangle %d, the displaces are [%10.8f ',cobj,f_ind,triangles_d(f_ind));
            for ii=2:3,
                tmp_d=dot(norms_v(:,f_ind),v_cam(:,objects{cobj}.f(ii,f_ind)));
                if abs(tmp_d-triangles_d(f_ind))>options.d_thresh,
                    fprintf(1,'For object %d, triangle %d, the norm is not correct.\n',cobj,f_ind);
                else
                    fprintf(1,'%10.8f ', tmp_d);
                end
            end
            fprintf(1,'].\n');
        end
        camera_visible_flag = (dot(thecamera.z,objects{cobj}.norms(:,f_ind)) < 0);
        projector_visible_flag=(dot(theprojector.z,objects{cobj}.norms(:,f_ind))<0);
        if camera_visible_flag == 1,
            if options.verbose>0,
                fprintf(1,'Triangle %d of object %d is visible in the camera.\n',f_ind,cobj);
            end

            %Here I am lazy and I just use the bounding box first to define a region and
            %use inpolygon to find the points that are inside the triangle
            %We first find the vertices of the triangle in the camera's pixel coordinate
            for ii=1:3,
                XV(ii)=v_image(1,objects{cobj}.f(ii,f_ind));
                YV(ii)=v_image(2,objects{cobj}.f(ii,f_ind));
            end
            x_min=floor(min(XV)); 
            if x_min < 1, 
                x_min=1; 
            end
            y_min=floor(min(YV));
            if y_min < 1,
                y_min=1;
            end
            x_max=ceil(max(XV));
            if x_max >thecamera.im_width,
                x_max=thecamera.im_width;
            end
            y_max=ceil(max(YV));
            if y_max >thecamera.im_height,
                y_max = thecamera.im_height;
            end
            [X_grid Y_grid]=meshgrid(x_min:x_max,y_min:y_max);
            X_grid_org=X_grid;
            X_grid=reshape(X_grid,[prod(size(X_grid)) 1]);
            Y_grid=reshape(Y_grid,[prod(size(Y_grid)) 1]);
            IN_triangle=inpolygon(X_grid,Y_grid,XV,YV);
            IN_bound=inpolygon(X_grid,Y_grid,thecamera.boundXV,thecamera.boundYV);
            IN_flags=IN_triangle & IN_bound;
            IN_flags_img=reshape(IN_flags,size(X_grid_org));
            
%             for b_ind=1:thecamera.nband,
%                 cam_image(X_grid,Y_grid,b_ind)=objects{cobj}.bkcolor(b_ind)/4;
%             end
            if projector_visible_flag ==1,
                if options.verbose>0,
                    fprintf(1,'Triangle %d of object %d is visible in the projector.\n',f_ind,cobj);
                end

                %H is the homography between the images of the triangle on the camera and on the projector
                %in the normalized coordinate
                %xp ~ H * xc, meaning that xp in the projector can be computed using H*xc in the homogenoues
                %coordinate
                H=(Rc2p+(1/triangles_d(f_ind))*Tc2p*norms_v(:,f_ind)')
                npoints = 0;
                p_ind=find(IN_flags);
                X_grid_sub=X_grid(p_ind);
                Y_grid_sub=Y_grid(p_ind);
                if length(p_ind)>0,
                Point_im=[X_grid(p_ind)';Y_grid(p_ind)';ones([1 size(p_ind)])];
                Point_n=inv(thecamera.K) * Point_im;
                Point_n_p=H*Point_n;
                Point_im_p=theprojector.K*Point_n_p;
                for ii=1:size(Point_im_p,2),
                    Point_im_p(:,ii)=Point_im_p(:,ii)/Point_im_p(end,ii);
                end
                Point_in_flag=inpolygon(Point_im_p(1,:)',Point_im_p(2,:)',theprojector.boundXV,theprojector.boundYV);
                p_indices=find(Point_in_flag>=0.5);
                %This point is the image plane
                for b_ind=1:thecamera.nband,
                    ZI=interp2(double(theprojector.image(:,:,b_ind)),Point_im_p(1,:)',Point_im_p(2,:)');
                    for pp=1:length(p_indices),
                        cam_image(X_grid_sub(p_indices(pp)),Y_grid_sub(p_indices(pp)),b_ind)=round(ZI(p_indices(pp))*theprojector.weight+objects{cobj}.bkcolor(b_ind)*(1-theprojector.weight));
                    end
                 end
                 end
            else
                p_ind=find(IN_flags);
                for i=1:length(p_ind),
                    for b_ind=1:thecamera.nband,
                        cam_image(X_grid(p_ind(i)),Y_grid(p_ind(i)),b_ind)=objects{cobj}.bkcolor(b_ind)*options.bklight;
                    end
                end
            end
            
        end
    end
end
return;
 