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


%% LOAD HAND MODEL

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

settings.D = 3;
settings.theta_factor = 0.1;

%% POSE MODEL 

delta_theta = {[-0.9;0.0;-0.9;-1.5], [-0.9;0.0;-0.9;-1.5], [-0.9;0.0;-0.9;-1.5], [-0.9;0.0;-0.9;-1.5], [-0.9;0.0;-0.9;-1.5]};
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 );

[ hand_model ] = update_centers(hand_model);
[ hand_model ] = update_membranes( hand_model );  

display_model(hand_model, 0.9, 'big')



%% CHECK JACOBIAN

[F,Jtheta] = COMPUTE_BOUNDS_ENERGY(hand_model, settings)


%% OPTIMIZE


tol = 1e-03;

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

disp('Optimize over bounds on theta')

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

    [F,Jtheta] = COMPUTE_BOUNDS_ENERGY(hand_model, settings) ;
    
    J =  Jtheta ;
 
    % 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')

