% 맨처음 방향 (frame1) 으로 일괄 orientation normalize
% => 1번프레임이 아니라 프레임들의 중앙값
% 키로 모든방향 normalize
% 노멀라이즈시 최소값 빼는게 아니라 joint1-> 0.5/0.5/0.5로 가도록 
% 실험할때 전방향 노멀라이즈도 해봐야겠다..

clear;

name = '/home/rfj/바탕화면/skeletones/S001C001P002R002A020.skeleton'
bodyinfo = read_skeleton_file(name);
frame_num = size(bodyinfo,2);

%initialize
cur_subject_x = zeros(frame_num, 25);
cur_subject_y = zeros(frame_num, 25);
cur_subject_z = zeros(frame_num, 25);

tot_x = zeros(frame_num,25);
tot_y = zeros(frame_num,25);
tot_z = zeros(frame_num,25);

joint_5 = zeros(1,3);
joint_9 = zeros(1,3);
joint_1 = zeros(1,3);
joint_3 = zeros(1,3);

%get total joints information
for FN = 1:frame_num
    cur_body = bodyinfo(FN).bodies(1);
    joints = cur_body.joints;
   
    for JN = 1:25
        tot_x(FN,JN) = joints(JN).x; 
        tot_y(FN,JN) = joints(JN).y; 
        tot_z(FN,JN) = joints(JN).z; 
    end
end

%get median values
M_x = median(tot_x);
M_y = median(tot_y);
M_z = median(tot_z);

%set 3 points for make plane
joint_5 = [M_x(5) M_y(5) M_z(5)];
joint_9 = [M_x(9) M_y(9) M_z(9)];
joint_1 = [M_x(1) M_y(1) M_z(1)];
joint_3 = [M_x(3) M_y(3) M_z(3)];

%find RIGID TRNASFORMATION matrix
d1 = joint_1 - joint_5;
d2 = joint_1 - joint_9;
n1 = cross(d1,d2); % because we will parallel transform, don't need to find belly
u1 = n1/norm(n1);
u2 = [0 0 1];
cs1 = dot(u1,u2)/norm(u1)*norm(u2);
ss1 = sqrt(1-cs1.^2);
v1 = cross(u1,u2)/norm(cross(u1,u2));

R1 = [v1(1)*v1(1)*(1-cs1)+cs1 v1(1)*v1(2)*(1-cs1)-v1(3)*ss1 v1(1)*v1(3)*(1-cs1)+v1(2)*ss1];
R1(2,:) = [v1(1)*v1(2)*(1-cs1)+v1(3)*ss1 v1(2)*v1(2)*(1-cs1)+cs1 v1(2)*v1(3)*(1-cs1)-v1(1)*ss1];
R1(3,:) = [v1(1)*v1(3)*(1-cs1)-v1(2)*ss1 v1(2)*v1(3)*(1-cs1)+v1(1)*ss1 v1(3)*v1(3)*(1-cs1)+cs1];

%1-3 number tolls to parallel x axis. Rigid transformation on plane surface
%Z axis coords oyler angle transform

t = joint_3 - joint_1;
d3 = R1(1,:) * t.'; 
d3(1,2) = R1(2,:) * t.'; 
d3(1,3) = R1(3,:) * t.'; 

u3 = d3(1:2)/norm(d3(1:2));
v3 = [u3(1) -u3(2)];
v3(2,:) = [u3(2) u3(1)];
u4 = [1 0].';

csss = v3\u4;
cs2 = csss(1);
ss2 = csss(2);

R2 = [cs2 -ss2 0];
R2(2,:) = [ss2 cs2 0];
R2(3,:) = [0 0 1];


