% cams =struct('position',{[1 1 3]' [0 0 2]' [2 2 2]' [1 1 3]'},'angle_cam',...
%              {pi/6 pi/3 pi/6 pi/6}, 'angles',...
%              { [0 0 0]' [pi/4 0 0]' [0 pi/4 0]' [pi/4 pi/2 0]'} )

             

function [cams voxels] = match_camvox(cams,space,voxels,d_voxel)

%global voxels;  %%attento il nome è simile ala funzione voxels
% global cams;
% global latovoxel latox latoy latoz

% if mod(latox,latovoxel)<0.5
%         latovoxel=floor(latox/latovoxel)
%     else
%         latovoxel=ceil(latox/latovoxel)
% end
    

% if latovoxel/2 < dx || latovoxel/2 < dy || latovoxel/2 < dz
%     disp('cambia la dimensione di d: devo avere d > incertezza');
%     return
% end
% 

latox=space(1);
if size(space,1)==1   %sarebbe meglio creare delle var globali e rinomiarle: latox
    latoy=latox;        % latoy
    latoz=latox;        % latoz
else 
    latoy=space(2);        % latoy
    latoz=space(3);        % latoz
end
    % ..o creare il vettore sapce[latox, latoy, latoz] 

latovoxel=d_voxel;

%avendo messo un'etichetta che mi dice quale telecamera è...posso
%riordinare le telecamere in altri modi

%posso creare in cams un field che mi dice camere vicine e cam ortogonali

    voxels(1).setcameras=[];
    
    for i=1:size(cams,2)
        
        if i==1
            stopBar= progressbar(1/(size(cams,2)*size(voxels,2)),0);
        else
            stopBar= progressbar(((i-1)*size(voxels,2))/(size(cams,2)*size(voxels,2)),0);
        end
           
        
        %N.B. è una rotazione con assi mobili!!!!!!!!
        %non è molto elagante utiliazzare 'makehgtform' all'interno di questo 
        % codice, infatti, sebbene la matrice di rotazione risultante sia 
        % corretta e coincidente con quella trovata svolgendo tutti i calcoli, tale
        % funzione è in realtà impiegata nella trasformazione di oggetti grafici
        % ...ho cmq deciso di adottarla in quanto immediata e di facile
        % comprensione
        
