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

hand_model.global_pose = eye(4);


%% ROTATE HAND MODEL 


pose.global_rotation = [0.0;0.0;0.0];     
pose.global_translation = zeros(3,1);

delta_theta = {zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1)};
 
[ hand_model ] = pose_model(hand_model, delta_theta,pose );
[ hand_model ] = update_membranes( hand_model );



%% DISPLAY HAND MODEL

display_model(hand_model, 0.9, 'big')


 %% GENERATE DATA POINTS

%downscaling = 12; 
downscaling = 14;
view_axis = 'Y';
sigma_noise = 0.2;

% Generate target model

target_hand_model = hand_model;

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

delta_theta = {zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1)};
 
[ target_hand_model ] = pose_model(target_hand_model, delta_theta,target_pose );
[ target_hand_model ] = update_membranes( target_hand_model );

% perturbate finger info
% sigma_radii = 1.0;
% sigma_beta = 5.0;
% sigma_theta = 0.6;
% 
% 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], 20);
 
 
%% 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], 20);
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 = 10.0;
err = tol + 1;


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

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

    J = [Jglobal_rotation, Jglobal_translation];
    
    
    % 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_pose.global_rotation = delta(1:3);
    delta_pose.global_translation = delta(4:6);   
      
    [ hand_model ] = pose_model(hand_model, delta_theta,delta_pose );
    
    % compute updated correspondencies
    [indices, model_points, block_indices] = compute_projections(data_points, hand_model);
    
    
end

%% DISPLAY RESULTS

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