%reorder_force.m
clear
clc

% read in mot file

fname = 'quickhop 6_grf';
force = read_motionFile([fname,'.mot']);


flabels = force.labels;
fdata = force.data;
nr = force.nr;
nc = force.nc;


rotationmatrix = [0 0 -1; 0 1 0; 1 0 0];
rotated_force_data = zeros(size(fdata));
rotated_force_data(:,1) = fdata(:,1);
for I = 1:(nc-1)/3
    rotated_force_data(:,3*I-1:3*I+1) = [rotationmatrix*fdata(:,3*I-1:3*I+1)']';
end


% add columns if necessary
if nc ~= 19
    flabels = [flabels,flabels(1,2:nc)];
    nc = length(flabels);
    filled_force_data = [rotated_force_data, zeros(nr,9)];
else
    filled_force_data = rotated_force_data;
end
% collect forces and positions
f = 0;
p = 0;
t = 0;
for j = 1:nc
    if strncmpi(flabels(j), 'ground_force_v', length('ground_force_v'))
        f = f+1;
        forces_labels(f) = flabels(j);
        forcesonly(:,f) = filled_force_data(:,j);
    elseif strncmpi(flabels(j), 'ground_force_p', length('ground_force_p'))
        p = p+1;
        positions_labels(p) = flabels(j);
        positions_only(:,p) = filled_force_data(:,j);
    elseif strncmpi(flabels(j), 'ground_torque', length('ground_torque'))
        t = t+1;
        torques_labels(t) = flabels(j);
        torques_only(:,t) = filled_force_data(:,j);
    end
end
ordered_labels = [flabels(1,1), forces_labels(1,1:3), positions_labels(1,1:3), ...
    forces_labels(1,4:6), positions_labels(1,4:6), torques_labels(1,1:3), torques_labels(1,4:6)];
ordered_data = [filled_force_data(:,1), forcesonly(:,1:3), positions_only(:,1:3),...
    forcesonly(:,4:6), positions_only(:,4:6), torques_only(:,1:3), torques_only(:,4:6)];

force.labels = ordered_labels;    
force.data=ordered_data;
force.nr=nr;
force.nc=nc;

new_fname = [fname,'_rot.mot'];
write_motionFile(force,new_fname);

