function [raggi punti]=associa3(raggi_1,raggi_2, punti_1, punti_2,epsilon,dim_stanza_x,dim_stanza_y,d_max)

%Questa funzione tiene conto dei raggi usati e invece di scartarli li usa
%per migliorare la ricostruzioeìne del punto ricalcolandone la minima
%distanza dai raggi.

%punti sarà in formato cella. ogni cella contiene a sua volta un vettore di 
%celle dove la cella 1 contiene il punto ricostrito
%mentre le seguenti celle contegono i raggi usati per ricostruirlo. 

%Confronto raggi_1 con punti_2

for i=1:length(punti_2)  %indice che scorre i punti
   if punti_2{i}{1}(1)~=-1
    for j=1:length(raggi_1)    %indice che scorre le tecamere
          
       bound=length(raggi_1{j}(:,1));
       k=1; %indice che scorre i raggi
       while k<bound+1
           d=distanza_punto_retta(punti_2{i}{1}(1:2),raggi_1{j}(k,1:3));
         if (distanza_punto_retta(punti_2{i}{1}(1:2),raggi_1{j}(k,1:3))<epsilon  && distanza_punto_punto(punti_2{i}{1}(1:2),[raggi_1{j}(k,4) raggi_1{j}(k,5)]))
             %ricalcolo punto con il nuovo raggio
             R=[raggi_1{j}(k,1:3)];
             for t=2:length(punti_2{i})
             R=[R ; punti_2{i}{t}(1:3)];
             end
             dr=max([d punti_2{i}{1}(3)]);
             [x y]=min_quad_gen(R);
             %aggiorno punto
             punti_2{i}{1}=[x y dr];
             %Aggiungo raggio
             punti_2{i}=[punti_2{i} , raggi_1{j}(k,:)];
             %elimino raggio
             raggi_1{j}=[raggi_1{j}(1:(k-1),:) ; raggi_1{j}((k+1):length(raggi_1{j}(:,1)),:)]; %elimino raggio
             bound=length(raggi_1{j}(:,1)); %avendo eliminato raggio, array più corto e quindi aggiorno il bound
         else
             k=k+1; 
         end
      end 
    end
   end
end
%Confronto raggi_2 con punti_1

for i=1:length(punti_1)  %indice che scorre i punti
    if punti_1{i}{1}(1)~=-1
    for j=1:length(raggi_2)    %indice che scorre le tecamere
          
       bound=length(raggi_2{j}(:,1));
       k=1; %indice che scorre i raggi
       while k<bound+1
           d=distanza_punto_retta(punti_1{i}{1}(1:2),raggi_2{j}(k,1:3));
         if (distanza_punto_retta(punti_1{i}{1}(1:2),raggi_2{j}(k,1:3))<epsilon && distanza_punto_punto(punti_1{i}{1}(1:2),[raggi_2{j}(k,4) raggi_2{j}(k,5)]))
             %ricalcolo punto con il nuovo raggio
             R=[raggi_2{j}(k,1:3)];
             for t=2:length(punti_1{i})
             R=[R ; punti_1{i}{t}(1:3)];
             end
             [x y]=min_quad_gen(R);
             dr=max([d punti_1{i}{1}(3)]);
             %aggiorno punto
             punti_1{i}{1}=[x y dr];
             %Aggiungo raggio
             punti_1{i}=[punti_1{i} , raggi_2{j}(k,:)];
             %elimino raggio
             raggi_2{j}=[raggi_2{j}(1:(k-1),:) ; raggi_2{j}((k+1):length(raggi_2{j}(:,1)),:)]; %elimino raggio
             bound=length(raggi_2{j}(:,1)); %avendo eliminato raggio, array più corto e quindi aggiorno il bound
         else
             k=k+1; 
         end
      end 
    end
    end
end

%Confronto raggi a 3 a 3
%Confronto 2 raggi di raggi_1 e uno di raggi_2

