% QUESTIONS

% - how can indices become negative? what does it mean?
% indices are positive when point is projected on the top of the wedge
% (u1,u2,u3), negative when point is projected on the bottom (v1,v2,v3)

% - first step is to compute the jacobian of the tangent planes {u1,u2,u3},
% {v1,v2,v3}, is it correct??
% jacobian of tangent plane is correct!

% - Jacobian is not correct here:
% model point outputted by function is correct!

clc
clear 
close all

%% build triangle

c1 = [10.5720; -0.7300; -0.6647];
c2 = [23.0799; -13.3820; 29.8253];
c3 = [9.2448; -11.1630; 36.7268];

r1 = 13.3858;
r2 = 8.9624;
r3 = 10.1917;

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

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

c1 = centers{blocks{1}(1)};
c2 = centers{blocks{1}(2)};
c3 = centers{blocks{1}(3)};
r1 = radii{blocks{1}(1)};
r2 = radii{blocks{1}(2)};
r3 = radii{blocks{1}(3)};

variables = {'c1', 'c2', 'c3', 'r1', 'r2', 'r3'};

%% data point

data_points = { [30;28;25] };

%% compute projection

[indices, model_points, block_indices] = compute_projections_debug(data_points, centers,radii,blocks);

%% compute normal

[model_normals] = compute_model_normals(centers, radii, blocks, model_points, indices);

%% compare results

n = data_points{1} - model_points{1};
n = n/norm(n);

[n, model_normals{1}]

%% display model & points

display_model_debug(centers,blocks,radii, 0.9, 'big')
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]);

% compute tangent plane and Jacobians- everything is correct here
[v1, v2, v3, Jv1, Jv2, Jv3, u1, u2, u3, Ju1, Ju2, Ju3] = jacobian_tangent_plane(c1, c2, c3, r1, r2, r3, variables);

mypoints({v1,v2,v3}, [0.8, 0.8, 0.3], 30);
mypoints({u1,u2,u3}, [0.8, 0.3, 0.8], 30);

mylines({v1}, {v2}, [0.8, 0.8, 0.3]);
mylines({v2}, {v3}, [0.8, 0.8, 0.3]);
mylines({v1}, {v3}, [0.8, 0.8, 0.3]);

mylines({u1}, {u2}, [0.8, 0.3, 0.8]);
mylines({u2}, {u3}, [0.8, 0.3, 0.8]);
mylines({u1}, {u3}, [0.8, 0.3, 0.8]);

