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


%% LOAD HAND MODEL

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

settings.D = 3;
settings.block_safety_factor = 1.1;

%% POSE MODEL 


delta_theta = {[-0.1465;0.0067;0.2697;0.3039], [0.0;0.0;0.0;0.0], [0.0;0.0;0.0;0.0], [0.0;0.0;0.0;0.0], [0.0;0.0;0.0;0.0]};
delta_finger_center= {zeros(3,1), zeros(3,1) , zeros(3,1), zeros(3,1), zeros(3,1)};

for i = 1:length(hand_model.theta)
  hand_model.theta{i} = hand_model.theta{i} + delta_theta{i};
end

[ hand_model ] = update_fingers_pose(hand_model, delta_theta, delta_finger_center );
% update membranes
[ hand_model ] = update_membranes( hand_model );  

display_model(hand_model, 1.0, 'big')



%% CHECK JACOBIAN
 [F,Jc,Jr, Jc_fingers, Jr_fingers, Jtheta, Jbeta] = COMPUTE_VALIDITY_ENERGY(hand_model, settings)
 
 
 %%
 
 
 %% prepare hand model
centers = hand_model.palm_wrist_centers;
%convert segments into centers
for i = 1:length(hand_model.segments)
    finger_segments =  hand_model.segments{i};
    for j = 1:length(finger_segments)
        centers{end+1} = finger_segments{j}.global(1:3, 4);
    end
end
for i = 1:length(hand_model.membrane_centers)
    centers{end+1} = hand_model.membrane_centers{i}; 
end

radii = {};
 for i = 1: length(hand_model.palm_wrist_radii)
    radii{end +1} = hand_model.palm_wrist_radii{i};
 end
 for i = 1: length(hand_model.finger_radii)
    radii{end +1} = hand_model.finger_radii{i};
 end
 for i = 1: length(hand_model.membrane_radii)
    radii{end +1} = hand_model.membrane_radii{i};
 end
blocks = hand_model.blocks;
D = 3;



[tangent_gradients] = jacobian_tangent_planes(centers, blocks, radii, {'c1', 'c2', 'c3', 'r1', 'r2', 'r3'});

%% OPTIMIZE


tol = 1e-03;

lambda = 10.0;
n_iter = 0;
target_delta = 2.0;
err = tol + 1;

disp('Optimize over validity')

while err > tol && n_iter < 10
    
    n_iter = n_iter + 1

    [F,Jc,Jr, Jc_fingers, Jr_fingers, Jtheta, Jbeta] = COMPUTE_VALIDITY_ENERGY(hand_model, settings)
    
    J =  Jtheta ;
    F = -F;
 
    % perform descent step      
    JtJ = J' * J;
    LHS = JtJ + lambda*eye(size(JtJ));
    delta = LHS \ (J' * F);
        
    err = norm(delta)
    lambda = err/target_delta * lambda;
    

    delta_theta = {delta(1:4), delta(5:8), delta(9:12), delta(13:16), delta(17:20)};
    for i = 1:length(hand_model.theta)
        hand_model.theta{i} = hand_model.theta{i} + delta_theta{i};
    end

    [ hand_model ] = update_fingers_pose(hand_model, delta_theta, delta_finger_center );
        
    % update membranes
    [ hand_model ] = update_membranes( hand_model );  
     
end

%% DISPLAY

display_model(hand_model, 0.9, 'big')