%apply rigid transformation
for FN = 1:frame_num
    cur_body = bodyinfo(FN).bodies(1);
    joints = cur_body.joints;
    
    for JN = 1:25
        a = R1(1,:) * [joints(JN).x joints(JN).y joints(JN).z].'; 
        b = R1(2,:) * [joints(JN).x joints(JN).y joints(JN).z].'; 
        c = R1(3,:) * [joints(JN).x joints(JN).y joints(JN).z].'; 

        cur_subject_x(FN,JN) = R2(1,:) * [a b c].'; 
        cur_subject_y(FN,JN) = R2(2,:) * [a b c].'; 
        cur_subject_z(FN,JN) = R2(3,:) * [a b c].'; 
        
    end
end

%orientation normalize 2 (with plane surface)
if cur_subject_x(1,4) < cur_subject_x(1,1) 
    cur_subject_x = 0 - cur_subject_x;
end

if cur_subject_y(1,9) > cur_subject_y(1,5)
    cur_subject_y = 0 - cur_subject_y;
end
    
%get current median
CM_x=median(cur_subject_x);
CM_y=median(cur_subject_y);
CM_z=median(cur_subject_z);

%for transform bellybutton to 0.5,0.5 (Except X) but it doesn't work
belly_button = 0.5 - CM_y(2);
belly_button(2) = 0.5 - CM_z(2);

% normalize with x... <- HERE! WANT TO PARALLEL TRANSFORM
... but if I plus belly_button for x and y axis , it dosn't work
cur_subject_x = cur_subject_x - min(cur_subject_x(:));
max_tall = max(cur_subject_x(:));
cur_subject_x = cur_subject_x ./ max_tall;

cur_subject_y = cur_subject_y - min(cur_subject_y(:));
cur_subject_y = cur_subject_y ./ max_tall;

cur_subject_z = cur_subject_z - min(cur_subject_z(:));
cur_subject_z = cur_subject_z ./ max_tall;


% 이미지 저장
motionpatch = cur_subject_x;
motionpatch(:,:,2) = cur_subject_y;
motionpatch(:,:,3) = cur_subject_z;
        
new_file_name = strcat('/home/rfj/바탕화면/sample.png');
imwrite(motionpatch,new_file_name);
    

% read image after write

ori = imread('/home/rfj/바탕화면/sample.png');
ori = im2double(ori);
ori = ori(:,:,:);

dx = [];
dy = [];
dz = [];

for f = 1:numel(ori(:,1,1))
    for j = 1:25
        dx = [dx;ori(f,j,1)];
        dy = [dy;ori(f,j,2)];
        dz = [dz;ori(f,j,3)];
    end
end

a = [1 0 0]; % Red 척추 1,2,3,4,20
b = [0 0 1]; % Blue 오른팔 8,9,10,11,23,24
c = [0 1 0]; % Green왼팔 5,6,7,21,22 (여기서 5번이 빠짐. 넣고싶으면 나중에 24 joint가 아니라 25 joint로 추가)
d = [1 1 0]; % Yellow 오른다리 16,17,18,19
e = [0 1 1]; % Skyblue 왼다리 12,13,14,15
colors = [a;a;a;a;c;c;c;c;b;b;b;b;e;e;e;e;d;d;d;d;a;c;c;b;b];

scatter3(dx,dy,dz,100,'filled');


connecting_joints= ...
    [2 1 21 3 21 5 6 7 21 9 10 11 1 13 14 15 1 17 18 19 2 8 8 12 12];

for jj=1:25:numel(dx)% 1부터 8개씩 numel = 열갯수..?
    current = [];
    current(:,1) = dy(jj:jj+24) ;
    current(:,2) = dz(jj:jj+24) ;
    current(:,3) = dx(jj:jj+24) ;
    
    scatter3(current(:,1),current(:,2),current(:,3),100,colors(:,:),'filled');
    
    for j =1:25
        k=connecting_joints(j);
        line([current(j,1) current(k,1)], [current(j,2) current(k,2)] , [current(j,3) current(k,3)])
    end
    
    set(gca,'Xdir','reverse','Ydir','reverse')
    xlim([0 1]);
    xlabel('x')
    ylim([0 1]);
    ylabel('y')
    zlim([0 1]);
    zlabel('z')
    drawnow
    pause(0.01)
end