function [out,nf] = compute_fix(fix, threshold)
% compute_fix - routine to compute final LBL fix given six pairwise fixes.

out = [];                  

% MIN_NUM_FIXES = 3;
MIN_NUM_FIXES = 2;
DEF_THRESHOLD = 20;
debug = 0;

if (nargin < 1) 
    disp('error in compute_fix, no input arguments')
    nargin
    return
else 
    if (nargin < 2)
       threshold = DEF_THRESHOLD;
    end
end


if (size(fix) ~= [6,2]) 
    disp('error in compute_fix, size of fix ~= [6,2]')
    return
end
if (size(threshold)~=[1,1]) 
    disp('error in compute_fix, size of fix ~= [1,1]')
    size(threshold)
    return
end

nf = 6;

% eliminate NaN's from fix;
nan_check = isnan(fix);
nan_count = length(find(isnan(fix)))/2;


nf = 6 - nan_count;

if (nf < MIN_NUM_FIXES)    % return the NULL matrix if nf < 3 
    return
end

fix2 = zeros(nf,2);
count = 1;
for i=1:6
    if (isnan(fix(i,1))==0)
        fix2(count,:) = fix(i,:);
        count = count + 1;
    end
end

nf = length(fix2);

% if (nf < 6) 
%    disp([num2str(nan_count), ' fixes are NaN'])
%     fix2
% end

% Algorithm: 
% while 
%    if nf < min_num_fixes return NULL
%    if dev < threhold, then return the mean
%    else discard fix furthest from mean

count=0;

sfix2 = size(fix2);
lenfix2 = sfix2(1);
while (lenfix2 >= MIN_NUM_FIXES) 

    count = count + 1;
    out=mean(fix2);
    s=std(fix2);					
    dev=sqrt(s(1,1)^2+s(1,2)^2);		% STD of New Fix

%    disp(['using ', num2str(length(fix2)), ' fixes dev = ', num2str(dev)])
    
    if (dev < threshold) 
        if (debug==1) 
            disp(['fix accepted, nf = ', num2str(length(fix2)), ' dev = ', ...
                 num2str(dev)]);
        end
        return
    end

    e = ones(length(fix2), 1);
    diff = fix2 - e*out;
    tdiff = diff(:,1).^2 + diff(:,2).^2;
    [MAX,I]=max(tdiff);
    fix2(I,:)=[];    
    % nf = length(fix2);

    sfix2 = size(fix2);   % because length gets in trouble for size=1,2
    lenfix2 = sfix2(1);
    nf = lenfix2;

end

out = [];

if (debug==1) 
    disp(['fix rejected, nf = ', num2str(length(fix2)), ' dev = ', ...
             num2str(dev), ' *****']);
end

return
