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

% 22 is super cool
%rng(22)

%% LOAD HAND MODEL

load('hand_model.mat');

[hand_model] = reindex_fullhand(hand_model);

%% DISPLAY HAND MODEL

display_model(hand_model, 0.9, 'big')


 %% GENERATE DATA POINTS

%downscaling = 12; 
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);
    if i>1
        target_hand_model.theta{i} =  sigma_theta* [2*rand-1;0;2*rand(2,1)-1];
    else
        target_hand_model.theta{i} =  2*sigma_theta* [2*rand-1;0;2*rand(2,1)-1];
    end
end

% displace palm
for i = 1 : 14
    target_hand_model.palm_wrist_centers{i} = target_hand_model.palm_wrist_centers{i} + 1*(rand(3,1) - 1);
    target_hand_model.palm_wrist_radii{i} = target_hand_model.palm_wrist_radii{i} + sigma_radii*(2*rand-1);
end
target_finger_center = {zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1)};
[ target_hand_model.segments ] = update_fingers_shape(target_hand_model.segments, target_hand_model.beta );
[ target_hand_model ] = update_fingers_pose(target_hand_model, target_hand_model.theta, target_finger_center );
[ target_hand_model ] = update_membranes( target_hand_model );

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], 10);
 
 
%% 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], 10);
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 = 1.0;
err = tol + 1;


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

    % compute Jacobian matrix and right hand side
    %[F, Jtheta, Jbeta, Jr, Jcenters_palm, Jr_palm] = COMPUTE_FULL_JACOBIAN_D2M(hand_model, data_points, model_points, indices, block_indices );
    [F, Jtheta, Jbeta, Jr, Jcenters_fingers, Jcenters_palm, Jr_palm, Js_membrane] = COMPUTE_D2M_ENERGY(hand_model, data_points, model_points, indices, block_indices,true );

    J = [Jtheta, Jbeta, Jr, Jcenters_palm, Jr_palm] ;
    
    
    % 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(21:23), hand_model.beta{2} + delta(24:26),  hand_model.beta{3} + delta(27:29), hand_model.beta{4} + delta(30:32), hand_model.beta{5} + delta(33:35) };
    delta_radii = delta(36:55);
    delta_centers = delta (56: 97);
    delta_radii_palm = delta(98 : 111);

    % 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
    
    for j = 1:length(hand_model.palm_wrist_radii)
        hand_model.palm_wrist_radii{j} = hand_model.palm_wrist_radii{j} + delta_radii_palm(j);
        hand_model.palm_wrist_centers{j} = hand_model.palm_wrist_centers{j} +  delta_centers( (3*(j-1) +1) : 3*j);
    end
    delta_finger_center = {zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1)};
    
    
    % update shape
    [ hand_model.segments ] = update_fingers_shape(hand_model.segments, hand_model.beta );    
    % update model pose
    [ hand_model ] = update_fingers_pose(hand_model, delta_theta, delta_finger_center );
    % update membranes
    [ hand_model ] = update_membranes( hand_model );
    
    % compute updated correspondencies
    [indices, model_points, block_indices] = compute_projections(data_points, hand_model);
    % reindex hand
    [hand_model] = reindex_fullhand(hand_model);
    
end

%% DISPLAY RESULTS

display_model(hand_model, 0.9, 'big')
mypoints(data_points, [0.3, 0.8, 0.3], 10);
mypoints(model_points, [0.3, 0.3, 0.8], 10);
mylines(data_points, model_points, [0.85, 0.85, 0.85]);