% 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];

c1 =[-15.5106;-7.8140; 6.3086];

c2 =[   -25.2949; -12.7870; 42.9622];

c3 =  [  -23.0970; -4.9150; 30.6650];

r1 =11.6440;

r2 =4.4666;

r3 = 6.5883;





% 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);

%% 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]);

%% compute analytical jacobians
n = data_points{1} - model_points{1};
n = n/ norm(n);



% check to which plane is the point projected to
if(indices{1}(1) > 0)
    [q, dq] = jacobian_convtriangle(data_points{1}, v1, v2, v3, Jv1, Jv2, Jv3, variables);
else
    [q, dq] = jacobian_convtriangle(data_points{1}, u1, u2, u3, Ju1, Ju2, Ju3, variables);
end


diff_c = { dq.dc1, dq.dc2, dq.dc3};
diff_r = { dq.dr1, dq.dr2, dq.dr3};


%% compute numerical jacobians
epsilon = 1e-04;

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

for i = 1 : 3
    
    centers_plus_x = centers;
    centers_minus_x = centers;
    centers_plus_y = centers;
    centers_minus_y = centers;
    centers_plus_z = centers;
    centers_minus_z = centers;
    
    centers_plus_x{i} = centers_plus_x{i} + epsilon*[1;0;0];
    centers_minus_x{i} = centers_minus_x{i} - epsilon*[1;0;0];
    centers_plus_y{i} = centers_plus_y{i} + epsilon*[0;1;0];
    centers_minus_y{i} = centers_minus_y{i} - epsilon*[0;1;0];
    centers_plus_z{i} = centers_plus_z{i} + epsilon*[0;0;1];
    centers_minus_z{i} = centers_minus_z{i} - epsilon*[0;0;1];
    
    [~, model_points_plus_x, ~] = compute_projections_debug(data_points, centers_plus_x,radii,blocks);
    [~, model_points_minus_x, ~] = compute_projections_debug(data_points, centers_minus_x,radii,blocks);
    [~, model_points_plus_y, ~] = compute_projections_debug(data_points, centers_plus_y,radii,blocks);
    [~, model_points_minus_y, ~] = compute_projections_debug(data_points, centers_minus_y,radii,blocks);
    [~, model_points_plus_z, ~] = compute_projections_debug(data_points, centers_plus_z,radii,blocks);
    [~, model_points_minus_z, ~] = compute_projections_debug(data_points, centers_minus_z,radii,blocks);
    
    disp(strcat('Derivatives with respect to c', num2str(i), ':'))
    [ (model_points_plus_x{1} - model_points_minus_x{1})/(2*epsilon), (model_points_plus_y{1} - model_points_minus_y{1})/(2*epsilon), (model_points_plus_z{1} - model_points_minus_z{1})/(2*epsilon), diff_c{blocks{1}(i)}]
    
    disp(strcat('Normal derivative with respect to c', num2str(i), ':'))
    [ n'*(model_points_plus_x{1} - model_points_minus_x{1})/(2*epsilon), n'*(model_points_plus_y{1} - model_points_minus_y{1})/(2*epsilon), n'*(model_points_plus_z{1} - model_points_minus_z{1})/(2*epsilon), n'*diff_c{blocks{1}(i)}]
    
    
    radii_plus = radii;
    radii_minus = radii;
    
    radii_plus{i} = radii_plus{i} + epsilon;
    radii_minus{i} = radii_minus{i} - epsilon;
    
    [~, model_points_plus, ~] = compute_projections_debug(data_points, centers,radii_plus,blocks);
    [~, model_points_minus, ~] = compute_projections_debug(data_points, centers,radii_minus,blocks);
    
    
    disp(strcat('Derivatives with respect to r', num2str(i), ':'))
    [ (model_points_plus{1} - model_points_minus{1})/(2*epsilon), diff_r{blocks{1}(i)}]
    
    disp(strcat('Normal derivative with respect to r', num2str(i), ':'))
    [ n'*(model_points_plus{1} - model_points_minus{1})/(2*epsilon), n'*diff_r{blocks{1}(i)}]
    
    
    
end
    

    
    
    
    



