% LOAD TEMPLATE HAND MODEL
clc
clear 
close

load('hand_model.mat');
[hand_model] = reindex_fullhand(hand_model);
% SET UP POSE

delta_theta = {zeros(4,1),[0.75;0;0;0],[0.75;0;0;0],[0.75;0;0;0],[0.75;0;0;0]};
delta_finger_center={zeros(3,1),[0.0;-6.0;0.0],[0.0;-6.0;0.0],[0.0;-6.0;0.0],[0.0;-6.0;0.0]};
[ hand_model ] = update_fingers_pose(hand_model, delta_theta, delta_finger_center );  



% CHECK INDEX DIGIT
c_p = hand_model.palm_wrist_centers_relative{hand_model.palm_wrist_names_map('palm_index')};
c_f = hand_model.segments{2}{1}.local(1:3,4);
r_p = hand_model.palm_wrist_radii{hand_model.palm_wrist_names_map('palm_index')};
r_f = hand_model.finger_radii{6};
%VERIFY RELATION
%finger one should be  bigger meaning that finger center is contained 
[ c_f(2)-c_p(2), r_p- r_f]


bound_factor = 1.1;
weight = 1;

% TEST
%hand_model.palm_wrist_centers_relative{hand_model.palm_wrist_names_map('palm_pinky')} = hand_model.palm_wrist_centers_relative{hand_model.palm_wrist_names_map('palm_pinky')} + 5.0;
[ hand_model ] = update_centers(hand_model);
[ hand_model ] = update_membranes( hand_model );  

display_model(hand_model, 1.0, 'big')

%% OPTIMIZE
tol = 1e-03;

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

disp('Optimize over bounds')

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

    [F,Jc_f,Jc_p] = COMPUTE_REALISTIC_BOUNDS_ENERGY(hand_model, bound_factor ,weight);
    
    J = [Jc_f,Jc_p] ;
 
    % perform descent step      
    JtJ = J' * J;
    LHS = JtJ + lambda*eye(size(JtJ));
    delta = LHS \ (J' * F);
        
    res = norm(F)
    %lambda = err/target_delta * lambda;
    
    delta_centers = delta (16:54 );
    delta_finger_center_vec = delta( 1:15 );
    delta_theta = {zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1)};
    

    for j = 1:length(hand_model.palm_wrist_radii)
        hand_model.palm_wrist_centers_relative{j} = hand_model.palm_wrist_centers_relative{j} +  delta_centers( (3*(j-1) +1) : 3*j);
    end
    delta_finger_center = {delta_finger_center_vec(1:3), delta_finger_center_vec(4:6), delta_finger_center_vec(7:9), delta_finger_center_vec(10:12), delta_finger_center_vec(13:15)};

    [ hand_model ] = update_centers(hand_model);
    [ hand_model ] = update_fingers_pose(hand_model, delta_theta, delta_finger_center );  
    [ hand_model ] = update_membranes( hand_model );  
     
end

%% DISPLAY

display_model(hand_model, 1.0, 'big')

[F,Jc_f,Jc_p] = COMPUTE_REALISTIC_BOUNDS_ENERGY(hand_model, bound_factor ,weight)