function [A, blob] = im_overlap(X_hat,dP,cams,camera,obj,lato,theta) %,fmt
%INPUT  = X_hat is a 3*N array, where N is the number of markers, that
%               contains the markers positions predictions
%         dP is the array that contains markers positions variances
%         K is the intrinsic parameters matrix of the camera
%         R,T are the extrinsic parameters of the camera  
%         camera is the label of the camera
%         mode is the type of algorithm used to calculate the markers
%               probability
%         lato is the number of x_pixel=y_pixel;
%OUTPUT = list of the markers seen by the camera with their probability
%cams ha campi position,angles,voxel                                       
%voxel ha campi cameras

[m,n]=size(cams(camera).setmarkers);%number of markers seen by the camera
num_marker=n;
A=zeros(2,num_marker);
A(1,:)=cams(camera).setmarkers(1,:);


x_hat=X_hat(:,cams(camera).setmarkers(1,:));
dp=dP(cams(camera).setmarkers(1,:));
f=(lato/2)*(cos(theta/2)/sin(theta/2));


K=cams(camera).intrinsic;
R=cams(camera).rotationmat;
T=cams(camera).position;

[uv_hat, x_cam, hidden, distance]=camera_simulation(R,T,x_hat,K,f,lato,lato);

idealmarker(1).center=[];
idealmarker(1).uncrt=[];
idealmarker(1).blob=[];

for i=1:n
%     X_cam=[R' -R'*T]*[x_hat(:,i);1];
    X_cam=x_cam(1:3,:);
%     temp=P*[x_hat(:,i);1]; %'seen' markers projection on image plane
    idealmarker(i).center=uv_hat(:,i);
    d=sqrt(X_cam(:,1)'*X_cam(:,1));
    ray=(3*dp(i)^0.5)*(f/d);
    idealmarker(i).uncrt=ray;      %uncertainty in pixel
end
% Estraction the blobs from a image.
if ~isnumeric(obj)
    originalImage=imread(obj);
    else
        originalImage=obj;
end
level=graythresh(originalImage);
% computes a global threshold (LEVEL) that can be
%     used to convert an intensity image to a binary image with IM2BW. LEVEL
%     is a normalized intensity value that lies in the range [0, 1].
binaryImage=im2bw(originalImage,level); % Threshold to binary
labeledImage = bwlabel(binaryImage, 8); % Label each blob so can do calc on it
blobMeasurements = regionprops(labeledImage, 'centroid'); % Get the blob centroids
numberOfBlobs = size(blobMeasurements, 1);
blob=zeros(2,numberOfBlobs);
for i=1:numberOfBlobs
    blob(:,i)=blobMeasurements(i).Centroid';
end
% 
% 
% if isnumeric(obj)
%     
%     blobMeasurements = regionprops(obj, 'centroid'); % Get the blob centroids
%     numberOfBlobs = size(blobMeasurements, 1);
%     blob=zeros(2,numberOfBlobs);
%     for i=1:numberOfBlobs
%         blob(:,i)=blobMeasurements(i).Centroid';
%     end
% end
% 
bfromim = inf(numberOfBlobs,num_marker+1);  %blob vs ideal markers and a counter
bfromim(:,end)=zeros(numberOfBlobs,1);



% % plot predizioni
% figure
% imagesc(originalImage); colormap(gray(256))
% hold on
% scatter(uv_hat(1,:),uv_hat(2,:),'green');
% hold on
% %plot centroidi
% blob=zeros(2,numberOfBlobs);
% for i=1:numberOfBlobs
%     blob(:,i)=blobMeasurements(i).Centroid;
% end
% scatter(blob(1,:),blob(2,:),'red');




for i=1:numberOfBlobs
    sum=0;
    for j=1:num_marker
        distance=norm(idealmarker(j).center-blob(:,i));
        if distance<=3*idealmarker(j).uncrt
            bfromim(i,j)=distance/idealmarker(j).uncrt;
            bfromim(i,end)=bfromim(i,end)+1;
            sum=sum+bfromim(i,j);
        end
    end
    for j=1:num_marker
        bfromim(i,j)=bfromim(i,j)/sum;
    end
end

% if mode=='q'
    for i=1:numberOfBlobs
        if bfromim(i,end)==1
            count=1;
            while bfromim(i,count)==inf & count<=num
                count=count+1;
            end
            A(2,count)=1;  %the index-th marker is seen with probability 1
            for j=1:numberOfBlobs
                bfromim(j,count)=Inf;
            end
        end        

        if bfromim(i,end)>1
            min=Inf;
            index=0;
            for j=1:num_marker
                if bfromim(i,j)<min
                    min=bfromim(i,j);
                    index=j;
                end
            end
            if index>0
                for j=1:numberOfBlobs
                    bfromim(j,index)=Inf;
                end
    %             ErrProb=0;
    %             for j=1:num_marker
    %                 if bfromim(i,j)<Inf
    %                     ErrProb=ErrProb+erfc(bfromim(i,j)/2^0.5)/2;
    %                 end
    %             end
                A(2,index)=1-min;
            end
        end
    end
end