%FIXED BUG IN THE FULL VERSION, THIS DOES NOT WORK BUT IT DIDNT MAKE SENSE
%TO FIX IT SINCE NOW IT WORKS ON THE FULL HAND






%% COMPUTE DERIVATIVES WITH RESPECT TO CENTER POSITIONS

clc
clear 
close all

%% build membranes

f1_c1 = [10.5720; -0.7300; -3.6647];
f1_l = 40;
f1_theta1 = 0.0;
f1_theta2 = -0.3;
f1_R = makehgtform('axisrotate', [1;0;0], f1_theta1)*makehgtform('axisrotate', [0;1;0], f1_theta2);
f1_c2 = f1_R(1:3,1:3)* f1_l* [0;0;1] + f1_c1 ;


f2_c1 = [45.5720; -0.7300; -0.6647];
f2_l = 30;
f2_theta1 = 0.0;
f2_theta2 = 0.2;
f2_R = makehgtform('axisrotate', [1;0;0], f2_theta1)*makehgtform('axisrotate', [0;1;0], f2_theta2);
f2_c2 = f2_R(1:3,1:3)* f2_l* [0;0;1] + f2_c1;


f1_r1 = 10;
f1_r2 = 9;

f2_r1 = 11;
f2_r2 = 9;


%% COMPUTE CENTER POSITION OF FIRST MEMBRANE
f1_s = 0.3;
f1_rm = 2.0;

location1 = f1_c1 + f1_s*(f1_c2 - f1_c1);
d1  = f1_R(1:3,1:3)*[0;-1;0];
location_ray1 = (1-f1_s)*f1_r1 + f1_s*f1_r2;
f1_cm = location1 + d1* (location_ray1-f1_rm);

%% COMPUTE CENTER POSITION OF SECOND MEMBRANE
f2_s = 0.6;
f2_rm = 2.0;

location2 = f2_c1 + f2_s*(f2_c2 - f2_c1);
d2  = f2_R(1:3,1:3)*[0;-1;0];
location_ray2 = (1-f2_s)*f2_r1 + f2_s*f2_r2;
f2_cm = location2 + d2* (location_ray2-f2_rm);


%% SET UP MODEL

centers = {f1_c1,f1_c2,f2_c1,f2_c2,f1_cm,f2_cm};
radii = {f1_r1,f1_r2,f2_r1,f2_r2, f1_rm,f2_rm};

blocks = {[1, 2], [3,4], [5,6,1]};

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


%% data point

