function  filteredRes = proximityFilter(res)
%Usage:
%filteredRes = proximityFilter(res)
%To improve the systems detection abilities the results are filtered so
%that the largest set of postitions nearest each other are the ones used
%by the system.
%
%Inputs
%res: An array with 6 elements each of which is an array containing the
%possible detection points of each LED (returned by ledDetect)
%
%Outputs
%filteredRes: Same structure as the input, but each of the 6 arrays will
%only have one element chosen according the criteria outlined above

pThresh = 100^2; %The distance threshold used

for m =1:5, %For the first 5 sets of results

    %Get the number of points in the set
    [numRes,temp] = size(res(m).pos);

    for i = 1:numRes, %For each of the points in the set
        res1 = res(m).pos(i,:); %Read the point values
        if( ~isempty(size(res1))),
            xLoc1 = res1(1); %Get the x and y coords 
            yLoc1 = res1(2);
            
            for j = m+1:6,
               %For each of the remaining sets of points
               [numPos temp] = size(res(j).pos); %Get the number of points in the set
               
               for k =1:numPos,
                   %Get the x and y coords
                   res2 = res(j).pos(k,:); 
                   xLoc2 = res2(1);
                   yLoc2 = res2(2);
                   %Get the square of the differences
                   dist = (xLoc2-xLoc1)^2 + (yLoc2-yLoc1)^2;

                   if(dist < pThresh)
                      %The distance is less than the threshold
                      %Increment the proximity count in both records
                      res(m).pos(i,5) = res(m).pos(i,5) + 1;
                      res(j).pos(k,5) = res(j).pos(k,5) + 1;
                   end
               end
            end
        end
    end
end

for m = 1:6, %For each points set    
    resDet = res(m).pos;
    if isempty(resDet)
       filteredRes(m).pos = [];
    else
        %Sort the points in decereasing order of proximity count
        sorResDet = sortrows(resDet,-5);
        if sorResDet(5) >= 2
            %There is at least one other point nearby and this is the
            %point with the highest proximity count
            filteredRes(m).pos = sorResDet(1,:);
        else
            %Failed to detect the led
            filteredRes(m).pos = [];
        end
    end
     
end

