%% SAVE CALIBRATED HAND TO FILE
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')

%% REPUT ORIGINALS TRANSFORMS ON FINGERS

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

%READ INITIAL TRANSFORMATIONS
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 = {[-0.7;0.3;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} = delta_theta{i};
end

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


%% READ RELATIVE CENTERS,RADII FROM OUR MODEL
centers = hand_model.palm_wrist_centers_relative;
centers{end+1} = hand_model.segments{1}{1}.local(1:3,4) + hand_model.segments{1}{1}.local(1:3,1:3)*hand_model.fold_offset;
%convert segments into centers
for i = 1:length(hand_model.segments)
    finger_segments =  hand_model.segments{i};
    LOCAL = eye(4);
    for j = 1:length(finger_segments)        
         LOCAL = LOCAL*finger_segments{j}.local;
         centers{end+1} = LOCAL(1:3, 4);
    end
end
for i = 1:length(hand_model.membrane_centers)
    s = hand_model.membrane_position{i};
    base = hand_model.segments{1+i}{1}.local(1:3,4);   
    bottom =  hand_model.segments{1+i}{1}.local*hand_model.segments{1+i}{2}.local;
    bottom=bottom(1:3,4);
    location = base + s*(bottom - base);    
    Rm = hand_model.segments{1+i}{1}.local(1:3,1:3);    
    u1 = Rm*[0;1;0];
    u2 = hand_model.global_pose(1:3,1:3)*[0;1;0];
    if (u1'*u2 <= 0)
        membrane_direction = [0;1;0];
    else
        membrane_direction = [0;-1;0];
    end   
    d  = Rm* membrane_direction;
    location_ray = (1-s)*hand_model.finger_radii{1+4*i+1} + s*hand_model.finger_radii{1+4*i+2};
    new_location = location + d* (location_ray - 3.0);   
    centers{end+1} = new_location; 
end
radii = {};
 for i = 1: length(hand_model.palm_wrist_radii)
    radii{end +1} = hand_model.palm_wrist_radii{i};
 end
 radii{end +1} = hand_model.fold_radii;
 for i = 1: length(hand_model.finger_radii)
    radii{end +1} = hand_model.finger_radii{i};
 end
 for i = 1: length(hand_model.membrane_radii)
    radii{end +1} = hand_model.membrane_radii{i};
 end

% display for debugging
display_model_debug(centers,hand_model.blocks(3:end),radii,1.0,'big');

