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

%%

%[centers, radii, blocks, ~, phalanges, ~] = read_cpp_model('template_model/');
[centers, radii, blocks, ~, phalanges, ~] = read_cpp_model('C:\Developer\honline-cpp\data\models\anastasia\');


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

% rotate
for i = 1:length(centers)
    centers{i} = Rz(pi) * Rx(pi/2) * centers{i};
end

% display_result(centers, [], [], blocks, radii, false, 1, 'big');
%display_model_debug(centers,blocks,radii, 1.0, 'big')
% need names map to change parameterization
load('names_map.mat');

%% CREATE NEW HAND

% INDICES FOR COMPONENTS INTERESTED BY THE OPTIMIZATION RIGHT NOW
indices_for_fingers = {[names_map('pinky_top'), names_map('pinky_middle')]; [names_map('pinky_middle'), names_map('pinky_bottom')]; [names_map('pinky_bottom'), names_map('pinky_base')];
    [names_map('ring_top'), names_map('ring_middle')]; [names_map('ring_middle'), names_map('ring_bottom')]; [names_map('ring_bottom'), names_map('ring_base')];
    [names_map('middle_top'), names_map('middle_middle')]; [names_map('middle_middle'), names_map('middle_bottom')]; [names_map('middle_bottom'), names_map('middle_base')];
    [names_map('index_top'), names_map('index_middle')]; [names_map('index_middle'), names_map('index_bottom')]; [names_map('index_bottom'), names_map('index_base')];};

% INDICES FOR FIXED COMPONENTS
indices_for_fixed_palm = { names_map('palm_left'), names_map('palm_back'), names_map('palm_right'), names_map('palm_pinky'), names_map('palm_ring'), names_map('palm_middle'), names_map('palm_index'), names_map('palm_thumb')};
indices_for_fixed_wrist = {names_map('wrist_top_left'), names_map('wrist_top_right'), names_map('wrist_bottom_left'), names_map('wrist_bottom_right')};


palm_wrist_centers = {};
palm_wrist_radii = {};
palm_wrist_indices  = {};
palm_wrist_names = {'palm_right', 'palm_back', 'palm_left','palm_pinky', 'palm_ring', 'palm_middle', 'palm_index', 'palm_thumb', 'wrist_top_left', 'wrist_top_right', 'wrist_bottom_left', 'wrist_bottom_right' };

for i = 1: length (indices_for_fixed_palm)
    palm_wrist_centers{end +1} = centers{ indices_for_fixed_palm{i} };
    palm_wrist_radii{end +1} = radii{ indices_for_fixed_palm{i} };
    palm_wrist_indices{end + 1} = length( palm_wrist_radii );
end
for i = 1: length (indices_for_fixed_wrist)
    palm_wrist_centers{end +1} = centers{ indices_for_fixed_wrist{i} };
    palm_wrist_radii{end +1} = radii{ indices_for_fixed_wrist{i} };
    palm_wrist_indices{end + 1} = length( palm_wrist_radii );
end

palm_wrist_names_map = containers.Map( palm_wrist_names, palm_wrist_indices);

indices_for_membrane = { names_map('index_membrane'), names_map('middle_membrane'), names_map('ring_membrane'), names_map('pinky_membrane') };

membrane_position = {};
membrane_radii = {};
membrane_indices  = {};
membrane_names = { 'index_membrane', 'middle_membrane', 'ring_membrane', 'pinky_membrane'};

% manually read from template
s = [  0.4302, 0.4225,  0.4742,  0.5694];
 
for i = 1: length (indices_for_membrane) 
   membrane_position{end +1} =  s(i);
   membrane_radii{end +1} = 3.00+1e-7*rand;    
   membrane_indices{end + 1} = length( membrane_radii );
end

membranes_names_map = containers.Map( membrane_names, membrane_indices);


base_centers = { centers{ names_map('thumb_base') } , centers{ names_map('index_base')}, centers{names_map('middle_base') } , centers{names_map('ring_base') } , centers{names_map('pinky_base')}} ;
finger_names = {'thumb','index', 'middle','ring','pinky' };
beta = { [ norm(centers{names_map('thumb_bottom')} -centers{names_map('thumb_base')}) ; norm(centers{names_map('thumb_bottom')} -centers{names_map('thumb_middle')}); norm(centers{names_map('thumb_top')} -centers{names_map('thumb_middle')}); norm(centers{names_map('thumb_top')} -centers{names_map('thumb_additional')})], [norm(centers{names_map('index_bottom')} -centers{names_map('index_base')}); norm(centers{names_map('index_bottom')} -centers{names_map('index_middle')}); norm(centers{names_map('index_top')} -centers{names_map('index_middle')})], [norm(centers{names_map('middle_bottom')} -centers{names_map('middle_base')}); norm(centers{names_map('middle_bottom')} -centers{names_map('middle_middle')}); norm(centers{names_map('middle_top')} -centers{names_map('middle_middle')})], [norm(centers{names_map('ring_bottom')} -centers{names_map('ring_base')}); norm(centers{names_map('ring_bottom')} -centers{names_map('ring_middle')}); norm(centers{names_map('ring_top')} -centers{names_map('ring_middle')})], [norm(centers{names_map('pinky_bottom')} -centers{names_map('pinky_base')}); norm(centers{names_map('pinky_bottom')} -centers{names_map('pinky_middle')}); norm(centers{names_map('pinky_top')} -centers{names_map('pinky_middle')})]};
finger_radii = { radii{ names_map('thumb_base')}, radii{ names_map('thumb_bottom')}, radii{ names_map('thumb_middle')} , radii{ names_map('thumb_top')}, radii{ names_map('thumb_additional')}, radii{ names_map('index_base')},radii{ names_map('index_bottom')},radii{ names_map('index_middle')},radii{ names_map('index_top')}, radii{ names_map('middle_base')},radii{ names_map('middle_bottom')},radii{ names_map('middle_middle')},radii{ names_map('middle_top')}, radii{ names_map('ring_base')},radii{ names_map('ring_bottom')},radii{ names_map('ring_middle')},radii{ names_map('ring_top')}, radii{ names_map('pinky_base')},radii{ names_map('pinky_bottom')},radii{ names_map('pinky_middle')},radii{ names_map('pinky_top')}};

%% READ INITIAL PHOSE

% copy it into segments
%theta = { [0.40;-0.68;-0.1;-0.0], [-0.05;-0.15;0.0;0.0], [-0.05;+0.05;0.0;0.0], [0.05;0.15;0.0;0.15], [0.13;0.36;0.0;0.0]};
theta = { [0.0;0.0;0.0;0.0], [0.0;0.0;0.0;0.0], zeros(4,1), zeros(4,1), [0.0;0.0;0.0;0.0]};

last_index = length(palm_wrist_centers);
[segments, joints, finger_name_map ] = generate_fingers(beta,base_centers,finger_names);

% transform from my orientation to old one
Rinv = Rx(-pi/2)*Rz(-pi);
% transform from old orientation to mine
R = Rz(pi) * Rx(pi/2);


segments{1}{1}.local(1:3,1:3) = Rinv* phalanges{2}.local(1:3,1:3)*R;
segments{1}{1}.global(1:3,1:3) = Rinv* phalanges{2}.local(1:3,1:3)*R;
segments{1}{2}.local(1:3,1:3) = Rinv* phalanges{3}.local(1:3,1:3)*R;
segments{1}{3}.local(1:3,1:3) = Rinv* phalanges{4}.local(1:3,1:3)*R;

%segments{1}{5}.local = segments{1}{5}.local* makehgtform('axisrotate', [0;1;0], 0.6);

segments{5}{1}.local(1:3,1:3) = Rinv*  phalanges{5}.local(1:3,1:3)*R;
segments{5}{1}.global(1:3,1:3) =  Rinv*  phalanges{5}.local(1:3,1:3)*R;
segments{5}{2}.local(1:3,1:3) =  Rinv*  phalanges{6}.local(1:3,1:3)*R;
segments{5}{3}.local(1:3,1:3) =  Rinv*  phalanges{7}.local(1:3,1:3)*R;

segments{4}{1}.local(1:3,1:3) = Rinv*  phalanges{8}.local(1:3,1:3)*R;
segments{4}{1}.global(1:3,1:3) =  Rinv*  phalanges{8}.local(1:3,1:3)*R;
segments{4}{2}.local(1:3,1:3) =  Rinv*  phalanges{9}.local(1:3,1:3)*R;
segments{4}{3}.local(1:3,1:3) =  Rinv*  phalanges{10}.local(1:3,1:3)*R;

segments{3}{1}.local(1:3,1:3) = Rinv*  phalanges{11}.local(1:3,1:3)*R;
segments{3}{1}.global(1:3,1:3) =  Rinv*  phalanges{11}.local(1:3,1:3)*R;
segments{3}{2}.local(1:3,1:3) =  Rinv*  phalanges{12}.local(1:3,1:3)*R;
segments{3}{3}.local(1:3,1:3) =  Rinv*  phalanges{13}.local(1:3,1:3)*R;
segments{3}{4}.local = segments{3}{4}.local* makehgtform('axisrotate', [1;0;0], -0.375);

segments{2}{1}.local(1:3,1:3) = Rinv*  phalanges{14}.local(1:3,1:3)*R;
segments{2}{1}.global(1:3,1:3) =  Rinv*  phalanges{14}.local(1:3,1:3)*R;
segments{2}{2}.local(1:3,1:3) =  Rinv*  phalanges{15}.local(1:3,1:3)*R;
segments{2}{3}.local(1:3,1:3) =  Rinv*  phalanges{16}.local(1:3,1:3)*R;



%START CREATING HAND MODEL
hand_model.segments = segments;
hand_model.joints = joints;
hand_model.global_pose = eye(4);

delta_centers = {zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1), zeros(3,1)};

