% TESTING OVER TWO RANDOMLY GENERATED POINTS

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

rng(23)

%% LOAD HAND MODEL

load('hand_model.mat');

[hand_model] = reindex_fullhand(hand_model);


pose.global_rotation = [0.0;-0.0;pi] + 0.1*rand(3,1);     
pose.global_translation = zeros(3,1);

%  pose.global_rotation = zeros(3,1);
%  pose.global_translation = zeros(3,1);
 
delta_theta = {[-0.3;+0.4;0.2;0.3], +[0.0;0.3;0.0;0.0], +[0.0;0.1;0.0;0.0], -[0.0;+0.1;0.0;0.0], -[0.0;+0.3;0.0;0.0]};
for i = 1:5
    hand_model.theta{i} = hand_model.theta{i} + delta_theta{i};
end

[ hand_model ] = pose_model(hand_model, delta_theta,pose );
[ hand_model ] = update_membranes( hand_model );

%% DISPLAY HAND MODEL

display_model(hand_model, 0.9, 'big')


 %% GENERATE DATA POINTS
 
data_points = { [10;-52;10] + 40*(rand(3,1)-0.5),[60;-10;60]+ 60*(rand(3,1)-0.5)  };

%% VISUALIZE DATA POINTS

mypoints(data_points, [0.3, 0.8, 0.3], 20);
  
%% PROJECT POINTS ON MODEL

[indices, model_points, block_indices] = compute_projections(data_points, hand_model);

%% VISUALIZE DATA POINTS PROJECTIONS

mypoints(model_points, [0.3, 0.3, 0.8], 20);
mylines(data_points, model_points, [0.85, 0.85, 0.85]);

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

n2 = data_points{2} - model_points{2};
n2 = n2 / norm(n2);


%% COMPUTE ANALYTICAL JACOBIANS

%[F, Jtheta, Jbeta, Jradii, Jcenters, Jradii_palm] = COMPUTE_FULL_JACOBIAN_D2M(hand_model, data_points, model_points, indices, block_indices );

[F, Jtheta, Jbeta, Jradii, Jcenters_fingers, Jcenters, Jradii_palm, Js_membrane] = COMPUTE_D2M_ENERGY(hand_model, data_points, model_points, indices, block_indices );

%% COMPUTE NUMERICAL JACOBIANS

derivative_hand_model = hand_model;

Jtheta_num = zeros(2,20);
Jbeta_num = zeros(2,15);
Jradii_num = zeros(2,20);
Jradii_palm_num = zeros(2,14);
Jcenters_num = zeros(2,42);

epsilon = 1e-04;

% loop over fingers


dtheta = {zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1)};
beta = hand_model.beta;
finger_radii = hand_model.finger_radii;
palm_radii = hand_model.palm_wrist_radii;
palm_centers = hand_model.palm_wrist_centers;

delta_finger_center = {zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1)};

for index_i = 1:5
  
    for index_j = 1 : 4
    
       dtheta_plus = dtheta;
       dtheta_plus{index_i}(index_j) = epsilon;
       dtheta_minus = dtheta;
       dtheta_minus{index_i}(index_j) = -epsilon;

       hand_model_plus = hand_model;
       hand_model_minus = hand_model;
             
       [ hand_model_plus.segments ] = update_fingers_pose(hand_model_plus.segments, hand_model_plus.joints, dtheta_plus, delta_finger_center );
       [ hand_model_minus.segments ] = update_fingers_pose(hand_model_minus.segments, hand_model_minus.joints, dtheta_minus, delta_finger_center );
        
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
       Jtheta_num(:, index_j + 4*(index_i-1) ) = [n1'*(model_points_plus{1} - model_points_minus{1})/(2*epsilon); n2'*(model_points_plus{2} - model_points_minus{2})/(2*epsilon)] ;
       
    end
    
    for index_j = 1 : 3
    
       beta_plus = beta;
       beta_plus{index_i}(index_j) = beta_plus{index_i}(index_j)+ epsilon;
       beta_minus = beta;
       beta_minus{index_i}(index_j) = beta_minus{index_i}(index_j) -epsilon;

       hand_model_plus = hand_model;
       hand_model_minus = hand_model;
       
       [ hand_model_plus.segments ] = update_fingers_shape(hand_model_plus.segments, beta_plus );
       [ hand_model_plus.segments ] = update_fingers_pose(hand_model_plus.segments, hand_model_plus.joints, dtheta, delta_finger_center );
       [ hand_model_minus.segments ] = update_fingers_shape(hand_model_minus.segments, beta_minus );
       [ hand_model_minus.segments ] = update_fingers_pose(hand_model_minus.segments, hand_model_minus.joints, dtheta, delta_finger_center );
        
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
       Jbeta_num(:, index_j + 3*(index_i-1) ) = [n1'*(model_points_plus{1} - model_points_minus{1})/(2*epsilon); n2'*(model_points_plus{2} - model_points_minus{2})/(2*epsilon)] ;
       
    end
    
    for index_j = 1 : 4
        
       radii_plus = finger_radii;
       radii_plus{4*(index_i-1) + index_j} = radii_plus{4*(index_i-1) + index_j} + epsilon;
       radii_minus = finger_radii;
       radii_minus{4*(index_i-1) + index_j} = radii_minus{4*(index_i-1) + index_j} - epsilon;

       hand_model_plus = hand_model;
       hand_model_minus = hand_model;
       
       hand_model_plus.finger_radii = radii_plus;
       hand_model_minus.finger_radii = radii_minus;
        
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
       Jradii_num(:, 4*(index_i-1) + index_j ) = [n1'*(model_points_plus{1} - model_points_minus{1})/(2*epsilon); n2'*(model_points_plus{2} - model_points_minus{2})/(2*epsilon)] ;
       
    end   
