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

%% LOAD HAND MODEL

load('hand_model.mat');

%% DISPLAY HAND MODEL

display_model(hand_model, 0.9, 'big')


 %% GENERATE DATA POINTS
 
% TO DO: choose them randomly when we are dealing with whole hand

data_points = { [-50;-30;84], [-100;-30;84]  };

%% 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] = COMPUTE_JACOBIAN_D2M(hand_model, data_points, model_points, indices, block_indices );
[F, Jtheta, Jbeta, Jradii] = jacobian_realsense(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);


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;
radii = hand_model.finger_radii;


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 );
       [ hand_model_minus.segments ] = update_fingers_pose(hand_model_minus.segments, hand_model_minus.joints, dtheta_minus );
        
       [~, 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 );
       [ 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 );
        
       [~, 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 = radii;
       radii_plus{4*(index_i-1) + index_j} = radii_plus{4*(index_i-1) + index_j} + epsilon;
       radii_minus = 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

[[n1'*Jtheta(1:3,:);n2'*Jtheta(4:6,:)];Jtheta_num]

[[n1'*Jbeta(1:3,:);n2'*Jbeta(4:6,:)];Jbeta_num]

[[n1'*Jradii(1:3,:);n2'*Jradii(4:6,:)];Jradii_num]