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

% EXAMPLES WORKING EVEN WITHOUT VALIDIDTY TERM
%rng(15)
%rng(18)
%rng(20)

% NEED TO ADD ENERGY TERM FOR VALIDITY
%rng(13) % works only using 4 frames without validity term
%rng(14) % works only using 5 frames without validity term

%% CHOOSE NUMBER OF FRAMES

N_frames = 6;

%% LOAD HAND MODEL

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

%% SET UP VECTOR OF POSES

vector_pose{1}.global_rotation = [0.0;0.0;0.0];
vector_pose{1}.global_translation = [0.0;0.0;0.0];
vector_pose{2}.global_rotation = [0.2;0.2;0.0];
vector_pose{2}.global_translation = [0.0;0.0;0.0];
vector_pose{3}.global_rotation = [0.0;0.0;pi];
vector_pose{3}.global_translation = [0.0;0.0;0.0];
vector_pose{4}.global_rotation = [0.0;0.5;pi];
vector_pose{4}.global_translation = [0.0;0.0;0.0];
vector_pose{5}.global_rotation = [0.0;0.0;pi/2];
vector_pose{5}.global_translation = [0.0;0.0;0.0];
vector_pose{6}.global_rotation = [-0.3;0.0;0.0];
vector_pose{6}.global_translation = [0.0;0.0;0.0];


vector_hands = {};

for i = 1:N_frames   
 delta_theta = {zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1)};
 [ p_hand_model ] = pose_model(hand_model, delta_theta,vector_pose{i} );
 [ p_hand_model ] = update_membranes( p_hand_model );
 vector_hands{i} = p_hand_model;   
end

 %% GENERATE TARGET_HAND

% Generate target model

target_hand_model = hand_model;

% perturbate finger info
sigma_radii = 1.0;
sigma_beta = 3.0;

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

% displace palm
for i = 1 : 14
    target_hand_model.palm_wrist_centers{i} = target_hand_model.palm_wrist_centers{i} + 2*(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 = {2*rand(3,1)-1, 2*rand(3,1)-1, 2*rand(3,1)-1, 2*rand(3,1)-1, 2*rand(3,1)-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, {zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1)}, target_finger_center );


%% CHANGE POSE & THETA OF EVERY TARGET HAND


vector_target_hands = {};
sigma_theta = 0.4;

for j = 1:N_frames   
 
    for i = 1:5
        if i>1
            DTHETA{i} =  sigma_theta* [2*rand-1;0;2*rand(2,1)-1];
        else
            DTHETA{i} =  2*sigma_theta* [2*rand-1;0;2*rand(2,1)-1];
        end   
    end
    
 [ p_target_hand_model ] = pose_model(target_hand_model, DTHETA,vector_pose{j} );
 for i = 1:5
  p_target_hand_model.theta{i} = p_target_hand_model.theta{i} + DTHETA{i};
 end
 [ p_target_hand_model ] = update_membranes( p_target_hand_model );
 vector_target_hands{j} = p_target_hand_model;   
end




%% CREATE POINT CLOUDS


downscaling = 10;
view_axis = 'Y';
sigma_noise = 0.2;

data_points = {};

for j = 1:N_frames   
    data_points{j} = generate_synthetic_point_cloud(vector_target_hands{j}, downscaling,view_axis, sigma_noise);
end
 
 
%% PROJECT POINTS ON MODEL
for j = 1:N_frames   
    [indices{j}, model_points{j}, block_indices{j}] = compute_projections(data_points{j}, vector_hands{j});
end


%% OPTIMIZE 

lambda = 100.0;
tol = 1e-03;

n_iter = 0;
target_delta = 2.0;
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;
settings.block_safety_factor = 1.1;



