
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 = {[2,3,1]};

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

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


%% compute analytical jacobians

[v1, v2, v3, Jv1, Jv2, Jv3, u1, u2, u3, Ju1, Ju2, Ju3] = jacobian_tangent_plane(c1, c2, c3, r1, r2, r3, variables);

diff_c = {{Jv1.dc1, Jv2.dc1, Jv3.dc1, Ju1.dc1, Ju2.dc1, Ju3.dc1}, {Jv1.dc2, Jv2.dc2, Jv3.dc2, Ju1.dc2, Ju2.dc2, Ju3.dc2}, {Jv1.dc3, Jv2.dc3, Jv3.dc3, Ju1.dc3, Ju2.dc3, Ju3.dc3}};
diff_r = {{Jv1.dr1, Jv2.dr1, Jv3.dr1, Ju1.dr1, Ju2.dr1, Ju3.dr1}, {Jv1.dr2, Jv2.dr2, Jv3.dr2, Ju1.dr2, Ju2.dr2, Ju3.dr2}, {Jv1.dr3, Jv2.dr3, Jv3.dr3, Ju1.dr3, Ju2.dr3, Ju3.dr3}};


%% display model & points

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

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

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

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


%% compute numerical jacobians


epsilon = 1e-04;

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];
    
    
    [tangent_points_plus_x] = blocks_tangent_points(centers_plus_x, blocks, radii);
    [tangent_points_minus_x] = blocks_tangent_points(centers_minus_x, blocks, radii);
    [tangent_points_plus_y] = blocks_tangent_points(centers_plus_y, blocks, radii);
    [tangent_points_minus_y] = blocks_tangent_points(centers_minus_y, blocks, radii);
    [tangent_points_plus_z] = blocks_tangent_points(centers_plus_z, blocks, radii);
    [tangent_points_minus_z] = blocks_tangent_points(centers_minus_z, blocks, radii);
    
    
    disp(strcat('Derivatives with respect to c', num2str(i), ':'))
    disp('v1')
    [ (tangent_points_plus_x{1}.v1 - tangent_points_minus_x{1}.v1)/(2*epsilon),(tangent_points_plus_y{1}.v1 - tangent_points_minus_y{1}.v1)/(2*epsilon),(tangent_points_plus_z{1}.v1 - tangent_points_minus_z{1}.v1)/(2*epsilon) , diff_c{blocks{1}(i)}{1} ]
    disp('v2')
    [ (tangent_points_plus_x{1}.v2 - tangent_points_minus_x{1}.v2)/(2*epsilon) ,(tangent_points_plus_y{1}.v2 - tangent_points_minus_y{1}.v2)/(2*epsilon) ,(tangent_points_plus_z{1}.v2 - tangent_points_minus_z{1}.v2)/(2*epsilon) , diff_c{blocks{1}(i)}{2} ]
    disp('v3')
    [ (tangent_points_plus_x{1}.v3 - tangent_points_minus_x{1}.v3)/(2*epsilon) ,(tangent_points_plus_y{1}.v3 - tangent_points_minus_y{1}.v3)/(2*epsilon) ,(tangent_points_plus_z{1}.v3 - tangent_points_minus_z{1}.v3)/(2*epsilon) , diff_c{blocks{1}(i)}{3} ]
    disp('u1')
    [ (tangent_points_plus_x{1}.u1 - tangent_points_minus_x{1}.u1)/(2*epsilon) ,(tangent_points_plus_y{1}.u1 - tangent_points_minus_y{1}.u1)/(2*epsilon) ,(tangent_points_plus_z{1}.u1 - tangent_points_minus_z{1}.u1)/(2*epsilon) , diff_c{blocks{1}(i)}{4} ]
    disp('u2')
    [ (tangent_points_plus_x{1}.u2 - tangent_points_minus_x{1}.u2)/(2*epsilon) , (tangent_points_plus_y{1}.u2 - tangent_points_minus_y{1}.u2)/(2*epsilon) ,(tangent_points_plus_z{1}.u2 - tangent_points_minus_z{1}.u2)/(2*epsilon) , diff_c{blocks{1}(i)}{5} ]
    disp('u3')
    [ (tangent_points_plus_x{1}.u3 - tangent_points_minus_x{1}.u3)/(2*epsilon) , (tangent_points_plus_y{1}.u3 - tangent_points_minus_y{1}.u3)/(2*epsilon) ,(tangent_points_plus_z{1}.u3 - tangent_points_minus_z{1}.u3)/(2*epsilon) , diff_c{blocks{1}(i)}{6} ]
    
    
    radii_plus = radii;
    radii_minus = radii;
    
    radii_plus{i} = radii_plus{i} + epsilon;
    radii_minus{i} = radii_minus{i} - epsilon;

    
    [tangent_points_plus] = blocks_tangent_points(centers, blocks, radii_plus);
    [tangent_points_minus] = blocks_tangent_points(centers, blocks, radii_minus);
    
    disp(strcat('Derivatives with respect to r', num2str(i), ':'))
    
    disp('v1')
    [ (tangent_points_plus{1}.v1 - tangent_points_minus{1}.v1)/(2*epsilon), diff_r{blocks{1}(i)}{1} ]
    disp('v2')
    [ (tangent_points_plus{1}.v2 - tangent_points_minus{1}.v2)/(2*epsilon), diff_r{blocks{1}(i)}{2} ]
    disp('v3')
    [ (tangent_points_plus{1}.v3 - tangent_points_minus{1}.v3)/(2*epsilon) , diff_r{blocks{1}(i)}{3} ]
    disp('u1')
    [ (tangent_points_plus{1}.u1 - tangent_points_minus{1}.u1)/(2*epsilon)  , diff_r{blocks{1}(i)}{4} ]
    disp('u2')
    [ (tangent_points_plus{1}.u2 - tangent_points_minus{1}.u2)/(2*epsilon) ,  diff_r{blocks{1}(i)}{5} ]
    disp('u3')
    [ (tangent_points_plus{1}.u3 - tangent_points_minus{1}.u3)/(2*epsilon)  , diff_r{blocks{1}(i)}{6} ]
     
end
    

    
    
