%% Function that transform all coordinates in input matrix to a reference point in first frame
function [ Xtransformed, Xhbc ] = transformAllToFirstFrameF( Xin, noOfFrames )
	% Xin = this is expected to comprise rows of examples, with columns of coordinates and frames of them
	% noOfFrames = no of frames in columns; each frame is expected to have multiple of 3 coordinates (x,y,z)
	% Xtransformed = this is the output matrix with all coordinates transformed

    fprintf("\nTransforming coordinates ...\n");
    
    %% Enumerate 15 joints
    SKEL_HEAD = 1;
    SKEL_NECK = 2;
    SKEL_TORSO = 3;
    SKEL_LEFT_SHOULDER = 4;
    SKEL_LEFT_ELBOW = 5;
    SKEL_LEFT_HAND = 6;
    SKEL_RIGHT_SHOULDER = 7;
    SKEL_RIGHT_ELBOW = 8;
    SKEL_RIGHT_HAND = 9;
    SKEL_LEFT_HIP = 10;
    SKEL_LEFT_KNEE = 11;
    SKEL_LEFT_FOOT = 12;
    SKEL_RIGHT_HIP = 13;
    SKEL_RIGHT_KNEE = 14;
    SKEL_RIGHT_FOOT = 15;
    	
	[ noOfExamples n ] = size( Xin);
	noOfCoordinatesPerFrame = n / noOfFrames;
	noOfJoints = noOfCoordinatesPerFrame / 3;
	Xtranformed = zeros( size( Xin ) );
	
	Xhbc = zeros( noOfExamples, 4,4 );
	
	for i=1:noOfExamples, 
		hbc = zeros(4); % declare transformation matrix to be used through out this example
        %% For each frame
        for j=1:noOfFrames,
            XframeFlat = Xin(i,noOfCoordinatesPerFrame*(j-1)+1:j*noOfCoordinatesPerFrame);
            Xframe = zeros(3,noOfJoints); %3 coordinates per joints
            for k=1:noOfJoints,
                Xframe(:,k) = XframeFlat(3*(k-1)+1:k*3)';
            endfor %rows are x,y,z of each joint, for convenient access
    
            %1% get torso point of first frame as origin of local coordinate
            if j==1, % torso at first frame is the reference coordinate frame
                t = Xframe(:,SKEL_TORSO); % torso, vec(t)
				nt = -1*t; % -vec(t)
                vtemp = Xframe(:,SKEL_LEFT_SHOULDER) - Xframe(:,SKEL_RIGHT_SHOULDER);  % vec(ls) - vec(ls)
                vtemp = vtemp .* [1;0;1]; % [ vec(ls) - vec(ls) ]y=0
                xb = vtemp / norm(vtemp); % vec(xb')
				xc = [ 1;0;0 ]; % vec(xc)
				theta = 0;
                hbc = [ cos(theta),0,sin(theta),nt(1) ; 0,1,0,nt(2) ; -1*sin(theta),0,cos(theta),nt(3) ; 0,0,0,1 ];  % translation first
				theta = sign( cross(xb,xc)(2) )*acos( dot(xb,xc) );
				hbc = [ cos(theta),0,sin(theta),0 ; 0,1,0,0 ; -1*sin(theta),0,cos(theta),0 ; 0,0,0,1 ]*hbc;  % rotation next
						%homogeneous transform matrix for coordinate frame change
            endif
        
            % transform all coordinates
            vtemp = ones( 1,noOfJoints );
			Xtemp = [ Xframe ; vtemp ]; % add ones to last row
			Xtemp = hbc * Xtemp;
			Xtemp = Xtemp( 1:3 , : ); % remove the ones in last row
			Xtransformed(i,noOfCoordinatesPerFrame*(j-1)+1:j*noOfCoordinatesPerFrame) = Xtemp(:); % flatten and store this frame
        
        endfor %end of for j=1:noOfFrames
        Xhbc( i,:,: ) = hbc; % store hbc of each sample
    endfor %end of for i=1:noOfExamples
	fprintf("Coordinate transform done!\n\n");
endfunction
