%-------------------------------------------------------------------------%
% 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                                  %
% ----------------------------------------------------------------------- %
%
% A function to realize the kinematic state of a model. It is currently
% slow but robust.
%
% TODO: write comments
%
% written: 2014/2015 (Griffith University)
% last modified: 
% February 2017 (comments)
% 29/11/2018 renamed and added to MFD Matlab version.
% ----------------------------------------------------------------------- %
function state = realizeMatStructKinematics(osimModel, state, kinStruct, n_frame)
% TO DO
% this function is slow. I should find a way of passing in only a row of
% coordinates! That would eliminate the need of a n_frame as well.

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

% getting model coordinates and their number
coordsModel = osimModel.updCoordinateSet();
N_coordsModel = coordsModel.getSize();

% checking is kinematics matches coordinates order
[out, missing_coords_list] = isKinMatchingModelCoords(osimModel, kinStruct.colheaders);

% correct if there are generalised coordinates not computed in IK (e.g.
% models modified after running IK)
if ~out
    for n_m = 1:length(missing_coords_list)
        coordName = missing_coords_list{n_m};
        missing_coords_vals(n_m) =  coordsModel.get(coordName).getDefaultValue;
    end
else
    missing_coords_vals = [];
end
% if needed
% kinStruct.colheader = [kinStruct.colheaders, missing_coords_list];

% account for time column in data structure
if any(strcmp('time', kinStruct.colheaders))    
    kin_state_angles = [kinStruct.data(n_frame, 2:end), missing_coords_vals];
else
    kin_state_angles = [kinStruct.data(n_frame, :), missing_coords_vals];
end

% looping thought coordinates to assign them a value
for n_modelCoords = 0:N_coordsModel-1
    
    % Value of the state variable at that frame
    cur_coordValue = kin_state_angles(n_modelCoords+1);
    
    % assigning the coordinates depending on their motion type
    switch char(coordsModel.get(n_modelCoords).get_motion_type())
        case 'rotational'
            % transform to radiant for angles
            coordsModel.get(n_modelCoords).setValue(state,cur_coordValue*pi/180);
        case 'translational'
            % not changing linear distances
            coordsModel.get(n_modelCoords).setValue(state,cur_coordValue);
        case 'coupled'
            error('motiontype ''coupled'' is not managed by realizeMatStructKinematics.m');
        otherwise
            error('motion type not recognized. Please check OpenSim model (maybe update error?')
    end
end

end