clear all
close all

% load('CameraPointsLB_20160414.mat')
% load('PAindex_20160414_Phantom_LB.mat')
% load('PAindex_20160414_Phantom_LB_new015.mat')
% load('PAindex_20160414_Phantom_LB_new005.mat')

% load('CameraPointsLU_20160414.mat')
% load('PAindex_20160414_Phantom_LU.mat')
% load('PAindex_20160414_Phantom_LU_new015.mat')
% load('PAindex_20160414_Phantom_LU_new005.mat')

% load('CameraPointsRB_20160414.mat')
% load('PAindex_20160414_Phantom_RB.mat')
% load('PAindex_20160414_Phantom_RB_new015.mat')
% load('PAindex_20160414_Phantom_RB_new005.mat')

load('CameraPointsRU_20160414.mat')
% load('PAindex_20160414_Phantom_RU.mat')
% load('PAindex_20160414_Phantom_RU_new015.mat')
load('PAindex_20160414_Phantom_RU_new005.mat')

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

% choices = 1:20;

%%% For LB
% choices = [1 2 3 4 5 6 7 8 9 12 15 16 19 20]; %% Cut no. 10, 11, 13, 14, 17, 18
% choices = [1 2 3 4 5 6 7 8 9 10 11 12 15 16 18 19 20]; %% Cut no. 13, 14, 17

%%% For LU
% choices = [1 2 3 6 7 8 9 10 11 13 15 18 19 20]; %% Cut no. 4, 5, 12, 14, 16, 17
% choices = [1 2 3 4 5 6 7 8 9 10 11 12 13 15 18 19 20]; %% Cut no. 14, 16, 17

%%% For RB
% choices = [2 4 5 6 7 8 9 11 12 14 16 17 19 20]; %% Cut no. 1, 3, 10, 13, 15, 18
% choices = [1 2 4 5 6 7 8 9 11 12 14 15 16 17 18 19 20]; %% Cut no. 3, 10, 13

%%% For RU
% choices = [1 3 4 5 6 7 8 9 10 11 14 15 17 18 19 20]; %% Cut no. 2, 12, 13, 16
choices = [1 3 4 5 6 7 8 9 10 11 12 14 15 17 18 19 20]; %% Cut no. 2, 13, 16

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

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

%%% Remove Outliers for LB (7 Points)
% delete = [1 116 21 27 11 17 6];

%%% Remove Outliers for LU (8 Points) Option#1
% delete = [21 13 113 74 46 119 28 80];

%%% Remove Outliers for LU (29 Points) Option#2
% delete = [45 211 196 89 179 128 210 223 195 178 209 194 238 222 240 239 177 106 99 68 61 21 13 113 74 46 119 28 80];

%%% Remove Outliers for RB (7 Points)
% delete = [77 234 253 48 61 252 100];

%%% Remove Outliers for RU (13 Points)
delete = [26 148 263 14 152 160 147 63 105 15 151 146 64];

keepInt(:,delete) = [];
point = mean(keepInt,2);

%%% Edit the number of count
% diffP = keepInt - repmat(point,[1 (count-1)]); %Original
% diffP = keepInt - repmat(point,[1 (count-1-7)]); %LB
% diffP = keepInt - repmat(point,[1 (count-1-8)]); %LU Option#1
% diffP = keepInt - repmat(point,[1 (count-1-29)]); %LU Option#2
% diffP = keepInt - repmat(point,[1 (count-1-7)]); %RB
diffP = keepInt - repmat(point,[1 (count-1-13)]); %RU

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)),'.')
