clc
clear
close all
%addpath(genpath('../'))
rng(16)

%% WARNING

% COMMENT BACK POINT REJECTION TO CHECK VALIDITY OF JACOBIANS

%% LOAD HAND MODEL

load('hand_model.mat');
[hand_model] = reindex_fullhand(hand_model);

%% ROTATE MODEL

%     pose.global_rotation = 0.3*pi*rand(3,1);     
%    pose.global_translation = 50*(rand(3,1)-1);
 
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 );



%% GENERATE DATA POINT
 
% GENERATE DATA POINTS HARDCORE

downscaling = 15;
view_axis = 'Y';
sigma_noise = 0.2;

data_points = generate_synthetic_point_cloud(hand_model, downscaling,view_axis, sigma_noise);

%% PROJECT POINTS ON MODEL

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

for i = 1: length(data_points)
    if isempty(indices{i}) || isempty(data_points{i})  || isempty(model_points{i})
        continue;
    end 
    nn = data_points{i} - model_points{i};
    n{i} = nn/norm(nn);
end


%% VISUALIZE 


display_model(hand_model, 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]);

%[ bool_is_on_membrane, is_membrane_center, membrane_index ] = is_on_membrane( hand_model, indices{1} , block_indices{1} );


%% COMPUTE ANALYTICAL JACOBIANS

[F, Jtheta, Jbeta, Jradii, Jcenters_fingers, Jcenters_palm, Jradii_palm, Js_membrane, Jglobal_rotation, Jglobal_translation, Joffset_fold, Jr_fold] = COMPUTE_D2M_ENERGY(hand_model, data_points, model_points, indices, block_indices, false,false );


%% COMPUTE NUMERICAL JACOBIANS

derivative_hand_model = hand_model;

Jtheta_num = zeros(length(model_points),20);
Jbeta_num = zeros(length(model_points),16);
Jradii_num = zeros(length(model_points),21);
Jcenters_fingers_num = zeros(length(model_points),15);
Js_membrane_num = zeros(length(model_points),4);
Jradii_palm_num = zeros(length(model_points),12);
Jcenters_palm_num = zeros(length(model_points),36);
Jglobal_rotation_num = zeros(length(model_points),3);
Jglobal_translation_num = zeros(length(model_points),3);
Joffset_fold_num = zeros(length(model_points),3);
Jr_fold_num = zeros(length(model_points),1);

epsilon = 1e-4;

% 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;
delta_finger_center = {zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1)};
membrane_position = hand_model.membrane_position;
palm_centers = hand_model.palm_wrist_centers_relative;
palm_radii = hand_model.palm_wrist_radii;


