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

%% DEFINE ALL PARAMETERS

% sample size
N_samples = 5;

% paramters of calibration
N_frames = 6;

% ROUGH DEFORMATION OF TARGET HAND
scale_x = 1.2;
scale_y = 1.2;
scale_z = 1.2;
fat_factor = 1.2;
finger_length_factor = 1.3;
alpha = [scale_x; scale_y; scale_z];


% FINER SCALE DEFORMATION OF TARGET HAND
sigma_radii = 0.5;
sigma_beta = 2.0;
sigma_center = 1.0;

% POSE PARAMETERS
sigma_theta = 0.4;
sigma_rotation = 0.1;
sigma_translation = 5;

% OPTIMIZATION PARAMETERS
N_iter_unif = 15;
N_iter_full = 10;

% NUMBER OF POINTS POINT CLOUD - the lower the higher
downscaling = 6;


%% DEFINE CONTAINERS FOR ERROR

error = zeros(N_samples, N_iter_unif + N_iter_full);
error_r_f = zeros(N_samples, N_iter_unif + N_iter_full);
error_r_p = zeros(N_samples, N_iter_unif + N_iter_full);
error_c_f = zeros(N_samples, N_iter_unif + N_iter_full);
error_c_p = zeros(N_samples, N_iter_unif + N_iter_full);
error_beta = zeros(N_samples, N_iter_unif + N_iter_full);

%% LOAD HAND MODEL

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

