function [ GenLog ] = Post_InvarRoot( GenLog,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','ms3'}))
    x=3;
    y=1;
    z=2; % this is the height of the root with respect to the ground plane
else if any(strcmp(MotionData.skel.type,{'cmubvh'}))
        x=3;
        y=1;
        z=2; % this is the height of the root with respect to the ground plane
    end
end

invarRootRep = GenLog.postvis(:,1:7);
orgRootRep = [];

orgRootRep(:,z) = invarRootRep(:,3);
%%%%%%%%%%% Starting Values %%%%%%%%%%%
orgRootRep(1,x) = [49.5586000000000];
orgRootRep(1,y) = [17.7164000000000];

orgRootRep(1,x) = [5.51261000000000];
orgRootRep(1,y) = [-165.299000000000];

%ms2
orgRootRep(1,6) = 0;0.0159;0.2170; % this is the orientation/rotation about the vertical axes - i.e., facing direction
orgRootRep(1,7) = 0;0.1227;2.31706714734187; % we are just keeping this to reconstruct the dorsoventral D easier.

%cmu
%orgRootRep(1,6) = 0;2.83721773485250; % this is the orientation/rotation about the vertical axes - i.e., facing direction
%orgRootRep(1,7) = 0;orgRootRep(1,6)-4.71136561725648;-1.87414788240397; % we are just keeping this to reconstruct the dorsoventral D easier.


%%%%%%%%%%% Convert from Incremental Values %%%%%%%%%%%%%


delta_theta = invarRootRep(:,6);
delta_feta = invarRootRep(:,7);
delta_x = invarRootRep(:,1);
delta_y = invarRootRep(:,2);

for f=2:size(GenLog.postvis,1)
    
    %get back the accumulative theta
    orgRootRep(f,6) = orgRootRep(f-1,6) + delta_theta(f-1);
    
    %get back the accumulative feta
    orgRootRep(f,7) = orgRootRep(f-1,7) + delta_feta(f-1);
    
    %calculate the offset
    beta = atan2(delta_y(f-1,1),delta_x(f-1,1));
    
    mag = norm([delta_x(f-1,1) delta_y(f-1,1)]);
    %beta =
    theta = orgRootRep(f-1,6); %taken with respect to the starting frame
    %al is the original orientation of the velocity vector (delta)
    al = theta - beta;
    orgRootRep(f,x) = orgRootRep(f-1,x) + mag*sin(al); %add x component
    orgRootRep(f,y) = orgRootRep(f-1,y) + mag*cos(al); %add y component
    
    
end


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


phi = invarRootRep(:,4); %out right hip
psi = invarRootRep(:,5); %straight ahead
quat = [];

for f=1:size(orgRootRep,1)-1
    
    theta = mod(orgRootRep(f,6),2*pi);
    
    
    %%%%%% reconstruct lateral, L
    Ly = -cos(psi(f));
    
    
    %L should be a unit vector, so Lx^2+Ly^2+Lz^2 = 1
    
    if theta < pi/2
        over = tan(theta);
        Lx = sqrt( (1 - Ly^2) / (1+over^2));
        Lz = Lx * over;
        
    elseif theta < pi
        over = tan(pi - theta);
        Lx = sqrt( (1 - Ly^2) / (1+over^2));
        Lz = Lx * over;
        
        Lx = -Lx;
    elseif theta < 3*pi/2
        over = tan(theta - pi);
        Lx = sqrt( (1 - Ly^2) / (1+over^2));
        Lz = Lx * over;
        
        Lx = -Lx;
        Lz = -Lz;
    else
        over = tan(2*pi - theta);
        Lx = sqrt( (1 - Ly^2) / (1+over^2));
        Lz = Lx * over;
        
        Lz = -Lz;
    end
    
    %%%%%% reconstruct dorsoventral, D
    Dy = -cos(phi(f));
    
    %D should be a unit vector, so Dx^2+Dy^2+Dz^2 = 1
    %also, we know that projection of D on the xz plane should be
    %perpendicular to the L's projection on that plane - we use feta(!) for
    %that
    
    
    % we store the difference between the projection of L and the
    % projection of D on the ground (x,z) plane. (theta and feta). Let's
    % construct the feta:
    
%    if theta < pi/2
%         feta = theta - invarRootRep(f,7);
% 
%     elseif theta < pi
%         over = tan(pi - feta);
% 
%     elseif theta < 3*pi/2
%         over = tan(feta - pi);
%    
%     else
%         over = tan(2*pi - feta);
%        
%     end 
    
    feta = theta - invarRootRep(f,7);
    %feta = theta - 3*3.14159265359/2;
    feta = mod(feta,2*pi);
       
    if feta < pi/2
        over = tan(feta);
        Dx = sqrt( (1 - Dy^2) / (1+over^2));
        Dz = Dx * over;
        quat(f) = 1;
        
         Dx2 = -((Dy*Ly + Dz*Lz)/Lx);
    elseif feta < pi
        over = tan(pi - feta);
        Dx = sqrt( (1 - Dy^2) / (1+over^2));
        Dz = Dx * over;
        Dx = -Dx;
        quat(f) = 2;
        
         Dx2 = -((Dy*Ly + Dz*Lz)/Lx);
         %Dz = - Dz;
    elseif feta < 3*pi/2
        over = tan(feta - pi);
        Dx = sqrt( (1 - Dy^2) / (1+over^2));
        Dz = Dx * over;
        
        Dx = -Dx;
        Dz = -Dz;
        quat(f) = 3;
        
         Dx2 = -((Dy*Ly + Dz*Lz)/Lx);
    else
        over = tan(2*pi - feta);
        Dx = sqrt( (1 - Dy^2) / (1+over^2));
        Dz = Dx * over;
        
        Dz = -Dz;
        
        quat(f) = 4;
         Dx2 = -((Dy*Ly + Dz*Lz)/Lx);
    end
    
    Dx2 = -((Dy*Ly + Dz*Lz)/Lx);
    
    L = [Lx  Ly Lz]; %out right hip
    D1 = [Dx  Dy Dz]; %straight ahead
   
    
    D2 = [Dx2  Dy Dz]; %straight ahead
    D2 = D2/norm(D2);
    
     A = -cross(L,D1);
     
     
    if abs(norm(A)-1)>1E-5
        %        error('err');
        A = A/norm(A); %ensure that v is a unit vector
    end
    
    if abs(norm(D2)-1)>1E-5
        error('Expected unit vectors; D is not a unit vector');
    end
    
    if  abs(norm(L)-1)>1E-5
        error('Expected unit vectors; L is not a unit vector');
    end
    
    if  abs(norm(A)-1)>1E-5
        error('Expected unit vectors; A is not a unit vector');
    end
    %%%%
    
    R = [L;A;D1];
    


    
    R1=R;
    
    %%%%%%%%%%%%%%%%%%%%%%
  Dy = -cos(phi(f));  
     %now we have a quadratic equation to solve for vz
  a = (Lx^2 + Lz^2);
  b = 2*Ly*Lz*Dy;
  c = (Lx^2*Dy^2 - Lx^2 + Ly^2*Dy^2);

  %there are two solutions to the quadratic equation, and one is right
  %depending on the quadrant of psi
  %note the other solution corresponds to the vector coming out of the other
  %hip
  if theta > 3*pi/2  || theta < pi/2
    Dz = (-b + sqrt(b^2 - 4*a*c))/(2*a);
  else
    Dz = (-b - sqrt(b^2 - 4*a*c))/(2*a);
  end
  

  Dx = (Ly*cos(phi(f)) - Lz*Dz) / Lx;
  
  D = [Dx  Dy Dz]; %straight ahead
    A = -cross(L,D);
    
    if abs(norm(A)-1)>1E-5
        %        error('err');
        A = A/norm(A); %ensure that v is a unit vector
    end
    
    R2 = [L;A;D];
    %%%%%%%%%%%%%%%%%%%%%%
    
    
    %%%%%%%%%%%%%%%%%%%%%%
    
     Dy = -cos(phi(f));
     
     
     a = Dy*Lz - Dz*Ly;
     b = Dz*Lx - Dx*Lz;
     c = Dx*Ly - Dy*Lx;
    
     % a^2+b^2+c^2 = 1
     %Dx = (-(Dy*Ly+Dz*Lz)/Lx);
    
     b = Dz*Lx - (-(Dy*Ly+Dz*Lz)/Lx)*Lz;
     c = (-(Dy*Ly+Dz*Lz)/Lx)*Ly - Dy*Lx;  
     
     
     
    %%%%%%%%%%%%%%%%%%%%%
    
    R = R1;
    
        tempInv = inv(R);
    tempTransl = R';
    if max(abs(tempInv(:) - tempTransl(:) )) > 1e-4
     f
     R
     R=R1;
      Rot = RotationMatrix(R,'rotationMatrix');
    exp = Rot.GetExponentialMap();
    
    %orgRootRep(f,MotionData.skel.tree(1).expmapInd) = exp;
    
     orgRootRep(f,MotionData.skel.tree(1).expmapInd) = orgRootRep(f-1,MotionData.skel.tree(1).expmapInd);
     else
    
%     if abs(det(R)-1) > 1e-4
%         
%         f
%     end
%     
%     if f==8
%         f
%     end
    
    Rot = RotationMatrix(R,'rotationMatrix');
    exp = Rot.GetExponentialMap();
    
    orgRootRep(f,MotionData.skel.tree(1).expmapInd) = exp;
    end
    
end

GenLog.postvis = [orgRootRep(:,1:6) GenLog.postvis(:,8:end)];

end

