%attenzion

function fig=plotcam4(cams,space,voxels,latovoxel,show,col)
%INPUT: -show: indica se mostrare: a)la telecamera 2)i versori 3) il tronco  
%              di piramide 4)i voxel del campo 5) i voxels interni 6) i
%              marker totali 7) i marker visti

%passo in ingresso un vettore con le etichette di tutte le telecamere di
%cui voglio fare il plot

% if nargin<1
%     for i=1:size(cams,2)
%           default=[default i];
%     end
% end
if nargin<5
    show=[1 1 1 0 0 0 0];
end
latox=space(1);
latoy=space(2);
latoz=space(3);
    %figure(5);     %cambiare e creare un figura con nome figure(fig)
    
    %hold off;
    %axis vis3d;
    %axis tight;
    
    %clf;
%     ax = axes('position',[.22 .1 .75 .85],'XLim',[-2 space(1)+2],...
%               'YLim',[-2 space(2)+2],...
%               'ZLim',[-2 space(3)+2]);
for i=1:size(cams,2)
    % comprensione
    
%     Rx= makehgtform('xrotate',cams(i).angles(1));
%     Ry= makehgtform('yrotate',cams(i).angles(2));
%     Rz= makehgtform('zrotate',cams(i).angles(3));
%     R = Rx*Ry*Rz;
%     R_cam=R(1:3,1:3);
    
    %in uscita ho una matrice 4x4 perchè  makehgtform permette trasformazioni
    %anche nel tempo...quindi a noi interessa solo la sottomatrice 3x3
    
    %altrimenti potrei uitlizzare:
%     Rx=rotionmat3D(cams(i).angles(1),[1,0,0])
%     Rz=rotionmat3D(cams(i).angles(2),[0,1,0])
%     Ry=rotionmat3D(cams(i).angles(3),[0,0,1])
%     R_cam=Rx*Ry*Rz
%     

