function [ outdata ] = postprocess2 (indata, facing, start_x, start_z)
  %addpath('/home/omid/Code/deepmove/Libs/MocapToolbox/MOCAP0p136');
  %addpath('/home/omid/Code/deepmove/Libs/TaylorScripts');
  addpath('matlab');

%%%%%%%%%%
% Note our convention (because I started this way with the CMU code)
% x- ground plane axis (looking at screen, going to the right)
% z- ground plane axis (looking at screen, going "in")
% y- vertical axis
% For MIT data, subject at 0 position is facing positive x axis
% channel 1 - x component (exponential map)
% channel 2 - z component (exponential map)
% channel 3 - y (vertical) component (exponential map)
% Note this is the way that MATLAB expects it -- for the CMU data, we need
%   to reverse channels 2,3 when we plot because the ordering is (x,y,z)
%%%%%%%%%%%%%%%%

%We unwrapped before, so that deltas could be calculated for the rotation
%about the vertical axis
%But now we want to wrap back to -pi < theta =< pi so before we call
%expmap2rotmat

%Extract relative angles
phi = indata(:,1);         % rotation along anteroposterior
theta = indata(:,2);       % rotation along mediolateral

%Extract the deltas
vertrotdelta = indata(:,3);    % angular velocity along the vertical axis
groundxdelta = indata(:,4);    % anterior component of the velocity
groundydelta = indata(:,5);    % lateral component of the velocity

%Remember original dimensions are different for CMU and MIT

  pos_x = 1;
  pos_y = 2;
  pos_z = 3;
  rot_x = 4;
  rot_y = 5;
  rot_z = 6;
  
  %For CMU, the absolute position is saved in a different dimension than it
  %originally was in; We need to write it back to this dimension
  indata(:,pos_y) = indata(:,6);  % vertical position
 

%%% FIRST FRAME
%Assuming we start all at zero (could put in initial conditions here)

%if we have defined the facing variable
%set this in radians
if exist('facing','var')
  indata(1,rot_y) = facing;% could be, for example,  newrep{1}(1,3)
else
  indata(1,rot_y) =  1.5846;
end
indata(1,pos_x) = [start_x];
indata(1,pos_z) = [start_z];

%%% END FIRST FRAME

%STEP ONE - CONVERT FROM DELTAS TO REAL VALUES
%           PROCESS THE GROUND PLANE TRANSLATIONS

%loop through each frame
for ii=2:size(indata,1)
  %We have horizontal movements (deltas) in the body-centred reference
  %frame
  %need to convert to horizontal positions in standard reference frame
  %orientation first (cumulative sum)
  indata(ii,rot_y) = indata(ii-1,rot_y) + vertrotdelta(ii-1,1);
  
  %now position
  %first, the magnitude
  m = norm([groundxdelta(ii-1,1) groundydelta(ii-1,1)]);
  %calculate the offset
  dab = atan2(groundydelta(ii-1,1),groundxdelta(ii-1,1));
  be = indata(ii-1,rot_y); %taken with respect to the starting frame
  %al is the original orientation of the velocity vector (delta)
  al = be - dab;
  indata(ii,pos_x) = indata(ii-1,pos_x) + m*cos(al); %add x component
  indata(ii,pos_z) = indata(ii-1,pos_z) + m*sin(al); %add z component
end

