clc
%clear
close all
%LOAD CALIBRATION OUTPUT
load('calib.mat');
%choose one it doesn't really matter which one
hand_model = v_hand_model{1};
load('phalanges.mat');
load('data_path')

%Display model
display_model(v_hand_model{1},0.9,'big')

%% REPUT ORIGINALS TRANSFORMS ON FINGERS

%store old pose
theta_old = hand_model.theta;

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

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

%PUT MODEL ON REST POSE
hand_model.segments{1}{1}.local(1:3,1:3) = Rinv* phalanges{2}.local(1:3,1:3)*R;
hand_model.segments{1}{2}.local(1:3,1:3) = Rinv* phalanges{3}.local(1:3,1:3)*R;
hand_model.segments{1}{3}.local(1:3,1:3) = Rinv* phalanges{4}.local(1:3,1:3)*R;

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

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

hand_model.segments{3}{1}.local(1:3,1:3) = Rinv*  phalanges{11}.local(1:3,1:3)*R;
hand_model.segments{3}{2}.local(1:3,1:3) =  Rinv*  phalanges{12}.local(1:3,1:3)*R;
hand_model.segments{3}{3}.local(1:3,1:3) =  Rinv*  phalanges{13}.local(1:3,1:3)*R;

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

 pose.global_rotation = zeros(3,1);
 pose.global_translation = zeros(3,1);
delta_theta = {zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1)};
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 );


display_model(hand_model,0.9,'big')

% try to repose the thumb
delta_theta = theta_old;
[ hand_model ] = pose_model(hand_model, delta_theta,pose );
[ hand_model ] = update_membranes( hand_model );


display_model(hand_model,0.9,'big')