[ hand_model ] = update_fingers_pose(hand_model, theta, delta_centers );

%% DEFINE CONNECTIVITY 
 
n_blocks = {[palm_wrist_names_map('wrist_bottom_left'),palm_wrist_names_map('wrist_bottom_right'),palm_wrist_names_map('wrist_top_left')], [palm_wrist_names_map('wrist_top_left'),palm_wrist_names_map('wrist_top_right'),palm_wrist_names_map('wrist_bottom_right')], [palm_wrist_names_map('palm_left'),palm_wrist_names_map('palm_pinky'),palm_wrist_names_map('palm_ring')], [palm_wrist_names_map('palm_left'),palm_wrist_names_map('palm_back'),palm_wrist_names_map('palm_ring')], [palm_wrist_names_map('palm_back'),palm_wrist_names_map('palm_ring'),palm_wrist_names_map('palm_middle')], [palm_wrist_names_map('palm_right'),palm_wrist_names_map('palm_back'),palm_wrist_names_map('palm_middle')], [palm_wrist_names_map('palm_right'),palm_wrist_names_map('palm_middle'),palm_wrist_names_map('palm_index')], [13,palm_wrist_names_map('palm_thumb'),14],[13,14,15], [15,16], [16,17], [17,18], [34 + membranes_names_map('index_membrane'),34 + membranes_names_map('middle_membrane'),palm_wrist_names_map('palm_index')], [19,20], [20,21], [21,22],[palm_wrist_names_map('palm_middle'),34 + membranes_names_map('middle_membrane'),palm_wrist_names_map('palm_index')] [23,24], [24,25], [25,26],[palm_wrist_names_map('palm_middle'), 34 + membranes_names_map('ring_membrane'),palm_wrist_names_map('palm_middle')],[34 + membranes_names_map('ring_membrane'),34 + membranes_names_map('middle_membrane'),palm_wrist_names_map('palm_middle')], [27,28], [28,29], [29,30],[34 + membranes_names_map('pinky_membrane'),palm_wrist_names_map('palm_ring'),palm_wrist_names_map('palm_pinky')],[34 + membranes_names_map('ring_membrane'),34 + membranes_names_map('pinky_membrane'),palm_wrist_names_map('palm_ring')], [31,32], [32,33], [33,34] };
 

