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



N_iter = 10;
N_samples = 10;

err_beta = zeros(N_samples,N_iter);
err_theta = zeros(N_samples,N_iter);
err_radii = zeros(N_samples,N_iter);
err_pcenters = zeros(N_samples,N_iter);
err_pradii = zeros(N_samples,N_iter); 

for count = 1: N_samples

    %% LOAD HAND MODEL

    load('hand_model.mat');

    [hand_model] = reindex_fullhand(hand_model);


     %% GENERATE DATA POINTS
     
    downscaling = 10;
    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.2;

    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} + 3*(rand(3,1) - 1);
        target_hand_model.palm_wrist_radii{i} = target_hand_model.palm_wrist_radii{i} + 2*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);



    %% PROJECT POINTS ON MODEL

    [indices, model_points, block_indices] = compute_projections(data_points, hand_model);

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

    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;



    while err > tol && n_iter < N_iter

        n_iter = n_iter + 1

        % compute Jacobian matrix and right hand side
        [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(hand_model, data_points, model_points, indices, block_indices,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(hand_model, data_points, settings,false );

        J = [Jtheta_d2m, Jbeta_d2m, Jr_d2m, Jcenters_palm_d2m, Jr_palm_d2m; Jtheta_m2d, Jbeta_m2d, Jr_m2d, Jcenters_palm_m2d, Jr_palm_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(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);


        %% COMPUTER ERRORS
        
        err_theta(count,n_iter) = norm(hand_model.theta{1} - target_hand_model.theta{1} ) + norm(hand_model.theta{2} - target_hand_model.theta{2} )+ norm(hand_model.theta{3} - target_hand_model.theta{3} )+ norm(hand_model.theta{4} - target_hand_model.theta{4} )+ norm(hand_model.theta{5} - target_hand_model.theta{5} );
        err_beta(count,n_iter) = norm(hand_model.beta{1} - target_hand_model.beta{1} ) + norm(hand_model.beta{2} - target_hand_model.beta{2} )+ norm(hand_model.beta{3} - target_hand_model.beta{3} )+ norm(hand_model.beta{4} - target_hand_model.beta{4} )+ norm(hand_model.beta{5} - target_hand_model.beta{5} );
        %err_theta(count,n_iter) =  norm(hand_model.theta{2} - target_hand_model.theta{2} )+ norm(hand_model.theta{3} - target_hand_model.theta{3} )+ norm(hand_model.theta{4} - target_hand_model.theta{4} )+ norm(hand_model.theta{5} - target_hand_model.theta{5} );
        %err_beta(count,n_iter) =  norm(hand_model.beta{2} - target_hand_model.beta{2} )+ norm(hand_model.beta{3} - target_hand_model.beta{3} )+ norm(hand_model.beta{4} - target_hand_model.beta{4} )+ norm(hand_model.beta{5} - target_hand_model.beta{5} );
        
        
        for j = 1: length(hand_model.finger_radii)
            err_radii(count,n_iter) = err_radii(count,n_iter) + abs(hand_model.finger_radii{j} - target_hand_model.finger_radii{j});
        end
        for j = 1: length(hand_model.palm_wrist_centers)
            err_pcenters(count,n_iter) = err_pcenters(count,n_iter) + norm(hand_model.palm_wrist_centers{j} - target_hand_model.palm_wrist_centers{j});
            err_pradii(count,n_iter) = err_pradii(count,n_iter) + norm(hand_model.palm_wrist_radii{j} - target_hand_model.palm_wrist_radii{j});
        end        

    end


end





%% PLOT CONVERGENCE RESULTS
