clc
clear
close all

data_path = 'C:\Users\remelli\Desktop\fitting_cpp_initialization\edoardo\stage1\';

Rx = @(alpha) [
    1, 0, 0;
    0, cos(alpha), -sin(alpha);
    0, sin(alpha), cos(alpha)];

Ry = @(alpha) [
    cos(alpha), 0, sin(alpha);
    0, 1, 0;
    -sin(alpha), 0, cos(alpha)];

Rz = @(alpha) [
    cos(alpha), -sin(alpha), 0;
    sin(alpha), cos(alpha), 0
    0, 0, 1];

% DEFINE USEFUL TRANSFORMS
% transform from my orientation to htrack's one
R = Rx(-pi/2)*Rz(-pi);
% transform from htrack's orientation to mine
Rinv = Rz(pi) * Rx(pi/2);

% LOAD TEMPLATE HAND MODEL
load('hand_model.mat');
[hand_model] = reindex_fullhand(hand_model);


% choose frames from folder
frames = { 1,2,3,4};

%% READ htrack INPUT

v_hand_model = {};
v_data_points = {};

for ii = 1 : length(frames)

   scale_x = 1.1;
    scale_y = 1.1;
    scale_z = 1.3;
    fat_factor = 1.0;
    finger_length_factor = 1.2;
    alpha = [scale_x; scale_y; scale_z];

    %% ROTATE MODEL

    pose.global_rotation = [0.0;0.0;0.0] + 0.1*rand(3,1);     
    pose.global_translation = zeros(3,1);

    %  pose.global_rotation = zeros(3,1);
    %  pose.global_translation = zeros(3,1);

    delta_theta = {[0.0;-0.4;0.0;0.0], [0.0;0.0;0.0;0.0], [0.0;0.00;0.0;0.0], -[0.0;0.00;0.0;0.0], [0.0;-0.0;0.0;0.0]};
    for i = 1:5
        hand_model.theta{i} = hand_model.theta{i} + delta_theta{i};
    end

    [ hand_model ] = pose_model(hand_model, delta_theta,pose );
    [ hand_model ] = update_membranes( hand_model );


    %% UNIFORMLY DEFORM HAND

    target_hand_model = hand_model;

    target_pose.global_rotation = 0.1*rand(3,1);     
    target_pose.global_translation = 5*rand(3,1); 

    delta_theta = {zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1)};

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

    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 );
    [ target_hand_model ] = pose_model(target_hand_model, delta_theta,target_pose );
    % update membranes
    [ target_hand_model ] = update_membranes( target_hand_model );  


    % PERTURBATE FINGER INFO

    sigma_radii = 2.0;
    sigma_beta = 2.5;
    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);
        target_hand_model.theta{i} =  sigma_theta* [2*rand-1;0;2*rand(2,1)-1];    
    end
    %target_hand_model.theta{1} = target_hand_model.theta{1} + [-0.4;-0.3;0.0;0.0];

    % % displace palm
    for i = 1 : 13
        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} + sigma_radii*(2*rand-1);
    end

    % update centers   
    [ target_hand_model ] = update_centers(target_hand_model);

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

    %% GENERATE PC


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

    data_points = generate_synthetic_point_cloud(target_hand_model, downscaling,view_axis, sigma_noise);
    
    
    v_hand_model{ii} = hand_model;
    v_data_points{ii} = data_points;
end

%% VISUALIZE TO CHECK EVERYTHING IS ALLRIGHT

for i = 1 : length(frames)
    [v_indices{i}, v_model_points{i}, v_block_indices{i}] = compute_projections(v_data_points{i}, v_hand_model{i});
end

%% START OPTIMIZATION

% OPTIMIZATION PARAMETERS
tol = 1e-03;
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 = 'Y';
settings.theta_factor = 0.2;
settings.block_safety_factor = 1.1;
w_bounds = 400;
w_val = 2000;
w_stabilization_theta = 300;
w_stabilization_radii = 1000;

disp('Full optimization')
    
    % compute Jacobian matrix and right hand side
    J_full = [];
    F_full = [];
    
    for i = 1:length(frames)
    
        disp('frame')
        % 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(v_hand_model{i}, v_data_points{i}, v_model_points{i}, v_indices{i}, v_block_indices{i},true,false );
        [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(v_hand_model{i}, v_data_points{i}, settings,false );
        [F_bounds,Jtheta_bounds] = COMPUTE_BOUNDS_ENERGY(v_hand_model{i}, settings);
        
        
        J = [zeros(length(F_d2m),26*(i-1)), Jtheta_d2m, Jglobal_rotation_d2m, Jglobal_translation_d2m,zeros(length(F_d2m),26*(length(frames)-i)),Jbeta_d2m, Jr_d2m, Jcenters_palm_d2m, Jr_palm_d2m, Jcenters_fingers_d2m; zeros(length(F_m2d),26*(i-1)), Jtheta_m2d, Jglobal_rotation_m2d, Jglobal_translation_m2d,zeros(length(F_m2d),26*(length(frames)-i)), Jbeta_m2d, Jr_m2d, Jcenters_palm_m2d, Jr_palm_m2d, Jcenters_fingers_m2d; zeros(length(F_bounds),26*(i-1)), Jtheta_bounds, zeros(length(F_bounds),6),zeros(length(F_bounds),26*(length(frames)-i)), zeros(length(F_bounds),104); zeros(26,26*(i-1)), eye(26,26) ,zeros(26,26*(length(frames)-i)), zeros(26,104) ] ;
        F = [F_d2m;F_m2d;F_bounds;zeros(26,1)];
        
        % store in full matrix
        J_full = [J_full;J];
        F_full = [F_full;F];       
    end
    
    [F_val,Jcenters_palm_val,Jr_palm_val, Jcenters_fingers_val, Jr_val, Jtheta_val, Jbeta_val] = COMPUTE_VALIDITY_ENERGY(v_hand_model{1}, settings);
    F_val = zeros(12,1);
    
    J_full = [J_full; zeros(length(F_val),length(frames)*26), ones(length(F_val),104) ];
    F_full = [F_full; F_val];  
                
    % perform descent step      
    JtJ = J_full' * J_full;
    
    spy(J_full)
    figure
    spy(JtJ)
    

