function [cgJacobians] = createCGJacobianDH(rob,varargin)
%% CREATECGJACOBIANDH Computes the symbolic geometric robot link centre of gravity jacobians for open chain robot arms.
% =========================================================================
%
%  [cgJacobians] = createCGJacobianDH(rob)
%  [cgJacobians] = createCGJacobianDH(rob,robOpts)
%
%  Description:
%    The function computes the symbolic expression of the geometric
%    per link centre of gravity jacobians for the robot defined in the
%    input structure rob. The optional robOpts structure allows to specify
%    the automatic generation of m-functions and real-time capable simulink
%    blocks.
%
%    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, the function
%    creates an m-file for the geometric centre of gravity jacobians. The
%    m-function takes a vector of generalized joint values as well as a
%    vector of generalized joint velocities as input and outputs the
%    numerical jacobian matrix for these joint values. The m-functions are
%    stored in a subdirectory named after the robot.
%
%    If the createSLBlock flag is set in the robOpts structure, a Simulink
%    Embedded Matlab Function Block is automatically generated for the
%    geometric centre of gravity jacobians. The blocks take a vector of
%    generalized joint values as input and output the numerical jacobian.
%    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:
%    cgJacobians: {1xn} including the geometrical [6xn] centre of garvity
%                 jacobian for each link
%
%  Example:
%    rob = stanfordarm3;
%    cgJacobian = createCGJacobianDH(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 createDirKinematicDH,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
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

%% Load existing or invoke computation of forward kinematics
nJoints = size(rob.DH,1);   % Number of joints
q = createGenCoordinates(rob);

curPath = [pwd '\',rob.name,'\'];    % compose path to output directory
if (~exist(curPath,'dir'))                 % create output directory if it does not already exist
    dirKinC = createDirKinematicDH(rob,robOpts);
else
    tmp = load([curPath,'\DirKinematicDH.mat']);
    dirKinC = tmp.dirKinC;
end

multIDFprintf([robOpts.verbose, logFid],...
    '%s: - Creating geometric centre of gravity jacobians ...\n',datestr(now));

%% Compute the linear motion jacobians for all centres of gravity
multIDFprintf([robOpts.verbose, logFid],...
    '\tProcessing translation part for joint: ');
posJacobians = cell(1,nJoints);
for iJoints = 1:nJoints
    multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints);
    posJacobians{iJoints} = ...
        simplify(jacobian(dirKinC{iJoints}(1:3,4)+dirKinC{iJoints}(1:3,1:3)*rob.COG(:,iJoints),q ));
end
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');

% Compute the rotary motion jacobians for all centres of gravity
rotJacobians = cell(1,nJoints); % Initialize jacobian container
z0 = [0 0 1].'; % default rotary axis.
rotJacobians{1} = sym(zeros(3,nJoints));
multIDFprintf([robOpts.verbose, logFid],...
    '\tProcessing rotational part for joint: ');
for iJoints = 1:nJoints
    multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints);
    if rob.joints(iJoints) ~= 'r'
        if iJoints ==1
            % do nothing for the first joint
        else
            rotJacobians{iJoints} = rotJacobians{iJoints-1};    % copy old jacobian
        end
        
    else
        if iJoints ==1
            rotJacobians{iJoints}(:,1) = z0;
        else
            rotJacobians{iJoints} = rotJacobians{iJoints-1};    % copy old jacobian
            rotJacobians{iJoints}(:,iJoints) = dirKinC{iJoints-1}(1:3,1:3)*z0; % insert new axis of rotation expressed in base frame
        end
    end
end
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');

cgJacobians = cell(1,nJoints);
multIDFprintf([robOpts.verbose, logFid],'\t%s ','Now simplifying jacobian for joint...');
for iJoints = 1:nJoints
    multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints);
    cgJacobians{iJoints} = simplify([posJacobians{iJoints};rotJacobians{iJoints}]);
end
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');


if robOpts.saveResult
    multIDFprintf([robOpts.verbose, logFid],...
        '\tSaving computed jacobians: ');
    % cosmetics to the symbolic equations
    save([curPath,'cgJacobianDH'],'cgJacobians');
    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 centre of gravity jacobian m-functions
    for iJoints=1:nJoints
        multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints);
        
        funName = ['CGJacobian_0_',num2str(iJoints)];
        fileName = [curPath,rob.name,funName,'.m'];                        % intermediate joint forward kinematics
        
        
        matlabFunction(cgJacobians{iJoints},'file',fileName,...            % generate function m-file
            'outputs', {'cgJ'},...
            'vars', {q(1:end)});
        
        hStruct = createHeaderStruct(rob,iJoints,nJoints,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 = createBlockCGJacobianDH( 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)
    fclose(logFid);
end

%% Are the results correct?
if robOpts.selfCheck
%     [testResult.cgJacobian, maxErr ] = checkCGJacobian(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 = ['Centre of gravity jacobians for the ',rob.name,' arm up to link ',int2str(curBody),' of ',int2str(nBodies),'.'];
hStruct.calls= {['cgJ = ',fName,'(q)']};
hStruct.detailedDescription = {['Given a full set of ',int2str(nBodies),' joint variables the function'],...
    'computes the geometric centre of gravity jacobian with respect to the',...
    ['base frame for the link subsequent to joint ',int2str(curBody),'.']};
hStruct.inputs = {['q:  ',int2str(nBodies),'-element vector of generalized coordinates.'],...
    '     Angles have to be given in radians!'};
hStruct.outputs = {['cgJ:  [6x',num2str(nBodies),'] geometric jacobian relating the Cartesian'],...
    ['       velocity of the centre of gravity belonging to link ',int2str(curBody),' of ',int2str(nBodies)],...
    'expressed in the robot 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 createCGJacobianDH written by:',...
    'Jörn Malzahn    '};
hStruct.seeAlso = {rob.name,'createCGJacobianDH'};
end