for i=1:(length(raggi_1)-1) %telecamera 1 da raggi_1
    for j=(i+1):length(raggi_1) %telecamera 2 da raggi_1
        for k=1:length(raggi_2)  %telecamera 3 da raggi_2
           bound1=length(raggi_1{i}(:,1)); %numero raggi telecamera 1
           bound2=length(raggi_1{j}(:,1)); %numero raggi telecamera 2           
           bound3=length(raggi_2{k}(:,1)); %numero raggi telecamera 3
           ii=1; %indice raggi telecamera i/1
           while ii<bound1+1
               jj=1; %indice raggi telecamera j/2
               while jj<bound2+1
                   kk=1; %indice raggi telecamera k/3
                   while kk<bound3+1
                       [x y] = min_quad( raggi_1{i}(ii,1:3) ,raggi_1{j}(jj,1:3) ,raggi_2{k}(kk,1:3) ); %calcolo il punto di minima distanza
                       dt1=distanza_punto_punto([x y],[raggi_1{i}(ii,4) raggi_1{i}(ii,5)]);
                       dt2=distanza_punto_punto([x y],[raggi_1{j}(jj,4) raggi_1{j}(jj,5)]);
                       dt3=distanza_punto_punto([x y],[raggi_2{k}(kk,4) raggi_2{k}(kk,5)]);                 
                       if (x>0 && y>0 && x<dim_stanza_x && y<dim_stanza_y && dt1<=d_max && dt2<=d_max && dt3<=d_max) %controllo che il punto sia valido ovvero interno alla stanza
                         d1=distanza_punto_retta([x y], raggi_1{i}(ii,1:3));  %calcolo distanze punto retta
                         d2=distanza_punto_retta([x y], raggi_1{j}(jj,1:3));
                         d3=distanza_punto_retta([x y], raggi_2{k}(kk,1:3));
                         d=max([d1 d2 d3]); %considero distanza maggiore
                         if  d<epsilon
                            %salvo il punto e i raggi usati per
                            %ricostruirlo
                            punto=cell(1,4);
                            punto{1}=[x y d];
                            punto{2}=raggi_1{i}(ii,1:3);
                            punto{3}=raggi_1{j}(jj,1:3);
                            punto{4}=raggi_2{k}(kk,1:3);
                            punti_1 {length(punti_1) + 1} = punto;
                            %elimino i raggi usati
                            raggi_1{i}=[raggi_1{i}(1:(ii-1),:) ; raggi_1{i}((ii+1):length(raggi_1{i}(:,1)),:)]; %accorcio i raggi
                            raggi_1{j}=[raggi_1{j}(1:(jj-1),:) ; raggi_1{j}((jj+1):length(raggi_1{j}(:,1)),:)];
                            raggi_2{k}=[raggi_2{k}(1:(kk-1),:) ; raggi_2{k}((kk+1):length(raggi_2{k}(:,1)),:)];
                            bound1=length(raggi_1{i}(:,1)); %aggiorno i bound
                            bound2=length(raggi_1{j}(:,1));            
                            bound3=length(raggi_2{k}(:,1));                          
                            kk=bound3;
                            jj=bound2;
                         end
                       end
                       kk=kk+1;
                   end
                   jj=jj+1;
               end
               ii=ii+1;
           end
        end
    end
end

%Confronto 1 raggio di raggi_1 e due di raggi_1

for i=1:(length(raggi_2)-1) %telecamera 1 da raggi_1
    for j=(i+1):length(raggi_2) %telecamera 2 da raggi_1
        for k=1:length(raggi_1)  %telecamera 3 da raggi_2
           bound1=length(raggi_2{i}(:,1)); %numero raggi telecamera 1
           bound2=length(raggi_2{j}(:,1)); %numero raggi telecamera 2           
           bound3=length(raggi_1{k}(:,1)); %numero raggi telecamera 3
           ii=1; %indice raggi telecamera i/1
           while ii<bound1+1
               jj=1; %indice raggi telecamera j/2
               while jj<bound2+1
                   kk=1; %indice raggi telecamera k/3
                   while kk<bound3+1
                       [x y] = min_quad( raggi_2{i}(ii,1:3) ,raggi_2{j}(jj,1:3) ,raggi_1{k}(kk,1:3) ); %calcolo il punto di minima distanza
                       dt1=distanza_punto_punto([x y],[raggi_2{i}(ii,4) raggi_2{i}(ii,5)]);
                       dt2=distanza_punto_punto([x y],[raggi_2{j}(jj,4) raggi_2{j}(jj,5)]);
                       dt3=distanza_punto_punto([x y],[raggi_1{k}(kk,4) raggi_1{k}(kk,5)]);
                       if (x>0 && y>0 && x<dim_stanza_x && y<dim_stanza_y && dt1<=d_max && dt2<=d_max && dt3<=d_max) %controllo che il punto sia valido ovvero interno alla stanza
                         d1=distanza_punto_retta([x y], raggi_2{i}(ii,1:3));  %calcolo distanze punto retta
                         d2=distanza_punto_retta([x y], raggi_2{j}(jj,1:3));
                         d3=distanza_punto_retta([x y], raggi_1{k}(kk,1:3));
                         d=max([d1 d2 d3]); %considero distanza maggiore
                         if  d<epsilon
                            %salvo il punto e i raggi usati per
                            %ricostruirlo
                            punto=cell(1,4);
                            punto{1}=[x y d];
                            punto{2}=raggi_2{i}(ii,1:3);
                            punto{3}=raggi_2{j}(jj,1:3);
                            punto{4}=raggi_1{k}(kk,1:3);
                            %punti_2=[punti_2 , punto];
                            punti_2{length(punti_2) + 1}=punto;
                            %elimino i raggi usati
                            raggi_2{i}=[raggi_2{i}(1:(ii-1),:) ; raggi_2{i}((ii+1):length(raggi_2{i}(:,1)),:)]; %accorcio i raggi
                            raggi_2{j}=[raggi_2{j}(1:(jj-1),:) ; raggi_2{j}((jj+1):length(raggi_2{j}(:,1)),:)];
                            raggi_1{k}=[raggi_1{k}(1:(kk-1),:) ; raggi_1{k}((kk+1):length(raggi_1{k}(:,1)),:)];
                            bound1=length(raggi_2{i}(:,1)); %aggiorno i bound
                            bound2=length(raggi_2{j}(:,1));            
                            bound3=length(raggi_1{k}(:,1));                          
                            kk=bound3;
                            jj=bound2;
                         end
                       end
                       kk=kk+1;
                   end
                   jj=jj+1;
               end
               ii=ii+1;
           end
        end
    end
end

%merge raggi e punti
punti=[punti_1 punti_2];
raggi=[raggi_1 raggi_2];

end
