%Ricostruzione mediante minimi quadrati pesati e mantiene raggi

%Costruisco le telecamere

T1P=[a{1}' b{1}' c{1}' num_pixel_centri{1}' x_f_as(1)*ones(length(a{1}),1) y_f_as(1)*ones(length(a{1}),1)];
T2P=[a{2}' b{2}' c{2}' num_pixel_centri{2}' x_f_as(2)*ones(length(a{2}),1) y_f_as(2)*ones(length(a{2}),1)];
T3P=[a{3}' b{3}' c{3}' num_pixel_centri{3}' x_f_as(3)*ones(length(a{3}),1) y_f_as(3)*ones(length(a{3}),1)];
T4P=[a{4}' b{4}' c{4}' num_pixel_centri{4}' x_f_as(4)*ones(length(a{4}),1) y_f_as(4)*ones(length(a{4}),1)];
T5P=[a{5}' b{5}' c{5}' num_pixel_centri{5}' x_f_as(5)*ones(length(a{5}),1) y_f_as(5)*ones(length(a{5}),1)];
T6P=[a{6}' b{6}' c{6}' num_pixel_centri{6}' x_f_as(6)*ones(length(a{6}),1) y_f_as(6)*ones(length(a{6}),1)];
T7P=[a{7}' b{7}' c{7}' num_pixel_centri{7}' x_f_as(7)*ones(length(a{7}),1) y_f_as(7)*ones(length(a{7}),1)];
T8P=[a{8}' b{8}' c{8}' num_pixel_centri{8}' x_f_as(8)*ones(length(a{8}),1) y_f_as(8)*ones(length(a{8}),1)];
T9P=[a{9}' b{9}' c{9}' num_pixel_centri{9}' x_f_as(9)*ones(length(a{9}),1) y_f_as(9)*ones(length(a{9}),1)];
T10P=[a{10}' b{10}' c{10}' num_pixel_centri{10}' x_f_as(10)*ones(length(a{10}),1) y_f_as(10)*ones(length(a{10}),1)];
T11P=[a{11}' b{11}' c{11}' num_pixel_centri{11}' x_f_as(11)*ones(length(a{11}),1) y_f_as(11)*ones(length(a{11}),1)];
T12P=[a{12}' b{12}' c{12}' num_pixel_centri{12}' x_f_as(12)*ones(length(a{12}),1) y_f_as(12)*ones(length(a{12}),1)];
T13P=[a{13}' b{13}' c{13}' num_pixel_centri{13}' x_f_as(13)*ones(length(a{13}),1) y_f_as(13)*ones(length(a{13}),1)];
T14P=[a{14}' b{14}' c{14}' num_pixel_centri{14}' x_f_as(14)*ones(length(a{14}),1) y_f_as(14)*ones(length(a{14}),1)];
T15P=[a{15}' b{15}' c{15}' num_pixel_centri{15}' x_f_as(15)*ones(length(a{15}),1) y_f_as(15)*ones(length(a{15}),1)];
T16P=[a{16}' b{16}' c{16}' num_pixel_centri{16}' x_f_as(16)*ones(length(a{16}),1) y_f_as(16)*ones(length(a{16}),1)];



%Primo passo dell'albero unisco solo 2 telecamere

R11Pa2=cell(1,2);
R21Pa2=cell(1,2);
R31Pa2=cell(1,2);
R41Pa2=cell(1,2);
R51Pa2=cell(1,2);
R61Pa2=cell(1,2);
R71Pa2=cell(1,2);
R81Pa2=cell(1,2);


%I punti ricostruiti saranno invece di tipo array Nx2 (Nx[x y])

%Associo le telecamere ricordando che al primo passo nn vi sono
%ricostruzioni

R11Pa2{1}=T1P;
R11Pa2{2}=T2P;
R21Pa2{1}=T3P;
R21Pa2{2}=T4P;
R31Pa2{1}=T5P;
R31Pa2{2}=T6P;
R41Pa2{1}=T7P;
R41Pa2{2}=T8P;
R51Pa2{1}=T9P;
R51Pa2{2}=T10P;
R61Pa2{1}=T11P;
R61Pa2{2}=T12P;
R71Pa2{1}=T13P;
R71Pa2{2}=T14P;
R81Pa2{1}=T15P;
R81Pa2{2}=T16P;

P11Pa2=[-1 -10 -1];

%dimensione stanza
dim_stanza_x=10000;
dim_stanza_y=10000;

%secondo passo dell'albero
tmpmed=0;
tic
[R12Pa2 P12Pa2]=associa2_pes(R11Pa2, R21Pa2, P11Pa2, P11Pa2,epsilon,dim_stanza_x,dim_stanza_y,d_max);
t1=toc;
set(S.tP1R4,'string',t1,'visible','on')
tic
[R22Pa2 P22Pa2]=associa2_pes(R31Pa2, R41Pa2, P11Pa2, P11Pa2,epsilon,dim_stanza_x,dim_stanza_y,d_max);
t2=toc;
set(S.tP1R3,'string',t2,'visible','on')
tic
[R32Pa2 P32Pa2]=associa2_pes(R51Pa2, R61Pa2, P11Pa2, P11Pa2,epsilon,dim_stanza_x,dim_stanza_y,d_max);
t3=toc;
set(S.tP1R2,'string',t3,'visible','on')
tic
[R42Pa2 P42Pa2]=associa2_pes(R71Pa2, R81Pa2, P11Pa2, P11Pa2,epsilon,dim_stanza_x,dim_stanza_y,d_max);
t4=toc;
set(S.tP1R1,'string',t4,'visible','on')

tmpmed=max([t1 t2 t3 t4]);
%terzo passo dell'albero

tic
[R13Pa2 P13Pa2]=associa2_pes(R12Pa2, R22Pa2, P12Pa2, P22Pa2,epsilon,dim_stanza_x,dim_stanza_y,d_max);
t1=toc;
set(S.tP2R2,'string',t1,'visible','on')
tic
[R23Pa2 P23Pa2]=associa2_pes(R32Pa2, R42Pa2, P32Pa2, P42Pa2,epsilon,dim_stanza_x,dim_stanza_y,d_max);
t2=toc;
set(S.tP2R1,'string',t2,'visible','on')
tmpmed=tmpmed+max([t1 t2]);

%quarto passo dell'albero

tic
[R14Pa2 P14Pa2]=associa2_pes(R13Pa2, R23Pa2, P13Pa2, P23Pa2,epsilon,dim_stanza_x,dim_stanza_y,d_max);
tmpo=toc;
set(S.tP3R1,'string',tmpo,'visible','on')
tmpmed=tmpmed+tmpo;
set(S.Ttot,'string',tmpmed,'visible','on')