while err > tol && n_iter < 20
    
    n_iter = n_iter + 1
    
    % compute Jacobian matrix and right hand side
    J = [];
    F = [];
    
    
    for j = 1: N_frames
    
        [F_d2m, Jtheta_d2m, Jbeta_d2m, Jr_d2m, Jcenters_fingers_d2m, Jcenters_palm_d2m, Jr_palm_d2m, Js_membrane_d2m, Jglobal_rotation_d2m, Jglobal_translation_d2m] = COMPUTE_D2M_ENERGY(vector_hands{j}, data_points{j}, model_points{j}, indices{j}, block_indices{j},true );
        [F_m2d, Jtheta_m2d, Jbeta_m2d, Jr_m2d, Jcenters_fingers_m2d, Jr_palm_m2d, Jcenters_palm_m2d, Js_membrane_m2d, Jglobal_rotation_m2d, Jglobal_translation_m2d] = COMPUTE_JACOBIAN_M2D(vector_hands{j}, data_points{j}, settings,false );
        
               
        full_Jtheta_d2m = zeros(length(F_d2m), N_frames*20);
        full_Jtheta_d2m (:, (j-1)*20+1:j*20) = Jtheta_d2m;
        
        full_Jtheta_m2d = zeros(length(F_m2d), N_frames*20);
        full_Jtheta_m2d (:, (j-1)*20+1:j*20) = Jtheta_m2d;
               
        J = [J; full_Jtheta_d2m, Jbeta_d2m, Jr_d2m, Jcenters_palm_d2m, Jr_palm_d2m, Jcenters_fingers_d2m;
                full_Jtheta_m2d, Jbeta_m2d, Jr_m2d, Jcenters_palm_m2d, Jr_palm_m2d, Jcenters_fingers_m2d] ;
        F = [F; F_d2m;F_m2d];      
    end
    
        [F_validity,Jc_validity,Jr_validity,Jc_f_validity,Jr_f_validity] = COMPUTE_VALIDITY_ENERGY(vector_hands{1}, settings);
        J = [J; 
            zeros(length(F_validity), N_frames*20), zeros(length(F_validity), 15), Jr_f_validity, Jc_validity, Jr_validity, Jc_f_validity] ;
        
        % NOT AT ALL SURE ABOUT THE SIGN HERE
        F = [F; -F_validity];    
    
    % perform descent step      
    JtJ = J' * J;
    LHS = JtJ + lambda*eye(size(JtJ));
    delta = LHS \ (J' * F);
        
    err = norm(delta)
    lambda = err/target_delta * lambda;
    
    for j = 1: N_frames
        delta_theta{j} = { delta((j-1)*20+1:(j-1)*20+4), delta((j-1)*20+5:(j-1)*20+8), delta((j-1)*20+9:(j-1)*20+12), delta((j-1)*20+13:(j-1)*20+16), delta((j-1)*20+17:(j-1)*20+20)};
        vector_hands{j}.beta = { vector_hands{j}.beta{1} + delta(N_frames*20+1:N_frames*20+3), vector_hands{j}.beta{2} + delta(N_frames*20+4:N_frames*20+6),  vector_hands{j}.beta{3} + delta(N_frames*20+7:N_frames*20+9), vector_hands{j}.beta{4} + delta(N_frames*20+10:N_frames*20+12), vector_hands{j}.beta{5} + delta(N_frames*20+13:N_frames*20+15) };
    end
    
    delta_radii = delta(N_frames*20+16:N_frames*20+35);
    delta_centers = delta (N_frames*20+36: N_frames*20+77);
    delta_radii_palm = delta(N_frames*20 + 78 : N_frames*20 + 91);
    delta_finger_center_vec = delta( N_frames*20 + 92: N_frames*20 + 106 );

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

        for i = 1:length(hand_model.finger_radii)
            vector_hands{j}.finger_radii{i} = vector_hands{j}.finger_radii{i} + delta_radii(i);
        end

        for i = 1:length(hand_model.palm_wrist_radii)
            vector_hands{j}.palm_wrist_radii{i} = vector_hands{j}.palm_wrist_radii{i} + delta_radii_palm(i);
            vector_hands{j}.palm_wrist_centers_relative{i} = vector_hands{j}.palm_wrist_centers_relative{i} +  delta_centers( (3*(i-1) +1) : 3*i);
        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)};

        % update centers   
        [ vector_hands{j} ] = update_centers(vector_hands{j});
        % update shape
        [ vector_hands{j}.segments ] = update_fingers_shape(vector_hands{j}.segments, vector_hands{j}.beta );    
        % update model pose
        [ vector_hands{j} ] = update_fingers_pose(vector_hands{j}, delta_theta{j}, delta_finger_center );
        % update membranes
        [ vector_hands{j} ] = update_membranes( vector_hands{j} );

        % compute updated correspondencies
        [indices{j}, model_points{j}, block_indices{j}] = compute_projections(data_points{j}, vector_hands{j});
        % reindex hand
        [vector_hands{j}] = reindex_fullhand(vector_hands{j});
    
    end
    
end

%% DISPLAY RESULTS

for j = 1:N_frames
    display_model(vector_hands{j}, 0.9, 'big')
    mypoints(data_points{j}, [0.3, 0.8, 0.3], 10);
    mypoints(model_points{j}, [0.3, 0.3, 0.8], 10);
    mylines(data_points{j}, model_points{j}, [0.85, 0.85, 0.85]);
end