% 맨처음 방향 (frame1) 으로 일괄 orientation normalize
% => 1번프레임이 아니라 프레임들의 중앙값
% 키로 모든방향 normalize
% 노멀라이즈시 최소값 빼는게 아니라 joint1-> 0.5/0.5/0.5로 가도록 
% 실험할때 전방향 노멀라이즈도 해봐야겠다..
path_name ='/home/hyuna/Documents/actionGAN_work/24/24_all_skeleton/';
dinfo=dir('/home/hyuna/Documents/actionGAN_work/24/24_all_skeleton/S007C002P016R002A024.skeleton');
%txt=fullfile(dir_to_search, '*.skeleton');
%dinfo=dir(txt);
count=[0,0,0];

for d = 1: length(dinfo)
    label=-1;
    file_name = dinfo(d).name;
    % disp(name);
    
    name = strcat(path_name,file_name(1:20),'.skeleton');
    [token,remainder] = strtok(file_name,'A');
 
    class = str2num(remainder(2:4));
    %class=remainder(2:4);
    
    if class == 24
        bodyinfo = read_skeleton_file(name);
        frame_num = size(bodyinfo,2);   
    
    else
        continue;
    end
    
    if isempty(bodyinfo) || isempty(bodyinfo(1).bodies())
        disp("empty body");
        newdir='/home/hyuna/Documents/actionGAN_work/24/24_emptybody.txt';
      
        txtfile=fopen(newdir,'a');
        fprintf(txtfile, file_name(1:20));
        fprintf(txtfile, '\n');
        fclose(txtfile);
        count(1)=count(1)+1;
        continue;
    end
    
     
    %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

    %Orientation normalization 1 : in space
    %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

    % for save origin subjects before data augment
    clear_subject_x = cur_subject_x;
    clear_subject_y = cur_subject_y;
    clear_subject_z = cur_subject_z;

    % patch_num*25
    red=[4,3,21,2,1];
    green=[5,6,7,8,23,22];
    blue=[9,10,11,12,25,24];
    yellow=[17,18,19,20];
    skyblue=[13,14,15,16];
    
    target = {skyblue,yellow,green,blue};
    [r,c]=size(target);
    
   %celldisp(target);
   %disp(c);
   %disp(target{1});
 
    % joint-to-joint   
    for tar_index = 1:c    
        diff_ave = comp_aver(clear_subject_x,clear_subject_y,clear_subject_z,frame_num,target{tar_index});

        if label~=0
           label=set_label(diff_ave,0.13); 
        end    

        if label==0 %bad
            disp(file_name(1:20));
            disp("difference");
            disp(tar_index);
            %disp(target(tar_index));
            disp(diff_ave);
        end
    end
    
    
    % reds(head-spine) shoud be parallel to z
    % 4 3 21 2 1
    
    max_t=[];
    
    for j = 1: length(red)-1
        theta_per_patch=[];
        for patch = 1:frame_num
            u = [clear_subject_x(patch, red(j+1))-clear_subject_x(patch, red(j)),clear_subject_y(patch, red(j+1))-clear_subject_y(patch, red(j)),clear_subject_z(patch, red(j+1))-clear_subject_z(patch, red(j))];
            % v = axis z
            v = [0,0,1];
            % angle between [vector joint to joint] and [axis z]
            theta = atan2(norm(cross(u,v)),dot(u,v));
            %if theta > 90
                %theta = theta - 90;
            %end

            %print all the theta and max of it.
            %disp(theta);

           theta_per_patch=[theta_per_patch, theta];
        end
     % maximum theta for each bones through all the patches
     % max_t[4]
     max_t = [max_t,max(theta_per_patch)];
     clear theta_per_patch;

    end
    %disp("max");
    %disp(max_t);
     % usually 1.6<max angle<=1.8
   if label~=0
   label=set_label(max_t,1.8);
   end 
    % green, blue(arms) should be located in almost the same position at the
    % beginning and end of motion.
    
    
    %print a distance between starting and ending coords
    g_distance=loc_end_to_end(clear_subject_x,clear_subject_y,clear_subject_z,frame_num,skyblue);
    b_distance=loc_end_to_end(clear_subject_x,clear_subject_y,clear_subject_z,frame_num,yellow);
    
    
    % only use just 2 joints at the tip of arms(hands)
    g_end=g_distance(end-1:end);
    b_end=b_distance(end-1:end);
    
    if label~=0
    label=set_label(g_end,0.7);
    end
 
    % check if label is still 'good'
    if label~=0
    label=set_label(b_end,0.7);
    end
    
  
    % save good and bad examples seperately 
    newdir='';
    if label==0 % bad
        newdir='/home/hyuna/Documents/actionGAN_work/24/24_bad.txt';
        count(2)=count(2)+1;
    else
        newdir='/home/hyuna/Documents/actionGAN_work/24/24_good.txt';
        count(3)=count(3)+1;
        copyfile(name,'/home/hyuna/Documents/actionGAN_work/24/24_good_skeleton');
        
        major =major_limb(clear_subject_z, skyblue, yellow);
        if(major==skyblue)
            s_dir='/home/hyuna/Documents/actionGAN_work/24/good_right.txt';
            copyfile(name,'/home/hyuna/Documents/actionGAN_work/24/24_right_skeleton');
        else
           s_dir='/home/hyuna/Documents/actionGAN_work/24/good_left.txt';
           copyfile(name,'/home/hyuna/Documents/actionGAN_work/24/24_left_skeleton');
        end


        split=fopen(s_dir,'a');
        fprintf(split, file_name(1:20));
        fprintf(split, '\n');
        fclose(split);
    
    end
   
    txtfile=fopen(newdir,'a');
    fprintf(txtfile, file_name(1:20));
    fprintf(txtfile, '\n');
    fclose(txtfile);
    
   
