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

rng(9)

%% LOAD HAND MODEL

load('hand_model.mat');

%% DISPLAY HAND MODEL

display_model(hand_model, 0.9, 'big')


 %% GENERATE DATA POINTS
 
downscaling = 12;
view_axis = 'Y';
sigma_noise = 0.2;

% Generate target model

target_hand_model = hand_model;

% perturbate finger info
sigma_radii = 1.0;
sigma_beta = 5.0;
sigma_theta = 0.4;

for i = 1 : length(target_hand_model.finger_radii)
    target_hand_model.finger_radii{i} = target_hand_model.finger_radii{i} + sigma_radii*rand;
end

for i = 1:5
    target_hand_model.beta{i} = target_hand_model.beta{i} + sigma_beta* (2*rand(size( hand_model.beta{i})) -1);
    target_hand_model.theta{i} =  sigma_theta* [2*rand-1;0;2*rand(2,1)-1];
end

[ target_hand_model.segments ] = update_fingers_shape(target_hand_model.segments, target_hand_model.beta );
[ target_hand_model.segments ] = update_fingers_pose(target_hand_model.segments, target_hand_model.joints, target_hand_model.theta );

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

%% 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]);




%% OPTIMIZE IN POSE SPACE
% TO DO: IMPROVE DESCENT ALGORITHM INCLUDING STEP REJECTION??

lambda = 100.0;
tol = 1e-03;

n_iter = 0;
target_delta = 0.1;
err = tol + 1;


settings.fov = 15;
settings.H = 480/12;
settings.W = 636/12;
settings.D = 3;
settings.sparse_data = false;
settings.RAND_MAX = 32767;
settings.side = 'front';
settings.view_axis = view_axis;


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

    % compute Jacobian matrix and right hand side
    [F_D2M, Jtheta_D2M, Jbeta_D2M, Jr_D2M] = COMPUTE_JACOBIAN_D2M(hand_model, data_points, model_points, indices, block_indices );
    [F_M2D, Jtheta_M2D, Jbeta_M2D, Jr_M2D] = COMPUTE_JACOBIAN_M2D(hand_model, data_points,settings,false );
    
    
    %[F_m2d, J_theta_m2d, J_beta_m2d , J_radii_m2d] = compute_m2d_energy(segments, joints, radii, blocks, data_points, settings, false);

    J = [Jtheta_D2M, Jbeta_D2M, Jr_D2M; Jtheta_M2D, Jbeta_M2D, Jr_M2D ] ;
    F = [F_D2M; F_M2D];
    
    % 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)};
    hand_model.beta = { hand_model.beta{1} + delta(20:22), hand_model.beta{2} + delta(23:25),  hand_model.beta{3} + delta(26:28), hand_model.beta{4} + delta(29:31), hand_model.beta{5} + delta(32:34) };
    delta_radii = delta(35:55);

    % update parameters
    for j = 1:length(hand_model.theta)
        hand_model.theta{j} = hand_model.theta{j} + delta_theta{j};
    end

    for j = 1:length(hand_model.finger_radii)
        hand_model.finger_radii{j} = max(hand_model.finger_radii{j} + delta_radii(j),0.1);
    end
    
    % update shape
    [ hand_model.segments ] = update_fingers_shape(hand_model.segments, hand_model.beta );    
    % update model pose
    [ hand_model.segments ] = update_fingers_pose(hand_model.segments, hand_model.joints, delta_theta );
    % compute updated correspondencies
    [indices, model_points, block_indices] = compute_projections(data_points, hand_model);
    
end

%% DISPLAY RESULTS

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]);