end

for i = 1 :length (hand_model.palm_wrist_radii)
    
       radii_plus = palm_radii;
       radii_plus{i} = radii_plus{i} + epsilon;
       radii_minus = palm_radii;
       radii_minus{i} = radii_minus{i} - epsilon;
       
       hand_model_plus = hand_model;
       hand_model_minus = hand_model;
       
       hand_model_plus.palm_wrist_radii = radii_plus;
       hand_model_minus.palm_wrist_radii = radii_minus;
        
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
       Jradii_palm_num(:, i ) = [n1'*(model_points_plus{1} - model_points_minus{1})/(2*epsilon); n2'*(model_points_plus{2} - model_points_minus{2})/(2*epsilon)] ;
        
end

for i = 1 :length (hand_model.palm_wrist_radii)
    
       centers_plus_x = palm_centers;
       centers_plus_x{i} = centers_plus_x{i} + epsilon*[1;0;0];
       
       centers_minus_x = palm_centers;
       centers_minus_x{i} = centers_minus_x{i} - epsilon*[1;0;0];
       
       centers_plus_y = palm_centers;
       centers_plus_y{i} = centers_plus_y{i} + epsilon*[0;1;0];
       
       centers_minus_y = palm_centers;
       centers_minus_y{i} = centers_minus_y{i} - epsilon*[0;1;0];
       
       centers_plus_z = palm_centers;
       centers_plus_z{i} = centers_plus_z{i} + epsilon*[0;0; 1];
       
       centers_minus_z = palm_centers;
       centers_minus_z{i} = centers_minus_z{i} - epsilon*[0;0;1];
       
       
       hand_model_plus_x = hand_model;
       hand_model_minus_x = hand_model;
       
       hand_model_plus_y = hand_model;
       hand_model_minus_y = hand_model;
       
       hand_model_plus_z = hand_model;
       hand_model_minus_z = hand_model;
       
       hand_model_plus_x.palm_wrist_centers = centers_plus_x;
       hand_model_minus_x.palm_wrist_centers = centers_minus_x;
       
       hand_model_plus_y.palm_wrist_centers = centers_plus_y;
       hand_model_minus_y.palm_wrist_centers = centers_minus_y;
       
       hand_model_plus_z.palm_wrist_centers = centers_plus_z;
       hand_model_minus_z.palm_wrist_centers = centers_minus_z;
        
       [~, model_points_plus_x, ~] = compute_projections(data_points, hand_model_plus_x);
       [~, model_points_minus_x, ~] = compute_projections(data_points, hand_model_minus_x);
       
       [~, model_points_plus_y, ~] = compute_projections(data_points, hand_model_plus_y);
       [~, model_points_minus_y, ~] = compute_projections(data_points, hand_model_minus_y);
       
       [~, model_points_plus_z, ~] = compute_projections(data_points, hand_model_plus_z);
       [~, model_points_minus_z, ~] = compute_projections(data_points, hand_model_minus_z);
       
       Jcenters_num(:, (3*(i-1) + 1) : (3*i) ) = [n1'*(model_points_plus_x{1} - model_points_minus_x{1})/(2*epsilon), n1'*(model_points_plus_y{1} - model_points_minus_y{1})/(2*epsilon), n1'*(model_points_plus_z{1} - model_points_minus_z{1})/(2*epsilon); n2'*(model_points_plus_x{2} - model_points_minus_x{2})/(2*epsilon), n2'*(model_points_plus_y{2} - model_points_minus_y{2})/(2*epsilon), n2'*(model_points_plus_z{2} - model_points_minus_z{2})/(2*epsilon)] ;
        
end

disp('Jacobian wrt theta') 
disp('ANALYTICAL') 
disp(Jtheta)
disp('NUMERICAL') 
disp(Jtheta_num)

disp('Jacobian wrt beta')
disp('ANALYTICAL') 
disp(Jbeta)
disp('NUMERICAL') 
disp(Jbeta_num)

disp('Jacobian wrt finger radii')
disp('ANALYTICAL')
disp(Jradii)
disp('NUMERICAL') 
disp(Jradii_num)

disp('Jacobian wrt palm & wrist radii')
disp('ANALYTICAL')
disp(Jradii_palm)
disp('NUMERICAL') 
disp(Jradii_palm_num)

disp('Jacobian wrt palm & wrist centers')
disp('ANALYTICAL')
disp(Jcenters)
disp('NUMERICAL') 
disp(Jcenters_num)

       
        
      
       
       
       
       
       