%     Rx=rotationmat3D(cams(i).angles(1),[1,0,0]);
%     Rz=rotationmat3D(cams(i).angles(2),[0,1,0]);
%     Ry=rotationmat3D(cams(i).angles(3),[0,0,1]);
%     R_cam=Rx*Ry*Rz;

    R_cam=cams(i).rotationmat;

    % per ogni baricentro calcolo il valore che hanno i 4(6 se considero il tronco)
    % piano per le x,y (z?) del baricentro => trovo 2(4) intervallo 
    % => inserisco tutti i baricentri dei voxel inerni a tali intervalli
    % sarebbe più logico partire dal centro prospettico e muoversi lungo i
    % voxel dell'asse della piramide

    if isfield(cams,'fov_angle')
        angle_cam = cams(i).fov_angle;
    else 
        angle_cam= pi/3;     %in realtà dovrei passarlo dalla edit text box
    end

    if isfield(cams,'dist')
        h_min = cams(i).dist(1);
        h_max = cams(i).dist(2);         
    else
        %inserisco i valori di default
        h_max = 5;          %5 metri
        h_min = 0.5;        %2 metri     
    end

    d_M       = h_max*tan(angle_cam/2);  %lato base maggiore tronco cono = fov_MAX
    d_m       = h_min*tan(angle_cam/2);  %lato base minore tronco cono = fov_min

    d_M       = h_max*tan(angle_cam/2);  %lato base maggiore tronco cono
    d_m       = h_min*tan(angle_cam/2);  %lato base minore tronco cono

    %vertici del tronco di piramide
    v =      [0 0 0;
             -d_m -d_m h_min; 
             -d_m +d_m h_min;
             +d_m +d_m h_min; 
             +d_m -d_m h_min;
             -d_M -d_M h_max; 
             -d_M +d_M h_max;
             +d_M +d_M h_max; 
             +d_M -d_M h_max;]';
    
    cube_cam = [-.1 .1 .1 -.1 -.1 .1 .1 -.1;
              -.1 -.1 .1 .1 -.1 -.1 .1 .1;
              -.1 -.1 -.1 -.1 .35 .35 .35 .35];

    trapezio = [-.1 .1 .1 -.1 -.15 .15 .15 -.15;
                -.1 -.1 .1 .1 -.15 -.15 .15 .15;
                .35 .35 .35 .35 .45 .45 .45 .45];
    %se conosco la matrice rotazione conosco anche come sono posti i versori
    versori =([ 0 1 0 0 0 0;
                0 0 0 1 0 0;
                0 0 0 0 0 1]);

    %i nuovi vertici della piramide ruotata sono:
    new_v = R_cam*v + cams(i).position * ones(1,9);
    %vertici della scataola telecamera
    scatola  = R_cam*cube_cam + cams(i).position * ones(1,8);
    new_trap = R_cam*trapezio + cams(i).position * ones(1,8);
    %mentre i nuovi versori sono dati da:
    newversori = R_cam*versori + cams(i).position * ones(1,6);
    %%
     %%plot della telecamera
      if  show(1) == 1  
        
        p3 = struct('vertices',scatola','faces',[1 2 3 4;5 6 7 8;2 6 7 3;...
            1 5 8 4;1 2 6 5; 4 3 7 8]);
        h3 = patch(p3);
        set(h3,'facecolor',col,'FaceAlpha',0.65,'EdgeColor', 'k');
        
        p4 = struct('vertices',new_trap','faces',[1 2 3 4;5 6 7 8;2 6 7 3;...
            1 5 8 4;1 2 6 5; 4 3 7 8]);
        h4 = patch(p4);
        set(h4,'facecolor',col,'FaceAlpha',0.65,'EdgeColor', 'k');
        %text(new_v(1,1),new_v(2,1),new_v(3,1)+0.45,num2str(i),'fontsize',11,'color','m');
  
     end

    %%plot dei versori
     if show(2) == 1
        hold on;                      
        %plot3(newversori(1,:),newversori(2,:),newversori(3,:),'m-','linewidth',1.1');
        plot3(newversori(1,1:2),newversori(2,1:2),newversori(3,1:2),'g-','linewidth',1.1');
        plot3(newversori(1,3:4),newversori(2,3:4),newversori(3,3:4),'c-','linewidth',1.1');
        plot3(newversori(1,5:6),newversori(2,5:6),newversori(3,5:6),'m-','linewidth',1.1');
     
     end

    %%plot del tronco di piramide
     if show(3) == 1
         
        p1 = struct('vertices',new_v','faces',[1 2 3;1 3 4;1 4 5;1 5 2]);
        h1 = patch(p1);
        set(h1,'facecolor','w','FaceAlpha',0.15,'EdgeColor', 'y'); %[52 217 160]/255
        
        p2 = struct('vertices',new_v','faces',[2 3 4 5]);
        h2 = patch(p2);
        set(h2,'facecolor','none','EdgeColor', 'b'); 
        
        p3 = struct('vertices',new_v','faces',[6 7 8 9]);
        h3 = patch(p3);
        set(h3,'facecolor','none','EdgeColor', 'r');
        
        p4 = struct('vertices',new_v','faces',[2 3 4 5; 3 7 6 2; 3 7 8 4;4 8 9 5; 2 6 9 5; ]);
        h4 = patch(p4);
        set(h4,'facecolor','r','FaceAlpha',0.1,'EdgeColor', 'r');
     end
      
     %%plot dei voxels visibili dalla telecamera
      if show(5) == 1 & isfield(cams,'voxels')
         size(cams(i).voxels,2)
%         for j=1:size(cams(i).voxels,2)
%        
% %             voxels(cams(i).voxels(j)).position
%         voxel(voxels(cams(i).voxels(j)).position,[latovoxel latovoxel latovoxel],'g',0.5);  
%         %N.B. non si confonda la funzione voxel che serveper il plot del 
%         %cubo con la variabile 'voxels' di tipo struct                                              
%         hold on
%         end
        for j=1:size(cams(i).voxels,2)      
            DrawCuboid( [latovoxel latovoxel latovoxel]',...
                         voxels(cams(i).voxels(j)).position,...
                         [0,0,0], 'g', 0.5);
        end
        
      end

end


     %%plot tutti i voxels del campo di visuale
      if show(4) == 1
%        for j=1:size(voxels,2)
%          voxel(voxels(j).position,[latovoxel latovoxel latovoxel],'g',0.01);
%          hold on
% %          
%        end        
        
        temp=0;
        while temp<=space(1)
            h=patch([temp temp temp temp],[0 0 space(2) space(2)],[0 space(3) space(3) 0],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
        temp=0;
        while temp<=space(3)
            h=patch([0 space(1) space(1) 0],[0 0 space(2) space(2)],[temp temp temp temp],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
         temp=0;
        while temp<=space(2)
            h=patch([0 space(1) space(1) 0],[temp temp temp temp],[0 0 space(3) space(3)],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
            
            
            
      end

