function [ MotionData,orgtheta,orgthetaw ] = Prep_InvarRoot( MotionData )
%PREP_INVARROOT Summary of this function goes here
%   Detailed explanation goes here

addpath ../Toolboxes/rotationMatrix
 
 
if any(strcmp(MotionData.skel.type,{'ms2','bvh','ms2.5'}))
    %MovingStories - Series 2
%     d0 = [0 1 0]'; %straight ahead
%     l0 = [1 0 0]'; %out right hip
%     v0 = [0 0 1]'; %neg vertical axis
%     
        d0 = [0 0 1]'; %straight ahead
        l0 = [1 0 0]'; %out right hip
        v0 = [0 1 0]'; %neg vertical axis
        %MovingStories - Series 2 this is what the rest position looks like
    %d0 = [0 -1 0]'; %straight ahead
    %l0 = [1 0 0]'; %out right hip
    %v0 = [0 0 1]'; %neg vertical axis
    
    x=1;
    y=3;
    z=2; % this is the height of the root with respect to the ground plane
    
else if any(strcmp(MotionData.skel.type,{'cmubvh'}))
        %CMU
        d0 = [0 0 1]'; %straight ahead
        l0 = [1 0 0]'; %out right hip
        v0 = [0 1 0]'; %neg vertical axis
        
        x=1;
        y=3;
        z=2; % this is the height of the root with respect to the ground plane
        
   
    end
end

rootRep = [];


%%%%%%%%%%% Convert to Body-Centered Orientations %%%%%%%%%%%%%
%%% as per Taylor's formulas

for m=1:length(MotionData.data)    
    
    for f=1:size(MotionData.data{m},1)             
        expRot = MotionData.data{m}(f,MotionData.skel.tree(1).expmapInd);
        
        Rot = RotationMatrix(expRot,'exponentialMap');
        
        R = Rot.GetRotationMatrix();
        
        D = R' * d0;
        L = R' * l0;
        
       
        %roll
        phi = acos(dot(D,-v0)/norm(D)); %this corresponds to V in the formulas
        
        %pitch
        psi = acos(dot(L,-v0)/norm(L)); %this corresponds to U in the formulas
        
        %yaw
        top = find(abs(d0)==1);
        bot = find(abs(l0)==1);
        
        
        if any(strcmp(MotionData.skel.type,{'cmubvh'}))
        %CMU - Lateral (L) is on X axis BUT somehow D works! ????
        theta = atan2(L(top),L(bot));   %% be careful: choose the components of the u based on the index d0 and l0 point
        
        elseif any(strcmp(MotionData.skel.type,{'ms2','bvh','ms2.5'}))
        %MS2 - Lateral (L) 
        theta = atan2(L(top),L(bot));   %% be careful: choose the components of the u based on the index d0 and l0 point
                    
        end
        
        %feta = atan2(D(3),D(2));
        feta = acos(dot([D(3) D(1)],[L(3) L(1)])/(norm([D(3) D(1)])*norm([L(3) L(1)])));
        
        %feta = atan(norm(cross(D,L))/ dot(D,L));%atan2(D(2),D(1));theta-feta
        %feta = acos(dot(D,L)/(norm(D)*norm(L)));
        
        rootRep{m}(f,4:7) = [phi psi theta feta];
    end   
    orgtheta = rootRep{m}(:,7);
    rootRep{m}(:,6) = unwrap(rootRep{m}(:,6));
    rootRep{m}(:,7) = unwrap(rootRep{m}(:,7));
    orgthetaw = rootRep{m}(:,7);
    pp = rootRep{m}(:,6) - rootRep{m}(:,7);
    
    figure(1);plot(rootRep{m}(:,6));
    figure(2);plot(orgtheta);
    figure(3);plot(orgthetaw);
    
    
    ddd = atan2(sin(rootRep{m}(:,6)-rootRep{m}(:,7)), cos(rootRep{m}(:,6)-rootRep{m}(:,7)));
    
    rootRep{m}(:,7) = mod(rootRep{m}(:,6),2*pi) - mod(rootRep{m}(:,7),2*pi);
    rootRep{m}(:,7) = rootRep{m}(:,6) - rootRep{m}(:,7);
    rootRep{m}(:,7) = unwrap(rootRep{m}(:,7));
    
    rootRep{m}(:,7) = pp;
     
    figure(4);plot(ddd);
    figure(5);plot(pp);
%%%%%%%%%%% Convert to Incremental Changes %%%%%%%%%%%%%
%%% as per Taylor's formulas

    
    for f=1:size(MotionData.data{m},1)-1
        
        
        theta = rootRep{m}(f,6);
        
        delta_theta = rootRep{m}(f+1,6) - rootRep{m}(f,6);
        %delta_feta = rootRep{m}(f+1,7) - rootRep{m}(f,7);
        delta_x = MotionData.data{m}(f+1,x) - MotionData.data{m}(f,x);
        delta_y = MotionData.data{m}(f+1,y) - MotionData.data{m}(f,y);
        
        alpha = norm([delta_y delta_x]);
        beta = atan2(delta_y,delta_x);   
        
        
        gamma = alpha * cos(theta - beta);
        xi = alpha * sin(theta - beta);
        
        %rootRep{m}(f,:) = [gamma xi  MotionData.data{m}(f,z) phi psi delta_theta];
        rootRep{m}(f,1:3) = [gamma xi  MotionData.data{m}(f,z)];
        rootRep{m}(f,6) = delta_theta;
        %rootRep{m}(f,7) = delta_feta;
               
    end 
    rootRep{m}(f+1,:) = rootRep{m}(f,:);
    
    %MotionData.data{m}(:,1:6) = rootRep{m}(:,1:6);
    %MotionData.data{m}(:,7) = rootRep{m}(:,7);
    
    MotionData.data{m} = [rootRep{m}(:,1:7) MotionData.data{m}(:,7:end)];
    
end

end