end

% number of [emptybody, bad, good]
disp(count);

function setlabel=set_label(target,value)
if target(target>value) %bad
        label=0;
    else
        label=1;        
end
setlabel=label;
end


function loc_end=loc_end_to_end(clear_subject_x,clear_subject_y,clear_subject_z,frame_num,target)
distance = []
     for j = 1: length(target)
        x_=clear_subject_x(1, target(j))-clear_subject_x(frame_num, target(j));
        y_=clear_subject_y(1, target(j))-clear_subject_y(frame_num, target(j));
        z_=clear_subject_z(1, target(j))-clear_subject_z(frame_num, target(j));
        d=sqrt(x_*x_+y_*y_+z_*z_);
        distance=[distance,d];
     end
loc_end =distance;
% disp(distance);
end

function comp_ave =comp_aver(clear_subject_x,clear_subject_y,clear_subject_z, frame_num,target)
for j = 1: length(target)    
         % distance between a particular joint and average coordinate per patch
        dist_ave=[];
        for patch = 2:frame_num-1
            ave_x=(clear_subject_x(patch-1,target(j))+clear_subject_x(patch+1,target(j)))/2;
            ave_y=(clear_subject_y(patch-1,target(j))+clear_subject_y(patch+1,target(j)))/2;
            ave_z=(clear_subject_z(patch-1,target(j))+clear_subject_z(patch+1,target(j)))/2;
                       
            % distance between joint and average 
            jnt_ave = sqrt((abs(clear_subject_x(patch, target(j))-ave_x)).^2+ (abs(clear_subject_y(patch,  target(j))-ave_y)).^2+ (abs(clear_subject_z(patch,  target(j))-ave_z)).^2);
            dist_ave=[dist_ave,jnt_ave];
                       
        end
        
end
    comp_ave=dist_ave;
end
function limb= major_limb(clear_subject_z, target1, target2)
    z1=max(clear_subject_z(:,target1(end))) - clear_subject_z(1, target1(end));
    z2=max(clear_subject_z(:,target2(end))) - clear_subject_z(1, target2(end));

    if z1>z2 % target1 raised higher than t2
        limb=target1;
    else
        limb=target2;
    end 
     
end