for iter_samples = 1 : N_samples

    
    iter_samples
    
    %% 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.1;0.0;pi/6];
    vector_pose{5}.global_translation = [0.0;0.0;0.0];
    vector_pose{6}.global_rotation = [-0.3;0.0;-0.4];
    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;

    target_hand_model.beta = { finger_length_factor*target_hand_model.beta{1}, finger_length_factor*target_hand_model.beta{2},  finger_length_factor*target_hand_model.beta{3}, finger_length_factor*target_hand_model.beta{4}, finger_length_factor*target_hand_model.beta{5} };

    for j = 1:length(target_hand_model.finger_radii)
        target_hand_model.finger_radii{j} = fat_factor*target_hand_model.finger_radii{j};
    end

    for j = 1:length(target_hand_model.palm_wrist_radii)
        target_hand_model.palm_wrist_radii{j} = fat_factor*target_hand_model.palm_wrist_radii{j};
        target_hand_model.palm_wrist_centers_relative{j} = alpha.*target_hand_model.palm_wrist_centers_relative{j};
    end
    % update centers   
    [ target_hand_model ] = update_centers(target_hand_model);
    % perturbate finger centers
    delta_finger_center = {(alpha-1).*target_hand_model.segments{1}{1}.local(1:3,4), (alpha-1).*target_hand_model.segments{2}{1}.local(1:3,4), (alpha-1).*target_hand_model.segments{3}{1}.local(1:3,4), (alpha-1).*target_hand_model.segments{4}{1}.local(1:3,4), (alpha-1).*target_hand_model.segments{5}{1}.local(1:3,4)};
    % update shape
    [ target_hand_model.segments ] = update_fingers_shape(target_hand_model.segments, target_hand_model.beta );    
    % update model pose
    [ target_hand_model ] = update_fingers_pose(target_hand_model, delta_theta, delta_finger_center );
    % update membranes
    [ target_hand_model ] = update_membranes( target_hand_model );  


    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} + sigma_center*(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 = {sigma_center*(2*rand(3,1) - 1), sigma_center*(2*rand(3,1) - 1), sigma_center*(2*rand(3,1) - 1), sigma_center*(2*rand(3,1) - 1), sigma_center*(2*rand(3,1) - 1)};
    % update centers   
    [ target_hand_model ] = update_centers(target_hand_model);
    [ 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 = {};


    for j = 1:N_frames   
        for i = 1:5
            DTHETA{i} =  sigma_theta* [2*rand-1;0;2*rand(2,1)-1];   
        end

        % perturbate a bit global pose
        vector_pose{j}.global_rotation = vector_pose{j}.global_rotation + sigma_rotation*rand(3,1);     
        vector_pose{j}.global_translation = vector_pose{j}.global_translation + sigma_translation*rand(3,1);     

        [ 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

    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 

    tol = 1e-03;

    % SETTINGS FOR OPTIMIZATION PIPELINE
    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;

    %% ROTO TRANSLATION TO ALIGN TO POINT CLOUD

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

    disp('Roto-translating model')

    while err > tol && n_iter < 10

        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*6);
            full_Jtheta_d2m (:, (j-1)*6+1:j*6) = [Jglobal_rotation_d2m, Jglobal_translation_d2m];

            full_Jtheta_m2d = zeros(length(F_m2d), N_frames*6);
            full_Jtheta_m2d (:, (j-1)*6+1:j*6) = [Jglobal_rotation_m2d, Jglobal_translation_m2d];

            J = [J; full_Jtheta_d2m;
                    full_Jtheta_m2d] ;
            F = [F; F_d2m;F_m2d];      
        end

        % 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_pose.global_rotation = delta((j-1)*6+1:(j-1)*6+3);
            delta_pose.global_translation = delta((j-1)*6+4:(j-1)*6+6);

            delta_theta = {zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1)};
            [ vector_hands{j} ] = pose_model(vector_hands{j}, delta_theta,delta_pose );
            [ vector_hands{j} ] = update_centers( vector_hands{j});
             % 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});
        end 
    end


    %% OPTIMIZE OVER REDUCED PARAMETERS

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

    disp('Optimization over reduced parameters')

    while err > tol && n_iter < N_iter_unif

       n_iter = n_iter + 1;
       
       [ error(iter_samples, n_iter), error_r_f(iter_samples, n_iter), error_r_p(iter_samples, n_iter), error_c_f(iter_samples, n_iter), error_c_p(iter_samples, n_iter), error_beta(iter_samples, n_iter) ] = compute_errors( vector_hands{1}, vector_target_hands{1} );


        J = [];
        F = [];

       for i = 1: N_frames

            % 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( vector_hands{i}, data_points{i}, model_points{i}, indices{i}, block_indices{i}, 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{i}, data_points{i}, settings, false );

            full_Jtheta_d2m = zeros(length(F_d2m), N_frames*26);
            full_Jtheta_d2m (:, (i-1)*26+1: (i-1)*26+20) = Jtheta_d2m;
            full_Jtheta_d2m (:, (i-1)*26 + 21: i*26) = [Jglobal_rotation_d2m,Jglobal_translation_d2m];

            full_Jtheta_m2d= zeros(length(F_m2d), N_frames*26);
            full_Jtheta_m2d (:, (i-1)*26+1: (i-1)*26+20) = Jtheta_m2d;
            full_Jtheta_m2d (:, (i-1)*26 + 21: i*26) = [Jglobal_rotation_m2d,Jglobal_translation_m2d];

            J = [J; full_Jtheta_d2m, Jr_d2m, Jr_palm_d2m, Jbeta_d2m, Jcenters_palm_d2m, Jcenters_fingers_d2m;
                    full_Jtheta_m2d, Jr_m2d, Jr_palm_m2d, Jbeta_m2d, Jcenters_palm_m2d, Jcenters_fingers_m2d] ;
            F = [F; F_d2m;F_m2d];      
        end

        l0 = 26*N_frames;
        absolute_size = ones( size(J));

        for j = 1:length(vector_hands{1}.finger_radii)
            absolute_size(:,l0 + j) = vector_hands{1}.finger_radii{j};
        end
        l1 = l0 + length(vector_hands{1}.finger_radii);
        for j = 1:length(vector_hands{1}.palm_wrist_radii)
           absolute_size(:,l1 + j) = vector_hands{1}.palm_wrist_radii{j};
        end
        l1 = l1 + length(vector_hands{1}.palm_wrist_radii);
        absolute_size(:, l1 + 1: l1 + length(vector_hands{1}.beta)*length(vector_hands{1}.beta{1})) = [ vector_hands{1}.beta{1}', vector_hands{1}.beta{2}',  vector_hands{1}.beta{3}', vector_hands{1}.beta{4}', vector_hands{1}.beta{5}' ].* ones(length(F),15);
        l2 = l1 + length(vector_hands{1}.beta)*length(vector_hands{1}.beta{1});

        for j = 1:length( vector_hands{1}.palm_wrist_centers_relative)
            for k = 1:3
                absolute_size(:,l2 + 3*(j-1) + k) = vector_hands{1}.palm_wrist_centers_relative{j}(k);
            end
        end
        l3 = l2 + 3*length( vector_hands{1}.palm_wrist_centers_relative);
        for j = 1:length(vector_hands{1}.segments)
            for k = 1:3
            absolute_size(:,l3 + 3*(j-1) + k) = vector_hands{1}.segments{j}{1}.local(k,4);
            end
        end
        l3 = l3 + 3*length(vector_hands{1}.segments);


        J = J.*absolute_size;

        J = [J(:, 1:l0), sum(J(:,l0 + 1:l1),2), sum(J(:,1+l1:l2),2), sum(J(:,1+l2:3:l3),2), sum(J(:,2+l2:3:l3),2), sum(J(:,3+l2:3:l3),2) ];
        % 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_radii = delta(l0 + 1);
        delta_beta = delta(l0 + 2);
        delta_alpha = delta(l0 + 3: l0 + 5);

        for i = 1: N_frames

            index = (i-1)*26;
            delta_theta = { delta(index +1 :index +4), delta(index +5 :index +8), delta(index + 9 :index +12), delta(index +13 :index +16), delta(index +17 :index +20)};
            delta_pose.global_rotation = delta(index +21 :index +23);
            delta_pose.global_translation = delta(index + 24 :index + 26);

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

            for j = 1:length(vector_hands{i}.palm_wrist_radii)
                vector_hands{i}.palm_wrist_radii{j} = vector_hands{i}.palm_wrist_radii{j}*(1 + delta_radii);
                vector_hands{i}.palm_wrist_centers_relative{j} = vector_hands{i}.palm_wrist_centers_relative{j}.*(1 + delta_alpha);
            end

            vector_hands{i}.beta = { vector_hands{i}.beta{1}*(1 + delta_beta), vector_hands{i}.beta{2}*(1 + delta_beta),  vector_hands{i}.beta{3}*(1 + delta_beta), vector_hands{i}.beta{4}*(1 + delta_beta), vector_hands{i}.beta{5}*(1 + delta_beta) };

            delta_finger_center = {delta_alpha.*vector_hands{i}.segments{1}{1}.local(1:3,4), delta_alpha.*vector_hands{i}.segments{2}{1}.local(1:3,4), delta_alpha.*vector_hands{i}.segments{3}{1}.local(1:3,4), delta_alpha.*vector_hands{i}.segments{4}{1}.local(1:3,4), delta_alpha.*vector_hands{i}.segments{5}{1}.local(1:3,4)};

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

            % update centers
            [ vector_hands{i} ] = update_centers(vector_hands{i});
            % update shape
            [ vector_hands{i}.segments ] = update_fingers_shape(vector_hands{i}.segments, vector_hands{i}.beta ); 

            % update model pose
            [ vector_hands{i} ] = update_fingers_pose(vector_hands{i}, delta_theta, delta_finger_center );
            delta_theta = {zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1)};
            [ vector_hands{i} ] = pose_model(vector_hands{i}, delta_theta,delta_pose );

            % update membranes
            [ vector_hands{i} ] = update_membranes( vector_hands{i} );   

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

    end


    %% FULL OPTIMIZATION

    disp('Full optimization')

    lambda = 100.0;


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

    while err > tol && n_iter < N_iter_full

        n_iter = n_iter + 1;
        
        [ error(iter_samples, N_iter_unif+n_iter), error_r_f(iter_samples, N_iter_unif+n_iter), error_r_p(iter_samples, N_iter_unif+n_iter), error_c_f(iter_samples, N_iter_unif+n_iter), error_c_p(iter_samples, N_iter_unif+n_iter), error_beta(iter_samples,N_iter_unif+ n_iter) ] = compute_errors( vector_hands{1}, vector_target_hands{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

end



save('convergence.mat')
%% VISUALIZE CONVERGENCE RESULTS

close all
load('convergence.mat')

iterations = 1: N_iter_unif + N_iter_full;

mean_error = mean(error,1);
var_error = var(error,1);

mean_error_r_f = mean(error_r_f, 1);
var_error_r_f = var(error_r_f, 1);

mean_error_r_p = mean(error_r_p, 1);
var_error_r_p = var(error_r_p, 1);

mean_error_c_f = mean(error_c_f, 1);
var_error_c_f = var(error_c_f, 1);

mean_error_c_p = mean(error_c_p, 1);
var_error_c_p = var(error_c_p, 1);

mean_error_beta = mean(error_beta, 1);
var_error_beta = var(error_beta, 1);

z_star = 1.645; %corresponding to p = 99%


figure

subplot(2,3,1)
plot(iterations,mean_error,'red','LineWidth',2)
hold on
grid on
plot(iterations,mean_error + z_star/sqrt(N_iter_unif + N_iter_full) * var_error, '--b')
plot(iterations,mean_error - z_star/sqrt(N_iter_unif + N_iter_full) * var_error, '--b')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error + z_star/sqrt(N_iter_unif + N_iter_full) * var_error )];
plot(x0,y0,'green', 'LineWidth',2)
title('ALL PARAMETERS');
xlabel('iterations')
ylabel('error in norm 2')

