% POSSIBLE BUG FOUND: SHIT HAPPENS WHEN WE ARE BUILDING A WEDGE OUT OF
% CENTERS HAVING THE SAME RADII, ALSO IF ONLY 2 HAVE THE SAME RADII RESULT
% DOES NOT LOOK GOOD ...

clc
clear 
close all
addpath(genpath('../'))

%% SET UP POINTS

c1 = [10.5720; -0.7300; -0.6647];
c2 = [65.5720; -0.7300; -0.6647];
c3 = [10.5720; 30.7300; -0.6647];

r1= 11;
r2 = 11 ;
r3 = 9;


%% SET UP MODEL

centers = {c1,c2,c3};
radii = {r1,r2,r3};
blocks = {[1,2,3] };

% reindex stuff
[blocks] = reindex(radii, blocks);

display_model_debug(centers,blocks,radii, 0.9, 'big')


% %% data point
% 
% data_points = { [53.5720; -20.7300; 10.6647] };
% 
% %% compute projection
% 
% [indices, model_points, block_indices] = compute_projections_debug(data_points, centers,radii,blocks);
% 
% indices{1}
% block_indices{1}
% 
% %% display model & points
% 
% mypoints(data_points, [0.3, 0.8, 0.3], 20);
% mypoints(model_points, [0.3, 0.3, 0.8], 20);
% mylines(data_points, model_points, [0.85, 0.85, 0.85]);