%% STORE STUFF
 hand_model.palm_wrist_centers_relative = palm_wrist_centers;
 hand_model.palm_wrist_centers = palm_wrist_centers;
 hand_model.palm_wrist_names_map = palm_wrist_names_map;
 hand_model.palm_wrist_radii = palm_wrist_radii;
 
 hand_model.beta = beta;
 hand_model.theta = theta;
 hand_model.finger_name_map = finger_name_map;
 hand_model.blocks = n_blocks;

 hand_model.finger_radii = finger_radii;
 hand_model.membrane_position = membrane_position;
 hand_model.membrane_radii = membrane_radii;
 hand_model.membrane_names_map = membranes_names_map;
 hand_model.global_rotation = zeros(3,1);
 hand_model.global_translation = zeros(3,1);
 hand_model.global_pose = eye(4);
 
 for i = 1:length(hand_model.palm_wrist_radii)
    hand_model.palm_wrist_radii{i} =  hand_model.palm_wrist_radii{i} + 1e-5*rand;
 end

% DETERMINE OFFSET`
R = hand_model.segments{1}{1}.global(1:3,1:3);
c_f = centers{names_map('thumb_fold')};
c_b = hand_model.segments{1}{1}.global(1:3,4);
offset = R\(c_f - c_b);

hand_model.fold_offset = offset;
hand_model.fold_center = centers{names_map('thumb_fold')};
hand_model.fold_radii = radii{names_map('thumb_fold')};
 
[ hand_model ] = update_membranes( hand_model );

%figure out transform%
c_tgt = centers{names_map('thumb_additional')};
c_start = hand_model.segments{1}{5}.global(1:3,4);
c_base = hand_model.segments{1}{4}.global(1:3,4);


% scale up model

scale_x = 0.85;
scale_y = 0.85;
scale_z = 0.85;
fat_factor = 0.85;
finger_length_factor = 0.85;
alpha = [scale_x; scale_y; scale_z];


%% UNIFORMLY DEFORM HAND

target_hand_model = hand_model;

target_pose.global_rotation = 0.0*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.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 );  
hand_model = target_hand_model;

%% DISPLAY MODEL
%display_model_debug(centers,blocks,radii, 1.0, 'big')
display_model(hand_model, 1.0, 'hold on')

%display_model(hand_model, 1.0, 'big')

%save( 'hand_model.mat', 'hand_model')


%% POSE MODEL TO TEST IF ITS ALL GOOD
% 
%  pose.global_rotation = zeros(3,1);
%  pose.global_translation = zeros(3,1);
% delta_theta = {-[0.2;0.6;0.3;0.7],[0.9;0.0;0.0;0.0],[0.9;0.0;0.0;0.0],[0.9;0.0;0.0;0.0],[0.9;0.0;0.0;0.0]};
% for i = 1:5
%     hand_model.theta{i} = delta_theta{i};
% end
% [ hand_model ] = pose_model(hand_model, delta_theta,pose );
% [ hand_model ] = update_membranes( hand_model );
% %hand_model.palm_wrist_centers{13} = hand_model.segments{1}{1}.global(1:3,4) + hand_model.segments{1}{1}.global(1:3,1:3)*offset;
% 
% display_model(hand_model,1.0,'big')