%%%%%%%%%%%%%%
%
% function [trajdata] = read_traj_file(filename)
%
% This function reads a file in the .traj format described at:
%
% http://jks-folks.stanford.edu/haptic_data/
%
% The function returns a struct, with one field for each metadata
% field found in the file, and additional fields called:
%
% t,x,y,z,forcex,forcey,forcez
%
% ...which represent the actual trajectory found in this file.
%
% Author: Dan Morris
%
%%%%%%%%%%%%%
function [trajdata] = read_traj_file(filename)

trajdata = struct;

% Open the input file
f = fopen(filename,'r');
if (f == -1) return; end;

% This token signals the beginning of the data
last_comment_token = 'DATA_START';

% Loop until we reach the end of the metadata
while(1)
  
  curline = fgetl(f);
  
  % Have we reached the end of the metadata?
  if (0 == isempty(strfind(curline,last_comment_token)))
    break;
  end
  
  % Find any 'field=value' tokens on this line
  results = regexp(curline,'(\S+)=(.*)$','tokens');

  % If we didn't find anything of the form 'field=value',
  % go on to the next line
  if (isempty(results))
    continue;
  end
  
  % Grab the values from this token
  field = results{1}{1};
  value = results{1}{2};
  [nval,count] = sscanf(value,'%f');
  
  % Convert the value to a number if we can
  if (count > 0)
    trajdata = setfield(trajdata,field,nval);
  else
    trajdata = setfield(trajdata,field,value);
  end
  
end

% Now read all the data
format = repmat('%f ',1,trajdata.columns);
trajdata.data = textscan(f,format);

% Now convert the data into individual column vectors
trajdata.t = trajdata.data{1};
trajdata.x = trajdata.data{2};
trajdata.y = trajdata.data{3};
trajdata.z = trajdata.data{4};
trajdata.forcex = trajdata.data{5};
trajdata.forcey = trajdata.data{6};
trajdata.forcez = trajdata.data{7};

% Clean up
fclose(f);