subplot(2,3,2)
plot(iterations,mean_error_r_f,'red','LineWidth',2)
hold on
grid on
plot(iterations,mean_error_r_f + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_r_f, '--b')
plot(iterations,mean_error_r_f - z_star/sqrt(N_iter_unif + N_iter_full) * var_error_r_f, '--b')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_r_f + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_r_f)];
plot(x0,y0,'green', 'LineWidth',2)
title('FINGER RADII');
xlabel('iterations')
ylabel('error in norm 2')

subplot(2,3,3)
plot(iterations,mean_error_r_p,'red','LineWidth',2)
hold on
grid on
plot(iterations,mean_error_r_p + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_r_p, '--b')
plot(iterations,mean_error_r_p - z_star/sqrt(N_iter_unif + N_iter_full) * var_error_r_p, '--b')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_r_p + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_r_p)];
plot(x0,y0,'green', 'LineWidth',2)
title('PALM RADII');
xlabel('iterations')
ylabel('error in norm 2')

subplot(2,3,4)
plot(iterations,mean_error_c_f,'red','LineWidth',2)
hold on
grid on
plot(iterations,mean_error_c_f + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_c_f, '--b')
plot(iterations,mean_error_c_f - z_star/sqrt(N_iter_unif + N_iter_full) * var_error_c_f, '--b')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_c_f + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_c_f)];
plot(x0,y0,'green', 'LineWidth',2)
title('FINGER BASE CENTERS');
xlabel('iterations')
ylabel('error in norm 2')

subplot(2,3,5)
plot(iterations,mean_error_c_p,'red','LineWidth',2)
hold on
grid on
plot(iterations,mean_error_c_p + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_c_p, '--b')
plot(iterations,mean_error_c_p - z_star/sqrt(N_iter_unif + N_iter_full) * var_error_c_p, '--b')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_c_p + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_c_p)];
plot(x0,y0,'green', 'LineWidth',2)
title('PALM CENTERS');
xlabel('iterations')
ylabel('error in norm 2')

subplot(2,3,6)
plot(iterations,mean_error_beta,'red','LineWidth',2)
hold on
grid on
plot(iterations,mean_error_beta + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_beta, '--b')
plot(iterations,mean_error_beta - z_star/sqrt(N_iter_unif + N_iter_full) * var_error_beta, '--b')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_beta + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_beta)];
plot(x0,y0,'green', 'LineWidth',2)
title('BETA');
xlabel('iterations')
ylabel('error in norm 2')











