clear all
close all

% load('CameraPoints20160314_2.mat')
% load('PA20160314_2.mat')

% load('CameraPoints_20160319_Phantom.mat')
% load('PAToF_20160319_Phantom.mat')
load('PAindex_20160319_Heart.mat')
% load('3DLwithoutOutliers.mat')
load('3DLafterRegression.mat')
% load('PAindex_20160319_Phantom.mat')
% load('PAindex_20160319_Phantom_new015.mat')
% load('PAindex_20160319_Phantom_new005.mat')

samp = 40e6;
sos = 1470;
delay = 6e-6;
shift = -200;

% choices = [1 2 3 4 5 8 10];
% choices = 1:30;
choices = 1:15;

minLength = 5;
minAngle = 10;
keepInt = [];
count = 1;

% ind = ToF_mean_array;
ind = ind_array;

permuteChoices = nchoosek(choices,3)';

n = size(permuteChoices,2);

for i = 1:n
    centers = point3DL(:,permuteChoices(:,i));
    
    [cond] = checkTriangleCondition( centers, minLength, minAngle );
    if cond == 0
        continue
    end
    radii = (ind(permuteChoices(:,i))+delay*samp+shift)*sos/samp*1000;
    intersections{count} = Trilateration( centers, radii );
    if isreal(intersections{count}) ~= 1
        continue
    end
    if intersections{count}(3,1) > intersections{count}(3,2)
        keepInt = [keepInt intersections{count}(:,1)];
    else
        keepInt = [keepInt intersections{count}(:,2)];
    end
    useIndex(:,count) = permuteChoices(:,i);
    count = count + 1;
end

% while count <= trials
% %     useIndex(:,count) = [2; 3; 7];
%     permuteChoices = randperm(length(choices));
%     useIndex(:,count) = choices(permuteChoices(1:3));
%     centers = point3DL(:,useIndex(:,count));
% %     [ col ] = checkCollinear( centers );
% %     if col == 1
% %         continue
% %     end
%     [cond] = checkTriangleCondition( centers, minLength, minAngle );
%     if cond == 0
%         continue
%     end
%     radii = (ind(useIndex(:,count))+delay*samp+shift)*sos/samp*1000;
%     intersections{count} = Trilateration( centers, radii );
%     if isreal(intersections{count}) ~= 1
%         continue
%     end
%     if intersections{count}(3,1) > intersections{count}(3,2)
%         keepInt = [keepInt intersections{count}(:,1)];
%     else
%         keepInt = [keepInt intersections{count}(:,2)];
%     end
%     count = count + 1;
% end

point = mean(keepInt,2)
diffP = keepInt - repmat(point,[1 (count-1)]);
dists = sqrt(diffP(1,:).^2+diffP(2,:).^2+diffP(3,:).^2);
[sortDists, sortInd] = sort(dists);
mean(sortDists)
std(sortDists)
figure;plot3(keepInt(1,sortInd(1:end)),keepInt(2,sortInd(1:end)),keepInt(3,sortInd(1:end)),'.')