%STEP TWO - CONVERT FROM BODY-CENTRED ORIENTATIONS TO EXPMAPS
%loop through each frame
for ii=1:size(indata,1)
  %for rotation about the vertical, work in radians
  %wrapping to 0,2pi makes it easier
  psi = mod(indata(ii,rot_y),2*pi);

  %Remember what was being stored as "angle about the vertical"
  %CMU - the right hip's offset from the x-axis
  %MIT - the "straight ahead's" offset from the x-axis
  %Here, we will let u refer to this vector (either hip or straight ahead)
  %v will be the other vector
  %w will point straight up
  %this will describe the rotation, and from these axes we can calculate
  %the rotation matrix and then Euler angles


    %CMU DATA
    uy = -cos(theta(ii));

  %finding x and z components of u trickier, as it depends on the quadrant
  if psi < pi/2
    uz_over_ux = tan(psi);
    magux = sqrt((1 - uy^2)/(1 + uz_over_ux^2));
    maguz = magux*uz_over_ux;
    uz = maguz; %pos
    ux = magux; %pos
  elseif psi < pi
    uz_over_ux = tan(pi - psi);
    magux = sqrt((1 - uy^2)/(1 + uz_over_ux^2));
    maguz = magux*uz_over_ux;
    uz = maguz;  %pos
    ux = -magux; %neg
  elseif psi < 3*pi/2
    uz_over_ux = tan(psi - pi);
    magux = sqrt((1 - uy^2)/(1 + uz_over_ux^2));
    maguz = magux*uz_over_ux;
    uz = -maguz; %neg
    ux = -magux; %neg
  else
    uz_over_ux = tan(2*pi - psi);
    magux = sqrt((1 - uy^2)/(1 + uz_over_ux^2));
    maguz = magux*uz_over_ux;
    uz = -maguz;  %neg
    ux = magux; %pos
  end
  
  %Now find the components of v
  %Remember for MIT and CMU, v means different things  

    %CMU DATA
    vy = -cos(phi(ii));

  
  %now we have a quadratic equation to solve for vz
  a = (ux^2 + uz^2);
  b = 2*uy*uz*vy;
  c = (ux^2*vy^2 - ux^2 + uy^2*vy^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 psi > 3*pi/2  || psi < pi/2
    vz = (-b + sqrt(b^2 - 4*a*c))/(2*a);
  else
    vz = (-b - sqrt(b^2 - 4*a*c))/(2*a);
  end
  
  %The rotation matrix is built up a little differently for MIT and CMU  
  
    %CMU DATA
    vx = (uy*cos(phi(ii)) - uz*vz) / ux;
    u = [ux uy uz]; %out right hip
    v = [vx vy vz]; %straight ahead
    
    %Occasionally I am finding that the solution for v is not exactly a
    %unit vector; this causes problems as the resulting rotation matrix 
    %isn't valid
    %So we check here and rescale when necessary
    if abs(norm(v)-1)>1E-5
      v = v/norm(v); %ensure that v is a unit vector
    end
    
    wv = -cross(u,v);
    
    if abs(norm(u)-1)>1E-5 || abs(norm(v)-1)>1E-5 || abs(norm(wv)-1)>1E-5
      error('Expected unit vectors; one of u,v,wv is not a unit vector');
    end
    %   %NOTE that the r__ subscripts refer to the order, and not to our definition
    %   %that we have used in the rest of this code
    %   %Yes -- it is confusing!
    %   %here, x- 1st dimension, y- 2nd dimension, z- 3rd dimension
    %   %regardless of what these dimensions represent
    rxx = dot(u,[1 0 0]);
    rxy = dot(u,[0 1 0]);
    rxz = dot(u,[0 0 1]);
    ryx = dot(wv, [1 0 0]);
    ryy = dot(wv, [0 1 0]);
    ryz = dot(wv, [0 0 1]);
    rzx = dot(v, [1 0 0]);
    rzy = dot(v, [0 1 0]);
    rzz = dot(v, [0 0 1]);

    R = [rxx ryx rzx; rxy ryy rzy; rxz ryz rzz]';
    %Now for display purposes, we want the exponential map representation
    %So do the conversion, based on the rotation matrix
    
    
%     if ii==7
%         ii
%     end
    
        
    if (norm(R*R'-eye(3,3))>1E-6 || det(R)<0)
       ii 
       indata(ii,4:6) = indata(ii-1,4:6);
    else
    
    r = rotmat2expmap(R);
 
    indata(ii,4:6) = r;
    end
  

end

%clear unnecessary variables
clear rxx rxy rxz ryx ryy ryz rzx rzy rzz R Eul u v wv ux uy uz vx vy vz;
clear phi theta psi a b c maguz maguz uz_over_ux;


outdata = indata;

end