function [dirKinC varargout] = createDirKinematicDH(rob,varargin)
%% CREATEDIRKINEMATICDH Computes the symbolic forward kinematics.
% =========================================================================
%
%  [dirKinC] = createDirKinematicDH(rob)
%  [dirKinC] = createDirKinematicDH(rob,robOpts)
%
%  Description:
%    The function outputs the symbolic representation of the per joint
%    analytic forward kinematics for the robot specified by rob. The
%    optional robOpts structure allows to specify the automatic generation
%    of m-functions and real-time capable simulink blocks.
%
%    The computation of the forward kinematics expression is based on the
%    Denavit-Hartenberg (DH) formalism. Up to now only serial rigid robot
%    arms are supported. Joints may be revolute or prismatic.
%
%    If the createMFun flag is set in the robOpts structure, m-files are
%    generated for the forward kinematics of each joint as well as the
%    end-effector. The generated m-functions take a vector of generalized
%    joint values as input and output the numerical homogenous
%    transformation. The m-functions are stored in a subdirectory named
%    after the robot.
%
%    If the createSLBlock flag is set in the robOpts structure, Simulink
%    Embedded Matlab Function Blocks are automatically generated for the
%    forward kinematics of each joint as well as the end-effector. The
%    Embedded Matlab Function Blocks take a vector of generalized joint
%    values as input and output the numerical homogenous transformation.
%    The blocks are added to a block library named "mdlrobotname" in the
%    subdirectory named after the robot.
%
%  Input:
%    rob :    Robot definition structure
%    robOpts: Optional robot model generation options structure.
%
%  Output:
%    dirKinC: {1xn} containing [4x4] symbolic homogenous transforms for the
%              direct kinematics for each of the n bodies.
%
%  Example:
%    rob = stanfordarm3;
%    dirKinC = createDirKinematicDH(rob)
%
%  Known Bugs:
%
%  References:
%         1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar
%         2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano
%         3) Introduction to Robotics, Mechanics and Control - Craig
%         4) Modeling, Identification & Control of Robots - Khalil & Dombre
%
%  Authors:
%    Jörn Malzahn
%    
%
%  See also createJacobianDH,stanfordarm3,getRobModelOpts.
%
%  This software may be used under the terms of CC BY-SA 3.0 license 
%          < http://creativecommons.org/licenses/by-sa/3.0/ >
%    
%          2012 RST, Technische Universität Dortmund, Germany
%               < http://www.rst.e-technik.tu-dortmund.de >
% 
% ========================================================================

%% Handle robot modeling options
% Read user specified options
if nargin > 1
    robOpts = completeRobOpts(varargin{1});
else
    robOpts = getRobotModelOpts('default');
end

% Output to logfile?
if ~isempty(robOpts.logFile)
    logFid = fopen(robOpts.logFile,'a');
else
    logFid = [];
end

multIDFprintf([robOpts.verbose, logFid],...                                
    '%s: - Creating forward kinematics ...\n',datestr(now));

%% Gather information about the robot and prepare temporary variables
nBodies = rob.n;
[ q phie dPhiedx] = createGenCoordinates(rob);                             % create symbolic variables for all generalized coordinates
if (robOpts.createMFun)
    curPath = fullfile(pwd, rob.name);                                      % compose path to output directory
    if (~exist(curPath,'dir'))                                             % create output directory if it does not already exist
        mkdir(curPath);
    end
end

%% Create the direct kinematics symbolic expression
dirKin = eye(4);                                                           % initialize containers for speed
dirKinC = cell(1,nBodies);
stateOffset = 0;

multIDFprintf([robOpts.verbose, logFid],'\tProcessing joint: ');           
for iBodies = 1:nBodies
    multIDFprintf([robOpts.verbose, logFid],' %i ',iBodies);               
    
    T = rob.links(iBodies).sym.A(q(iBodies));
    
    % Append the current homogenous transformation to the kinematic chain
    dirKin = dirKin*T;
    dirKinC{1,iBodies} = dirKin;
end
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');                

%{
%% Save results
if (robOpts.saveResult)
    savePath = fullfile(curPath,'DirKinematicDH');
    multIDFprintf([robOpts.verbose, logFid],...                            
        '\tSaving computed kinematics: ');
    % cosmetics to the symbolic direct kinematics equation
    dirKin = simple(dirKin);
    save(savePath,'dirKinC');
    
    multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');            
end

%% Create m-functions
if (robOpts.createMFun)
    multIDFprintf([robOpts.verbose, logFid],...                            
        '\tGenerating m-function for joint: ');
    
    % create per joint direct kinematics m-functions
    for iBodies=1:nBodies
        multIDFprintf([robOpts.verbose, logFid],' %i ',iBodies);           
        
        if (iBodies ~= nBodies)
            funName = ['DirKinDH_0_',num2str(iBodies)];
            fileName = fullfile(curPath,rob.name,...                               % intermediate joint direct kinematics
                [funName,'.m']);
        else
            funName = 'DirKinDH';
            fileName = fullfile(curPath,rob.name, [funName,'.m']);                    % end-effector direct kinematics
        end
        
        matlabFunction(dirKinC{1,iBodies},'file',fileName,...              % generate function m-file
            'outputs', {'T'},...
            'vars', {[q(1:end), findsym(phie),findsym(dPhiedx)]});
        
        hStruct = createHeaderStruct(rob,iBodies,nBodies,funName);                 % replace autogenerated function header
        replaceHeader(hStruct,fileName);
        
    end
    multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');            
end

%% Create Simulink Embedded Matlab Function Block
if robOpts.createSLBlock
    multIDFprintf([robOpts.verbose, logFid],...                            
        '\tGenerating Simulink Block... ');
    status = createBlockDirKinematicDH( rob );
    if (status == 1)
        multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');        
    else
        multIDFprintf([robOpts.verbose*2, logFid],'\t%s\n',' failed!');    
    end
end

if ~isempty(robOpts.logFile)                                               % close log file if necessary
    fclose(logFid);
end

%% Are the results correct?
if robOpts.selfCheck
    [testResult.directKinematics, maxErr ] = checkDirKinematics(rob,robOpts);
end

end % of main function


%% Definition of the RST-header contents for each generated file
function hStruct = createHeaderStruct(rob,curBody,nBodies,fName)
hStruct.funName = fName;
hStruct.shortDescription = ['Forward kinematics solution for the ',rob.name,' arm up to frame ',int2str(curBody),' of ',int2str(nBodies),'.'];
hStruct.calls= {['T = ',fName,'(q)']};
hStruct.detailedDescription = {['Given a set of joint variables up to joint number ',int2str(curBody),' the function'],...
                                'computes the pose belonging to that joint with respect to the base frame.'};
hStruct.inputs = {['q:  ',int2str(curBody),'-element vector of generalized coordinates.'],...
                   'Angles have to be given in radians!'};
hStruct.outputs = {['T:  [4x4] Homogenous transformation matrix relating the pose of joint ',int2str(curBody),' of ',int2str(nBodies)],...
                    '          for the given joint values to the base frame.'};
hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
    '3) Introduction to Robotics, Mechanics and Control - Craig',...
    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
hStruct.authors = {'This is an autogenerated function!',...
    'Generator createDirKinematicDH written by:',...
    'Jörn Malzahn   '};
hStruct.seeAlso = {rob.name,'createDirKinematicDH'};
end
%}