%-------------------------------------------------------------------------%
% Copyright (c) 2019 Modenese L.                                          %
%                                                                         %
% Licensed under the Apache License, Version 2.0 (the "License");         %
% you may not use this file except in compliance with the License.        %
% You may obtain a copy of the License at                                 %
% http://www.apache.org/licenses/LICENSE-2.0.                             %
%                                                                         % 
% Unless required by applicable law or agreed to in writing, software     %
% distributed under the License is distributed on an "AS IS" BASIS,       %
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or         %
% implied. See the License for the specific language governing            %
% permissions and limitations under the License.                          %
%                                                                         %
%    Author:   Luca Modenese                                              %
%    email:    l.modenese@imperial.ac.uk                                  % 
% ----------------------------------------------------------------------- %
% This script converts a .sto file generated by OpenSim analyses in a
% matlab structure "MatStruct" with fields "colheaders" and "data".
% This is a convenient script to robustly read OpenSim files.
%
% INPUTS:
%           - Storage_file: a .sto file, including path, e.g. from static
%           optimization or JointReaction analysis.
% OUTPUTS:
%           -MatStruct: matlab structure with the following fields:
%               a) MatStruct.colheaders: cell array of headers of the
%                                           .sto file.
%               b) MatStruct.data: matrix containing the data that
%                                        were stored in the input file.
%
%
% written: 2014/2015 (Griffith University)
% last modified: 
% 26 Jun 2017: comments (LM)
% 29 Nov 2018: changed name - added to MFD Matlab version.
% ----------------------------------------------------------------------- %
function MatStruct = sto2Mat(storage_file)

% OpenSim suggested settings
import org.opensim.modeling.*
OpenSimObject.setDebugLevel(3);

% read storage file
openSimStorage = Storage(storage_file);

% extract time column and place it in time array
timeColumn = ArrayDouble();
openSimStorage.getTimeColumn(timeColumn);
for n_t = 0:timeColumn.getSize-1
    time_vec(n_t+1,1) = timeColumn.getitem(n_t);
end

% converting labels
OSLabelsArray = openSimStorage.getColumnLabels;
for n_lab = 0:OSLabelsArray.getSize-1
    MatStruct.colheaders{n_lab+1} = char(OSLabelsArray.get(n_lab));
end

% converting data
dataColumn = ArrayDouble();
ind = 0;
while openSimStorage.getDataColumn(ind, dataColumn)
    for n_d = 0:dataColumn.getSize-1
        StorageData(n_d+1,ind+1) = dataColumn.getitem(n_d);
    end
    ind = ind+1;
end
MatStruct.data = [time_vec, StorageData];

% checking that dimensionality of data and colheaders are consistent
n_diff = size(MatStruct.colheaders,2)-size(MatStruct.data,2);

% if not consistent let the user decide what to do
if n_diff>0
    answ = menu(['In the conversion from storage to MATLAB structure ',num2str(n_diff),' more header(s) than data were found. Removing extra headers (y) or stop (n).'], 'Yes','No');
    if answ == 1
        MatStruct.colheaders = MatStruct.colheaders(1:size(MatStruct.data,2));
    elseif answ == 2
        error('Please check your sto files manually.');
    end
end

end