%         Rx= makehgtform('xrotate',cams(i).angles(1))
%         Ry= makehgtform('yrotate',cams(i).angles(2))
%         Rz= makehgtform('zrotate',cams(i).angles(3))
%         R = Ry*Rx*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=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;

        %cams = rotationmatrix(cams);


        % 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,'angle_cam')
            angle_cam = cams(i).angle_cam;
        else 
            angle_cam= pi/3;       %in realtà dovrei passarlo dalla edit text box
        end

        if isfield(cams,'dist')
            h_max = cams(i).dist(1);         
            h_min = 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*(angle_cam/2);  %lato base maggiore tronco cono = fov_MAX
        d_m       = h_min*(angle_cam/2);  %lato base minore tronco cono = fov_min

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

        %vertici del tronco di piramide
        v =      [0 0 0;
                 -d_m/2 -d_m/2 h_min; 
                 -d_m/2 +d_m/2 h_min;
                 +d_m/2 +d_m/2 h_min; 
                 +d_m/2 -d_m/2 h_min;
                 -d_M/2 -d_M/2 h_max; 
                 -d_M/2 +d_M/2 h_max;
                 +d_M/2 +d_M/2 h_max; 
                 +d_M/2 -d_M/2 h_max;]';


        %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);

        %mentre i nuovi versori sono dati da:
        newversori = R_cam*versori + cams(i).position * ones(1,6);

        %cerco un punto sicuramente interno al cono:
        p_int = [0 0 h_min+abs((h_max-h_min)/2)];
        newp_int   = R_cam*p_int' + cams(i).position;

        %%in realtà non voglio num complessi allora dovrei definire
        %realdot = @(u, v) u*transpose(v);
        % 
        % normal = cross(v(:,2)-new_v(:,3), new_v(:,2)-new_v(:,4))
        % syms x y z
        % P = [x,y,z]
        % base_m = realdot(normal', P-new_v(:,2)')
        % 
        % normal = cross(new_v(:,6)-new_v(:,7), new_v(:,6)-new_v(:,7))
        % base_M = realdot(normal', P-new_v(:,6)')
        % 
        % normal  = cross(new_v(:,2)-new_v(:,6), new_v(:,2)-new_v(:,3))
        % lato_sx = realdot(normal', P-v(:,2)')
        % normal  = cross(new_v(:,4)-new_v(:,8), new_v(:,4)-new_v(:,5))
        % lato_dx = realdot(normal', P-new_v(:,4)')
        % normal  = cross(new_v(:,2)-new_v(:,6), new_v(:,2)-new_v(:,5))
        % fondo   = realdot(normal', P-new_v(:,2)')
        % normal  = cross(new_v(:,3)-new_v(:,7), new_v(:,3)-new_v(:,4))
        % cima    = realdot(normal', P-new_v(:,3)')
        %
        %%...ma funziona anche senza definirla

        %cerco le equazioni dei 6 piani che mi delimitano il tronco di piramide
        syms x y z;
        P = [x,y,z];
          normal = cross(new_v(:,2)-new_v(:,3), new_v(:,2)-new_v(:,5));
        base_m = dot(normal, P-new_v(:,2)');
          normal = cross(new_v(:,6)-new_v(:,7), new_v(:,6)-new_v(:,9));
        base_M = dot(normal, P-new_v(:,6)');
          normal  = cross(new_v(:,2)-new_v(:,3), new_v(:,2)-new_v(:,6));
        lato_sx = dot(normal, P-new_v(:,2)');
          normal  = cross(new_v(:,5)-new_v(:,4), new_v(:,5)-new_v(:,9));
        lato_dx = dot(normal, P-new_v(:,5)');
          normal  = cross(new_v(:,2)-new_v(:,6), new_v(:,2)-new_v(:,5));
        fondo   = dot(normal, P-new_v(:,2)');
          normal  = cross(new_v(:,3)-new_v(:,7), new_v(:,3)-new_v(:,4));
        cima    = dot(normal, P-new_v(:,3)');

        tronco_p  = [base_m, base_M ,lato_sx, lato_dx, fondo, cima ]'; 

        %inizializzo
        voxels_cam=[];

        %in realta potrei migliorarla senza vederli tutti mi fermo quando trovo che
        %non vi appartengono più        
        
        zona_int = subs(tronco_p,[x,y,z],[newp_int(1),newp_int(2),newp_int(3)])>0;
        % non è detto che sia interno!!! es se il tronco d piramide ha base
        % maggiore olto + grande delle dim del cubo => il suo centro non
        % appartiene al cubo di visibilità
       
        for j=1:size(voxels,2)
            stopBar= progressbar(((i-1)*size(voxels,2)+j)/(size(cams,2)*size(voxels,2)),0);
         
            %vedo da che parte dello spazio sta

                bound    = subs(tronco_p,[x,y,z],voxels(j).position')>0;

                %vedo cosa restituisce un punto che giace sicuramente all'interno del tronco
                %...(forse nn è necessario e basta vedere se bound è ==0)

                if bound == zona_int
                    %aggiungo il voxel corrente alla lista di quelli  visibili dalla 
                    %telecamera
                    
                    voxels_cam = [voxels_cam j]; %oppure [voxels_cam voxels(j).position]
                    
                    %aggiungo al campo 'setcameras' del voxel corrente la telecamera
                    %(ove 'setcameras' indica le telecamere che vedono il voxel)
                    
                    voxels(j).setcameras = [voxels(j).setcameras; i];
                    %
                end
            if (stopBar)
                break;
            end
        end
        if (stopBar) 
                break;
            end
         %aggiungo alla telecamera/e un campo con la lista dei voxel che vede
        cams(i).voxels=voxels_cam;  
  end

  
    
%%-----------------------------------------------------------------------%%