clear all
clc
% X_hat=[10 3;1 3;4 3];
profile on
space=[5 5 5];
d_min=2
d_max=5
X_hat=marker_generator(space,30);
dP=(0.05)^2*ones(1,size(X_hat,2));
cams=auto_position1(15,space,1,6);
lato=500;
[cams voxels]=space_partition2(cams,space,pi/3,0.2,d_min,d_max);
[cams voxels]=mark2vox(X_hat,cams,voxels,space,0.2);
for i=1:size(cams,2)
    cams(i).intrinsic=[1 0 lato/2;0 1 lato/2;0 0 1];
end
latovoxel=0.2;
X=real_marker(X_hat,dP);

for i=1:size(cams,2)
    [cams,P,rays,uv_hat]= im_simulation(X_hat,X,dP,cams,i,lato,0.1,pi/3);
    Real=genera_piano_3D(P, lato, lato, d_min, d_max, 50, 10000);
    cams(i).realimg=Real;
    cams(i).fictiousimg=[uv_hat;rays];
    
    % figure;
    % camso=camso(1);
    % geps=camso(1);
    % % figure(2)
    % % grid on
    % % plotcam3(geps,space,voxels,latovoxel,[1 1 1 0 0 0 0])
    % % view([75,30])
    % % grid on
    % % axis equal
    % % hold on
    % for i=1:size(X_hat,2)
    % % DrawCuboid([.2 .2 .2], mark(:,i),[0 0 0],'m',1)
    % plot3(X_hat(1,i),X_hat(2,i),X_hat(3,i),'go')
    % hold on
    % end

    [A,blob] = im_overlap(X_hat,dP,cams,i,Real,lato,pi/3);
    cams(i).setmarkers(2,:) = A(2,:);
    
    % %
    % % for i=1:size(camso(1).setmarkers,2)
    % % % DrawCuboid([.2 .2 .2], mark(:,i),[0 0 0],'m',1)
    % % plot3(X_Hat(1,camso(1).setmarkers(i)),X_Hat(2,camso(1).setmarkers(i)),X_Hat(3,camso(1).setmarkers(i)),'go')
    % % hold on
    % % end
    % % 
    % 
%     figure(61);
%     imshow(Fictious);
% 
%     figure(62);
%     imshow(Fictiouscntrd);

%     figure;
%     imshow(Real);
%     hold on
%     for j=1:size(uv_hat,2)
%     %     PlotCircle(uv_hat(1,i),uv_hat(2,i),rays(i),10000,color(i,:))
%         PlotCircle(uv_hat(1,j),uv_hat(2,j),rays(j),10000,'r');
%         hold on;
%     end
end
%%
for i=1:size(cams,2)
    cams(i).versore = cams(i).rotationmat*[0 0 1]';
end

C=zeros(size(cams,2),size(cams,2));
for i=1:size(cams,2)
    for j=(i+1):size(cams,2)
        [c mark_comuni mark_rimast]=costo1(cams,i,j);
        C(i,j)=c;
        C(j,i)=c;
    end
end
%
% C=zeros(size(provaR,2),size(provaR,2));
% for i=1:size(provaR,2)
%     for j=(i+1):size(provaR,2)
%         [c mark_comuni mark_rimast]=costo1(provaR,i,j);
%         C(i,j)=c;
%         C(j,i)=c;
%     end
% end



Tree=association_tree_mod(C);
Rate= rate4level3(cams,Tree)
profilo = profile('info');
profile off
profile viewer