data_points = { [23.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

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

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

%% COMPUTE ANALYTICAL DERIVATIVES WITH FUNCTION

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

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

[v1, v2, v3, Jv1, Jv2, Jv3, u1, u2, u3, Ju1, Ju2, Ju3] = jacobian_tangent_plane(c1, c2, c3, r1, r2, r3, variables);
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

variables = {'theta11','theta12'};

c1 = f1_c1;
c2 = f2_c1;
beta1 = f1_l;
beta2 = f2_l;
R1 = f1_R;
R2 = f2_R;
r11 = f1_r1;
r12 = f1_r2;
r21 = f2_r1;
r22 = f2_r2;
s1 = f1_s;
s2 = f2_s;

[dc1, dc2, dcm1, dcm2, dc1_r, dc2_r, dcm1_r, dcm2_r ] = jacobian_membrane( c1,c2, beta1, beta2, R1, R2, r11, r12, r21, r22, s1, s2, variables);


%% F1THETA1 : compute numerical derivative

epsilon = 1e-05;

f1_theta1_plus = f1_theta1 + epsilon;
f1_R_plus = makehgtform('axisrotate', [1; 0; 0], f1_theta1_plus)*makehgtform('axisrotate', [0;1;0], f1_theta2);
f1_c2_plus = f1_R_plus(1:3,1:3)* f1_l* [0;0;1] + f1_c1 ;
location1_plus = f1_c1 + f1_s*(f1_c2_plus - f1_c1);
d1_plus = f1_R_plus(1:3,1:3)*[0;-1;0];
f1_cm_plus = location1_plus + d1_plus* (location_ray1-f1_rm);
centers_plus = {f1_c1,f1_c2_plus,f2_c1,f2_c2,f1_cm_plus,f2_cm};
[~, model_points_plus, ~] = compute_projections_debug(data_points, centers_plus,radii,blocks);

f1_theta1_minus = f1_theta1 - epsilon;
f1_R_minus = makehgtform('axisrotate', [1; 0; 0], f1_theta1_minus)*makehgtform('axisrotate', [0;1;0], f1_theta2);
f1_c2_minus = f1_R_minus(1:3,1:3)* f1_l* [0;0;1] + f1_c1 ;
location1_minus = f1_c1 + f1_s*(f1_c2_minus - f1_c1);
d1_minus = f1_R_minus(1:3,1:3)*[0;-1;0];
f1_cm_minus = location1_minus + d1_minus* (location_ray1-f1_rm);
centers_minus = {f1_c1,f1_c2_minus,f2_c1,f2_c2,f1_cm_minus,f2_cm};
[~, model_points_minus, ~] = compute_projections_debug(data_points, centers_minus,radii,blocks);

J_F1THETA1_NUM = n'*(model_points_plus{1} - model_points_minus{1})/(2*epsilon)


%% F1THETA1 : compute analytical derivative

v = cross([1; 0; 0], f1_cm - c1 );
J_F1THETA1_AN = n'*dq.dc2*v

J_F1THETA1_AN_FCT =  n'*(dq.dc1 * dc1.dtheta11  + dq.dc2 * dcm1.dtheta11 + dq.dc3 * dcm2.dtheta11 + dq.dr1 * dc1_r.dtheta11  + dq.dr2 * dcm1_r.dtheta11 + dq.dr3 * dcm2_r.dtheta11) 




%% F1THETA2 : compute numerical derivative

epsilon = 1e-05;

f1_theta2_plus = f1_theta2 + epsilon;
f1_R_plus = makehgtform('axisrotate', [1; 0; 0], f1_theta1)*makehgtform('axisrotate', [0;1;0], f1_theta2_plus);
f1_c2_plus = f1_R_plus(1:3,1:3)* f1_l* [0;0;1] + f1_c1 ;
location1_plus = f1_c1 + f1_s*(f1_c2_plus - f1_c1);
d1_plus = f1_R_plus(1:3,1:3)*[0;-1;0];
f1_cm_plus = location1_plus + d1_plus* (location_ray1-f1_rm);
centers_plus = {f1_c1,f1_c2_plus,f2_c1,f2_c2,f1_cm_plus,f2_cm};
[~, model_points_plus, ~] = compute_projections_debug(data_points, centers_plus,radii,blocks);

f1_theta2_minus = f1_theta2 - epsilon;
f1_R_minus = makehgtform('axisrotate', [1; 0; 0], f1_theta1)*makehgtform('axisrotate', [0;1;0], f1_theta2_minus);
f1_c2_minus = f1_R_minus(1:3,1:3)* f1_l* [0;0;1] + f1_c1 ;
location1_minus = f1_c1 + f1_s*(f1_c2_minus - f1_c1);
d1_minus = f1_R_minus(1:3,1:3)*[0;-1;0];
f1_cm_minus = location1_minus + d1_minus* (location_ray1-f1_rm);
centers_minus = {f1_c1,f1_c2_minus,f2_c1,f2_c2,f1_cm_minus,f2_cm};
[~, model_points_minus, ~] = compute_projections_debug(data_points, centers_minus,radii,blocks);

J_F1THETA2_NUM = n'*(model_points_plus{1} - model_points_minus{1})/(2*epsilon)


%% F1THETA2 : compute analytical derivative



v = cross([0;1;0],f1_cm - c1);
J_F1THETA2_AN = n'*dq.dc2*v


J_F1THETA2_AN_FCT =  n'*(dq.dc1 * dc1.dtheta12  + dq.dc2 * dcm1.dtheta12 + dq.dc3 * dcm2.dtheta12 + dq.dr1 * dc1_r.dtheta12  + dq.dr2 * dcm1_r.dtheta12 + dq.dr3 * dcm2_r.dtheta12) 