for index_i = 1:5
  
    local_J_finger_center =[];
    
    for i = 1:3
        e = zeros(3,1);
        e(i) = epsilon* 1.0;
    
        delta_finger_center_plus = delta_finger_center;
        delta_finger_center_plus{index_i} = e;
        
        delta_finger_center_minus = delta_finger_center;
        delta_finger_center_minus{index_i} = -e;
    
       hand_model_plus = hand_model;
       hand_model_minus = hand_model;
       
       [ hand_model_plus ] = update_fingers_pose(hand_model_plus, dtheta, delta_finger_center_plus );
       [ hand_model_plus ] = update_membranes( hand_model_plus );
       [ hand_model_minus] = update_fingers_pose(hand_model_minus, dtheta, delta_finger_center_minus );
       [ hand_model_minus ] = update_membranes( hand_model_minus );
       
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
       
       update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
            if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii})|| isempty(n{ii})                
                 Jcenters_fingers(ii,3*(index_i-1) +1: 3*index_i) = 0.0;
                 continue;
            else
                 
            update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon); 
            end
       end       
       local_J_finger_center = [ local_J_finger_center,update];
    end
        
    Jcenters_fingers_num(:,3*(index_i-1) +1: 3*index_i) = local_J_finger_center;
    
    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 ] = update_fingers_pose(hand_model_plus, dtheta_plus, delta_finger_center );
       [ hand_model_plus ] = update_membranes( hand_model_plus );
       [ hand_model_minus ] = update_fingers_pose(hand_model_minus, dtheta_minus, delta_finger_center );
       [ hand_model_minus ] = update_membranes( hand_model_minus );
       
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
       update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
            if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                Jtheta(ii, index_j + 4*(index_i-1)) = 0.0;
                continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end       
       
       
       Jtheta_num(:, index_j + 4*(index_i-1) ) = update;
       
    end
    
    thumb_index = 0;
    if index_i == 1
        thumb_index = 1;
    end
    
     thumb_correction = 0;
    if index_i > 1
        thumb_correction = 1;
    end
    
    for index_j = 1 : (3+thumb_index)
    
       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] = update_fingers_pose(hand_model_plus, dtheta, delta_finger_center );
       [ hand_model_plus ] = update_membranes( hand_model_plus );
       [ hand_model_minus.segments ] = update_fingers_shape(hand_model_minus.segments, beta_minus );
       [ hand_model_minus ] = update_fingers_pose(hand_model_minus, dtheta, delta_finger_center );
       [ hand_model_minus ] = update_membranes( hand_model_minus );
       
       
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
       update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
             if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                 Jbeta(ii, thumb_correction + index_j + 3*(index_i-1)) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end  
       
       Jbeta_num(:, thumb_correction + index_j + 3*(index_i-1) ) = update;
       
    end
    
       corr = 0;
       if (index_i>1)
           corr =1;
       end
    
    for index_j = 1 : (4+thumb_index)
         
       radii_plus = radii;
       radii_plus{corr+ 4*(index_i-1) + index_j} = radii_plus{corr+4*(index_i-1) + index_j} + epsilon;
       radii_minus = radii;
       radii_minus{corr+4*(index_i-1) + index_j} = radii_minus{corr+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;
       
       [ hand_model_plus ] = update_membranes( hand_model_plus );
       [ hand_model_minus ] = update_membranes( hand_model_minus );
       [hand_model_plus] = reindex_fullhand(hand_model_plus);
       [hand_model_minus] = reindex_fullhand(hand_model_minus);
        
       [aa, model_points_plus, a] = compute_projections(data_points, hand_model_plus);
       [bb, model_points_minus, b] = compute_projections(data_points, hand_model_minus);
       
       update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
             if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                 Jradii_num(ii, corr+ 4*(index_i-1) + index_j) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end  
       
       Jradii_num(:, corr+ 4*(index_i-1) + index_j ) = update;
       
    end
  
end


%% PALM AND MEMBRANES
for i = 1:4
    
    membrane_position_plus = membrane_position;
    membrane_position_plus{i} = membrane_position_plus{i} + epsilon;
    
    membrane_position_minus = membrane_position;
    membrane_position_minus{i} = membrane_position_minus{i} - epsilon;
    
    hand_model_plus = hand_model;
    hand_model_plus.membrane_position = membrane_position_plus;
    
    hand_model_minus = hand_model;
    hand_model_minus.membrane_position = membrane_position_minus;
    
    [ hand_model_plus ] = update_membranes( hand_model_plus );
    [ hand_model_minus ] = update_membranes( hand_model_minus );

    [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
    [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
    
    update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
             if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                 Js_membrane_num(ii, i) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end  

    Js_membrane_num(:, i ) = update;
  
end

for i = 1: 12
    
    palm_radii_plus = palm_radii;
    palm_radii_minus = palm_radii;
    
    palm_radii_plus{i} = palm_radii_plus{i} + epsilon;
    palm_radii_minus{i} = palm_radii_minus{i} - epsilon;
    
    hand_model_plus = hand_model;
    hand_model_plus.palm_wrist_radii = palm_radii_plus;
    
    hand_model_minus = hand_model;
    hand_model_minus.palm_wrist_radii = palm_radii_minus;

    [a, model_points_plus, aa] = compute_projections(data_points, hand_model_plus);
    [b, model_points_minus, bb] = compute_projections(data_points, hand_model_minus);
    
    update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
             if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                Jradii_palm(ii, i) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end  
    
    Jradii_palm_num(:, i ) = update;
    
    
    local_J_palm_center =[];
    
    for j = 1:3
        e = zeros(3,1);
        e(j) = epsilon* 1.0;
    
        palm_centers_plus = palm_centers;
        palm_centers_plus{i} = palm_centers_plus{i} +  e;
        
        palm_centers_minus = palm_centers;
        palm_centers_minus{i} = palm_centers_minus{i} -  e;
    
        hand_model_plus = hand_model;
        hand_model_minus = hand_model;
        
        hand_model_plus.palm_wrist_centers_relative = palm_centers_plus;
        hand_model_minus.palm_wrist_centers_relative = palm_centers_minus;
        [ hand_model_plus ] = update_centers(hand_model_plus);
        [ hand_model_minus ] = update_centers(hand_model_minus);
        
       [ hand_model_plus ] = update_membranes( hand_model_plus );
       [ hand_model_minus ] = update_membranes( hand_model_minus );
       
       [a, model_points_plus, aa] = compute_projections(data_points, hand_model_plus);
       [b, model_points_minus, bb] = compute_projections(data_points, hand_model_minus);
       
           update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
             if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                 Jcenters_palm(ii, 3*(i-1) +1: 3*i) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end  
       
       local_J_palm_center = [ local_J_palm_center, update];
    end
        
    Jcenters_palm_num(:,3*(i-1) +1: 3*i) = local_J_palm_center;
    
      
end


pose.global_rotation = zeros(3,1);
pose.global_translation = zeros(3,1);

for j = 1:3
    e = zeros(3,1);
    e(j) = epsilon* 1.0;

    pose_plus = pose;
    pose_plus.global_translation = pose_plus.global_translation+  e;

    pose_minus = pose;
    pose_minus.global_translation= pose_minus.global_translation -  e;

    hand_model_plus = hand_model;
    hand_model_minus = hand_model;

   [ hand_model_plus ] = pose_model(hand_model_plus, dtheta, pose_plus );
   [ hand_model_minus ] = pose_model(hand_model_minus, dtheta, pose_minus );


   [ hand_model_plus ] = update_membranes( hand_model_plus );
   [ hand_model_minus ] = update_membranes( hand_model_minus );

   [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
   [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
   
       update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
            if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                Jglobal_translation(ii,j) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end  

   Jglobal_translation_num(:,j) = update;
end
    


    for j = 1:3
        e = zeros(3,1);
        e(j) = epsilon* 1.0;
    
        pose_plus = pose;
        pose_plus.global_rotation = pose_plus.global_rotation +  e;
        
        pose_minus = pose;
        pose_minus.global_rotation = pose_minus.global_rotation -  e;
    
        hand_model_plus = hand_model;
        hand_model_minus = hand_model;
        
       [ hand_model_plus ] = pose_model(hand_model_plus, dtheta, pose_plus );
       [ hand_model_minus ] = pose_model(hand_model_minus, dtheta, pose_minus );
        
        
       [ hand_model_plus ] = update_membranes( hand_model_plus );
       [ hand_model_minus ] = update_membranes( hand_model_minus );
       
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
              update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
             if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                 Jglobal_rotation(ii,j) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end  
     
       Jglobal_rotation_num(:,j) = update;
    end

   for j = 1:3
        e = zeros(3,1);
        e(j) = epsilon* 1.0;
    
        fold_plus = hand_model.fold_offset;
        fold_plus = fold_plus +  e;
        
        fold_minus = hand_model.fold_offset;
        fold_minus = fold_minus -  e;
    
        hand_model_plus = hand_model;
        hand_model_minus = hand_model;
        hand_model_plus.fold_offset = fold_plus;
        hand_model_minus.fold_offset = fold_minus;       
        
       [ hand_model_plus ] = update_membranes( hand_model_plus );
       [ hand_model_minus ] = update_membranes( hand_model_minus );
       
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
              update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
             if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                 Joffset_fold(ii,j) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end       
       Joffset_fold_num(:,j) = update;
   end
    
        fold_plus = hand_model.fold_radii;
        fold_plus = fold_plus +  epsilon;
        
        fold_minus = hand_model.fold_radii;
        fold_minus = fold_minus -  epsilon;
    
        hand_model_plus = hand_model;
        hand_model_minus = hand_model;
        hand_model_plus.fold_radii = fold_plus;
        hand_model_minus.fold_radii = fold_minus;       
        
       [ hand_model_plus ] = update_membranes( hand_model_plus );
       [ hand_model_minus ] = update_membranes( hand_model_minus );
       
       [~, model_points_plus, ~] = compute_projections(data_points, hand_model_plus);
       [~, model_points_minus, ~] = compute_projections(data_points, hand_model_minus);
       
              update = zeros(length(model_points),1);
       for ii = 1: length(model_points)
             if  isempty(model_points_plus{ii})  || isempty(model_points_minus{ii}) || isempty(n{ii})
                 Jr_fold(ii,1) = 0.0;
                 continue;
             end 
          update(ii) = n{ii}'*(model_points_plus{ii} - model_points_minus{ii})/(2*epsilon)  ;          
       end       
       Jr_fold_num(:,1) = update;
   
    
%global_translation = hand_model.global_translation;    

%% DISPLAY JACOBIANS TO SEE IF THEY ARE RIGHT

disp('Maximum error measured in norm \infty')
disp('--------------------------------------')

disp('theta')
[val,ind] = max(max(abs(Jtheta - Jtheta_num),[],2 ))

disp('beta')
[val,ind] = max(max(abs(Jbeta - Jbeta_num),[],2 ))

disp('radii')
[val,ind] = max(max(abs(Jradii - Jradii_num),[],2 ))

disp('centers_fingers')

[val,ind] = max(max(abs(Jcenters_fingers-Jcenters_fingers_num),[],2 ))

disp('membrane')

[val,ind] = max(max(abs(Js_membrane - Js_membrane_num),[],2 ))

disp('radii palm')

[val,ind] = max(max(abs(Jradii_palm - Jradii_palm_num),[],2 ))

disp('centers palm')

[val,ind] = max(max(abs(Jcenters_palm - Jcenters_palm_num),[],2 ))

disp('rotation')

[val,ind] = max(max(abs(Jglobal_rotation - Jglobal_rotation_num),[],2 ))

disp('translation')

[val,ind] = max(max(abs(Jglobal_translation - Jglobal_translation_num),[],2 ))

disp('fold offset')

[val,ind] = max(max(abs(Joffset_fold - Joffset_fold_num),[],2 ))

disp('fold radii')

[val,ind] = max(max(abs(Jr_fold - Jr_fold_num),[],2 ))
