%[v,f]=read_off('home1_shapes.off');
%[v,f]=read_off('cube.off');
if exist('capture_images','var')==0,
   capture_images=0;
end

[v,f]=read_off('sphere_mesh_12.off');

figure(1);
clf;
%subplot(1,3,1);
set(gcf,'Color',[1 1 1]);
options.line_width =2;
options.style =['-'];
options.cstyle=['r'];
plot_mesh_edges(v,f,options);
[f_n, f_flags]=calc_mesh_norm(v,f);
plot_mesh_norms(v,f,f_n);
nobjects = 2;
objects=cell([nobjects 1]);
%The object is three walls
wall_size=1;
wall_pos=[-50 -50 -50]/(5);
wall_v=[0 0 0  %v1
    1 0 0
    1 1 0
    0 1 0
    0 1 1
    0 0 1
    1 0 1];
wall_f=[1 2 3
    1 3 4
    1 4 6
    4 5 6
    1 6 7
    1 7 2];
wall_norm=[0 0 1
    0 0 1
    1 0 0
    1 0 0
    0 1 0
    0 1 0];
wall_v=wall_v*wall_size;
for i=1:size(wall_v,1),
    wall_v(i,:)=wall_v(i,:)+wall_pos;
end
figure(2);
clf;
plot_mesh(v,f);
hold on;
plot_mesh(wall_v,wall_f);
objects{1}.v=[wall_v';ones([1 size(wall_v,1)])];
objects{1}.f=wall_f';
objects{1}.norms=wall_norm';
objects{1}.bkcolor=[255 255 255];
%Here each vertex is a column
objects{2}.v=[v;ones([1 size(v,2)])];
%Here we assume each triangle is a column in the face list f
objects{2}.f=f;
%We assume the normal of a triangle is pointing outside
%Here each norm is a column
objects{2}.norms=f_n';
objects{2}.bkcolor=[0 255 0];
ncameras=3;
cameras=cell([ncameras 1]);
c_cam=1;
cameras{c_cam}.center=[10 6 10]';
cameras{c_cam}.center=cameras{c_cam}.center/2;
cameras{c_cam}.target=[0 0 0]';
cameras{c_cam}.target=wall_pos';
cameras{c_cam}.f=2.0;
cameras{c_cam}.upvector=[0 0 1]';
cameras{c_cam}.im_height = 256;
cameras{c_cam}.im_width = 256;
cameras{c_cam}.pl_height = 1;
cameras{c_cam}.pl_width = 1;
cameras{c_cam}.bkcolor=[64 0 0];
cameras{c_cam}.ray_len=7.5;
cameras{c_cam}.ray_style='r--';

c_cam=c_cam+1;
cameras{c_cam}.center=[6 10 10]';
cameras{c_cam}.center=cameras{c_cam}.center/2;
cameras{c_cam}.target=[0 0 0]';
cameras{c_cam}.target=wall_pos';

cameras{c_cam}.f=2.0;
cameras{c_cam}.upvector=[0 0 1]';
cameras{c_cam}.im_height = 256;
cameras{c_cam}.im_width = 256;
cameras{c_cam}.pl_height = 1;
cameras{c_cam}.pl_width = 1;
cameras{c_cam}.bkcolor=[64 0 0];
cameras{c_cam}.ray_len=20;
cameras{c_cam}.ray_style='r--';

c_cam=c_cam+1;
cameras{c_cam}.center=[10 10 6]';
cameras{c_cam}.center=cameras{c_cam}.center/2;
cameras{c_cam}.target=[0 0 0]';
cameras{c_cam}.target=wall_pos';

cameras{c_cam}.f=2.5;
cameras{c_cam}.upvector=[0 0 1]';
cameras{c_cam}.im_height = 256;
cameras{c_cam}.im_width = 256;
cameras{c_cam}.pl_height = 1;
cameras{c_cam}.pl_width = 1;
cameras{c_cam}.weight = 0.60;
cameras{c_cam}.ray_len=25;
cameras{c_cam}.ray_style='r--';
cameras{c_cam}.bkcolor=[64 0 0];


for i=1:ncameras,
   cameras{i}.z=cameras{i}.target-cameras{i}.center;
   cameras{i}.z=cameras{i}.z/norm(cameras{i}.z);
   cameras{i}.y=cameras{i}.upvector-dot(cameras{i}.upvector,cameras{i}.z)*cameras{i}.z;
   cameras{i}.y=cameras{i}.y/norm(cameras{i}.y);
   cameras{i}.x=cross(cameras{i}.y,cameras{i}.z);
   %This is the pose of the camera in the world coordinate
   cameras{i}.Rw=[cameras{i}.x,cameras{i}.y,cameras{i}.z];
   %This is the position of the camera in the world coordinate
   cameras{i}.Tw=cameras{i}.center;
   %Note that given a point Xw in the world coordinate, its coorindate in the camera Xc is given by
   % Xc = cameras{i}.Rw'*Xw-cameras{i}.Rw'*cameras{i}.center
   % The following is the matrix that converts a point in the image plane to the corresponding point
   % in the pixel coordinate
   %
   cameras{i}.world2cam=[cameras{i}.Rw',-cameras{i}.Rw'*cameras{i}.Tw; 0 0 0 1];
   cameras{i}.cam2image=[-cameras{i}.im_width/cameras{i}.pl_width 0 (cameras{i}.im_width+1)/2
      0   -cameras{i}.im_height/cameras{i}.pl_height (cameras{i}.im_height+1)/2
      0                 0                                1];
   cameras{i}.f_K=diag([cameras{i}.f cameras{i}.f 1]);
   cameras{i}.K=cameras{i}.cam2image*cameras{i}.f_K;
   %Given a point in the pixel coordinate xim, we can get the point on the normalized image plane
   %xn = inv(cameras{i}.K) * xim;
   cameras{i}.P=cameras{i}.K*[1 0 0 0;0 1 0 0; 0 0 1 0]*cameras{i}.world2cam;
   %Checking, the target point should be mapped to the center of the image
   target_cam=cameras{i}.P*[cameras{i}.target;1];
   target_cam=target_cam./target_cam(end);
   if norm(target_cam-cameras{i}.cam2image(:,end))>0.00001,
      fprintf(1,'Something is wrong for camera %d. Its target should be mapped to ', i);
      fprintf(1,'%6.2f %6.2f instead of ', cameras{i}.cam2image(1,end),cameras{i}.cam2image(2,end));
      fprintf(1,'%6.2f %6.2f\n',target_cam(1),target_cam(2));
   end
   target_z=cameras{i}.Rw'*[cameras{i}.z];
   if norm(target_z-[0 0 1]')>0.00001,
      fprintf(1,'Something is wrong for camera %d. Its z axis should be mapped to ', i);
      fprintf(1,'[0 0 1] instead of ');
      target_cam=cameras{i}.world2cam*[cameras{i}.z; 1];
      fprintf(1,'[%6.2f %6.2f %6.2f]\n',target_z(1),target_z(2),target_z(3));
   end
   center_cam=cameras{i}.world2cam*[cameras{i}.center;1];
   center_cam=center_cam/center_cam(end);
   center_cam=center_cam(1:3);
   
   if norm(center_cam-[0 0 0]') > 0.00001,
      fprintf(1,'Something is wrong for camera %d. Its center should be mapped to ', i);
      fprintf(1,'[0 0 0] instead of ');
      fprintf(1,'%6.2f %6.2f %6.2f\n',center_cam(1),center_cam(2),center_cam(3));
   end
   image_plane_center=cameras{i}.Tw+cameras{i}.f*cameras{i}.z;
   cameras{i}.v=[cameras{i}.pl_width/2*cameras{i}.x+cameras{i}.pl_height/2*cameras{i}.y+image_plane_center];
   cameras{i}.v=[cameras{i}.v,cameras{1}.pl_width/2*cameras{i}.x-cameras{i}.pl_height/2*cameras{i}.y+image_plane_center];
   cameras{i}.v=[cameras{i}.v, -cameras{1}.pl_width/2*cameras{i}.x-cameras{i}.pl_height/2*cameras{i}.y+image_plane_center];
   cameras{i}.v=[cameras{i}.v, -cameras{1}.pl_width/2*cameras{i}.x+cameras{i}.pl_height/2*cameras{i}.y+image_plane_center];
   cameras{i}.f=[1 2 4;2 3 4];
   figure(2);
   hold on;
   plot_mesh(cameras{i}.v,cameras{i}.f);
   for ii=1:size(cameras{i}.v,2),
      dir_v=cameras{i}.v(:,ii)-cameras{i}.Tw;
      dir_v=dir_v/norm(dir_v);

      plot3arr([cameras{i}.Tw';(cameras{i}.Tw+cameras{i}.ray_len*dir_v)'],cameras{i}.ray_style,2);
   end
   plot3arr([cameras{i}.Tw';(cameras{i}.Tw+cameras{i}.ray_len*cameras{i}.z)'],cameras{i}.ray_style,4);
   hh=plot3arr(cameras{i}.Tw','ro',4);
   set(hh,'MarkerSize',[6]);
   hh=plot3arr(cameras{i}.Tw','r+',4);
   set(hh,'MarkerSize',[6]);
end





nprojectors=1;
projectors=cell([nprojectors 1]);
projectors{1}.center=[12 12 15]';
projectors{1}.target=[0 0 0]';
projectors{1}.target=wall_pos';

projectors{1}.f=2.5;
projectors{1}.upvector=[0 0 1]';
projectors{1}.im_height = 256;
projectors{1}.im_width = 256;
projectors{1}.pl_height = 1;
projectors{1}.pl_width = 1;
projectors{1}.weight = 0.60;
projectors{1}.ray_len=25;
projectors{1}.ray_style='g--';

%If the following flag is zero, then the image size will be changed to the size given by
%the image
projectors{1}.scaling_image_flag = 0;
projectors{1}.imagename='Reneg-large.JPG';
projectors{1}.imagename='S3250large.jpg';
projectors{1}.imagename='frame_patterns_4.bmp';
for i=1:nprojectors,
   [projectors{i}.image, cmap]=imread(projectors{1}.imagename);
   projectors{i}.image=ind2rgb(projectors{i}.image, cmap);
   projectors{i}.image=projectors{i}.image(:,1:1000,:)*255;
   
   if projectors{1}.scaling_image_flag==0,
      projectors{1}.im_height=size(projectors{i}.image,1);
      projectors{1}.im_width = size(projectors{i}.image,2);
   else
      projectors{i}.image=imresize(projectors{i}.image,[projectors{1}.im_height projectors{1}.im_width],'bilinear');
   end
   
   projectors{i}.z=projectors{i}.target-projectors{i}.center;
   projectors{i}.z=projectors{i}.z/norm(projectors{i}.z);
   projectors{i}.y=projectors{i}.upvector-dot(projectors{i}.upvector,projectors{i}.z)*projectors{i}.z;
   projectors{i}.y=projectors{i}.y/norm(projectors{i}.y);
   projectors{i}.x=cross(projectors{i}.y,projectors{i}.z);
   %This is the pose of the projector in the world coordinate
   projectors{i}.Rw=[projectors{i}.x,projectors{i}.y,projectors{i}.z];
   %This is the position of the projector in the world coordinate
   projectors{i}.Tw=projectors{i}.center;
   %Note that given a point Xw in the world coordinate, its coorindate in the projector Xc is given by
   % Xc = projectors{i}.Rw'*Xw-projectors{i}.Rw'*projectors{i}.center
   % The following is the matrix that converts a point in the image plane to the corresponding point
   % in the pixel coordinate
   %
   projectors{i}.world2proj=[projectors{i}.Rw',-projectors{i}.Rw'*projectors{i}.Tw; 0 0 0 1];
   projectors{i}.proj2image=[-projectors{i}.im_width/projectors{i}.pl_width 0 (projectors{i}.im_width+1)/2
      0   -projectors{i}.im_height/projectors{i}.pl_height (projectors{i}.im_height+1)/2
      0                 0                                1];
   projectors{i}.f_K=diag([projectors{i}.f projectors{i}.f 1]);
   projectors{i}.K=projectors{i}.proj2image*projectors{i}.f_K;
   %Given a point in the pixel coordinate xim, we can get the point on the normalized image plane
   %xn = inv(projectors{i}.K) * xim;
   projectors{i}.P=projectors{i}.K*[1 0 0 0;0 1 0 0; 0 0 1 0]*projectors{i}.world2proj;
   %Checking, the target point should be mapped to the center of the image
   target_proj=projectors{i}.P*[projectors{i}.target;1];
   target_proj=target_proj./target_proj(end);
   if norm(target_proj-projectors{i}.proj2image(:,end))>0.00001,
      fprintf(1,'Something is wrong for projector %d. Its target should be mapped to ', i);
      fprintf(1,'%6.2f %6.2f instead of ', projectors{i}.proj2image(1,end),projectors{i}.proj2image(2,end));
      fprintf(1,'%6.2f %6.2f\n',target_proj(1),target_proj(2));
   end
   target_z=projectors{i}.Rw'*[projectors{i}.z];
   if norm(target_z-[0 0 1]')>0.00001,
      fprintf(1,'Something is wrong for projector %d. Its z axis should be mapped to ', i);
      fprintf(1,'[0 0 1] instead of ');
      target_proj=projectors{i}.world2proj*[projectors{i}.z; 1];
      fprintf(1,'[%6.2f %6.2f %6.2f]\n',target_z(1),target_z(2),target_z(3));
   end
   center_proj=projectors{i}.world2proj*[projectors{i}.center;1];
   center_proj=center_proj/center_proj(end);
   center_proj=center_proj(1:3);
   if norm(center_proj-[0 0 0]') > 0.00001,
      fprintf(1,'Something is wrong for projector %d. Its center should be mapped to ', i);
      fprintf(1,'[0 0 0] instead of ');
      fprintf(1,'%6.2f %6.2f %6.2f\n',center_proj(1),center_proj(2),center_proj(3));
   end
   image_plane_center=projectors{i}.Tw+projectors{i}.f*projectors{i}.z;
   projectors{i}.v=[projectors{i}.pl_width/2*projectors{i}.x+projectors{i}.pl_height/2*projectors{i}.y+image_plane_center];
   projectors{i}.v=[projectors{i}.v,cameras{1}.pl_width/2*projectors{i}.x-projectors{i}.pl_height/2*projectors{i}.y+image_plane_center];
   projectors{i}.v=[projectors{i}.v, -cameras{1}.pl_width/2*projectors{i}.x-projectors{i}.pl_height/2*projectors{i}.y+image_plane_center];
   projectors{i}.v=[projectors{i}.v, -cameras{1}.pl_width/2*projectors{i}.x+projectors{i}.pl_height/2*projectors{i}.y+image_plane_center];
   projectors{i}.f=[1 2 4;2 3 4];
   figure(2);
   hold on;
   hh_proj=plot_mesh(projectors{i}.v,projectors{i}.f);
   for ii=1:size(projectors{i}.v,2),
      dir_v=projectors{i}.v(:,ii)-projectors{i}.Tw;
      dir_v=dir_v/norm(dir_v);
      plot3arr([projectors{i}.Tw';(projectors{i}.Tw+projectors{i}.ray_len*dir_v)'],projectors{i}.ray_style,2);
   end
   plot3arr([projectors{i}.Tw';(projectors{i}.Tw+projectors{i}.ray_len*projectors{i}.z)'],projectors{i}.ray_style,4);
   hh=plot3arr(projectors{i}.Tw','go',4);
   set(hh,'MarkerSize',[6]);
   hh=plot3arr(projectors{i}.Tw','g+',4);
   set(hh,'MarkerSize',[6]);
   %warp(projectors{i}.v(1,:)',projectors{i}.v(2,:)',projectors{i}.v(3,:)',projectors{i}.image);
   warp_x=[projectors{i}.v(1,1) (projectors{i}.v(1,1)+projectors{i}.v(1,4))/2 projectors{i}.v(1,4)
      projectors{i}.v(1,2) (projectors{i}.v(1,2)+projectors{i}.v(1,3))/2 projectors{i}.v(1,3)];
   warp_y=[projectors{i}.v(2,1) (projectors{i}.v(2,1)+projectors{i}.v(2,4))/2 projectors{i}.v(2,4)
   projectors{i}.v(2,2) (projectors{i}.v(2,2)+projectors{i}.v(2,3))/2 projectors{i}.v(2,3)];
  warp_z=[projectors{i}.v(3,1) (projectors{i}.v(3,1)+projectors{i}.v(3,4))/2 projectors{i}.v(3,4)
      projectors{i}.v(3,2) (projectors{i}.v(3,2)+projectors{i}.v(3,3))/2 projectors{i}.v(3,3)];
  warp(warp_x,warp_y,warp_z,imresize(projectors{i}.image,[20 20]));
  

end
campos=projectors{1}.Tw;
camup=projectors{1}.y;
camtarget=projectors{1}.target;
camva=20;

if capture_images~=0,
cameras{3}.image=Project_Record_Frame(projectors{1},cameras{3},objects);
figure(1);
clf;
image(cameras{3}.image/255);
end