%IKINE_SYM  Symbolic inverse kinematics
%
% Q = R.IKINE_SYM(K, OPTIONS) is a cell array (Cx1) of inverse kinematic
% solutions of the SerialLink object ROBOT.  The cells of Q represent the
% different possible configurations.  Each cell of Q is a vector (Nx1), and
% element J is the symbolic expressions for the J'th joint angle.  The
% solution is in terms of the desired end-point pose of the robot which is
% represented by the symbolic matrix (3x4) with elements
%      nx ox ax tx
%      ny oy ay ty
%      nz oz az tz
% where the first three columns specify orientation and the last column
% specifies translation.
%
% K <= N can have only specific values:
%  - 2 solve for translation tx and ty
%  - 3 solve for translation tx, ty and tz
%  - 6 solve for translation and orientation
%
% Options::
%
% 'file',F    Write the solution to an m-file named F
%
% Example::
%
%         mdl_planar2
%         sol = p2.ikine_sym(2);
%         length(sol)
%         ans = 
%               2       % there are 2 solutions
%         s1 = sol{1}  % is one solution
%         q1 = s1(1);      % the expression for q1
%         q2 = s1(2);      % the expression for q2
%
% Notes::
% - Requires the Symbolic Toolbox for MATLAB.
% - This code is experimental and has a lot of diagnostic prints.
% - Based on the classical approach using Pieper's method.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
% 
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function out = ikine_sym(srobot, N, varargin)
    
    %
    % Given a robot model the following steps are performed:
    % 1. Convert model to symbolic form
    % 2. Find relevant trig equations and solve them for joint angles
    % 3. Write an M-file to implement the solution
    %      xikine(T)
    %      xikine(T, S) where S is a 3 vector with elements 1 or 2 to select
    %       the first or second solution for the corresponding joint.
    %
    % TODO:
    %  - handle the wrist joints, only first 3 joints so far
    %  - handle base and tool transforms
    
    opt.file = [];
    opt = tb_optparse(opt, varargin);
    
    % make a symbolic representation of the passed robot
    srobot = sym(srobot);
    q = srobot.gencoords();

    % test N DOF has an allowable value
    switch N
        case 2
        case 3
        case 6
        otherwise
            error('RTB:ikine_sym:badarg', 'Can only solve for 2,3,6 DOF');
    end
    
    % define symbolic elements of the homogeneous transform
    syms nx ox ax tx
    syms ny oy ay ty
    syms nz oz az tz
    syms d3
    
    % inits
    Q = {};
    trigsubOld = [];
    trigsubNew = [];

    % loop over each joint variable
    for j=1:N
        fprintf('----- solving for joint %d\n', j);
        
        % create some equations to sift through
        [left,right] = pieper(srobot, j, 'left');
        
        % decide which equations to look at
        if j <= 3
            % for first three joints only focus on translational part
            left = left(1:3, 4); left = left(:);
            right = right(1:3, 4); right = right(:);
        else
            % for last three joints only focus on rotational part
            left = left(1:3, 1:3); left = left(:);
            right = right(1:3, 1:3); right = right(:);
        end
        
        % substitute sin/cos for preceding joint as S/C, essentially removes
        % the joint variables from the equations and treats them as constants.
        if ~isempty(trigsubOld)
            left = subs(left, trigsubOld, trigsubNew);
            right = subs(right, trigsubOld, trigsubNew);
        end
        
        % then simplify the LHS
        %   do it after the substitution to prevent sum of angle terms being introduced
        left = simplify(left);
        
        % search for a solveable equation:
        %    function of current joint variable on the LHS
        %    constant element on the RHS
        k = NaN;
        for i=1:length(left)
            if hasonly(left(i), j) && isconstant(right(i))
                k = i;
                break;
            end
        end
        
        eq = [];
        
        if ~isnan(k)
            % create the equation to solve: LHS-RHS == 0
            eq = left(k) - right(k);
        else
            % ok, we weren't lucky, try another strategy
            
            % find all equations:
            %    function of current joint variable on the LHS
            
            k = [];
            for i=1:length(left)
                % has qj on the left and constant on the right
                if hasonly(left(i), j)
                    k = [k i];
                end
            end
            
            % hopefully we found two of them
            if length(k) < 2
                continue;
            end
            
            % we did, lets see if the sum square RHS is constant
            rhs = simplify(right(k(1))^2 + right(k(2))^2); % was simple
            if isconstant( rhs )
                % it is, let's sum and square the LHS
                fprintf('lets square and add %d %d\n', k);
                
                eq = simplify( expand( left(k(1))^2 + left(k(2))^2 ) ) - rhs; % was simple
            end
        end
        
        % expand the list of joint variable subsitutions
        fprintf('subs sin/cos q%d for S/C\n', j);
        trigsubOld = [trigsubOld mvar('sin(q%d)', j) mvar('cos(q%d)', j)];
        trigsubNew = [trigsubNew mvar('S%d', j) mvar('C%d', j)];
        
        if isempty(eq)
            fprintf('cant solve this equation');
            k
            left(k)==right(k)
            error('cant solve');
        end
        % now solve the equation
        if srobot.links(j).isrevolute()
            % for revolute joint it will be a trig equation, do we know how to solve it?
            Q{j} = solve_joint(eq, j );
            if isempty(Q)
                warning('cant solve this kind of equation');
            end
        else
            fprintf('prismatic case\n')
            q = sym( sprintf('q%d', j) );
            Q{j} = solve( eq == 0, q);
        end
    end
    
    % final simplification
    %  get rid of C^2+S^2 and C^4, S^4 terms
    fprintf('**final simplification pass\n')
    
    % create a list of simplifications
    %  substitute S^2 = 1-C^2, S^4=(1-C^2)^2
    tsubOld = [];
    tsubNew = [];
    for j=1:N
        tsubOld = [tsubOld mvar('S%d', j)^2 mvar('S%d', j)^4];
        tsubNew = [tsubNew 1-mvar('C%d', j)^2 (1-mvar('C%d', j)^2)^2];
    end
    
    for j=1:N
        for k=1:5
            % seem to need to iterate this, not quite sure why
            Q{j} = simplify( expand( subs(Q{j}, tsubOld, tsubNew) ) );
        end
    end

    % Q is a cell array of equations for joint variables
    if nargout > 0
        out = Q;
    end
    
    if ~isempty(opt.file)
        fprintf('**generate MATLAB code\n')
        gencode(Q);
    end
end

%PIEPER Return a set of equations using Pieper's method
%
% [L,R] = pieper(robot, n, which)
%
% If robot has link matrix A1 A2 A3 A4 then returns 12 equations from equating the coefficients of
%
%  A1' T = A2 A3 A4     n=1, which='left'
%  A2' A1' T = A3 A4    n=2, which='left'
%  A3' A2' A1' T = A4   n=3, which='left'
%
%  T A4' = A1 A2 A3     n=1, which='right'
%  T A4' A3' = A1 A2    n=2, which='right'
%  T A4' A3' A2' = A1   n=3, which='right'
%
% Judicious choice of the equations can lead to joint solutions

function [L,R] = pieper(robot, n, which)
    
    if nargin < 3
        which = 'left';
    end
    
    syms nx ox ax tx real
    syms ny oy ay ty real
    syms nz oz az tz real
        
    T = [nx ox ax tx
        ny oy ay ty
        nx oz az tz
        0  0  0  1 ];
    
    T = inv(robot.base) * T * inv(robot.tool);
    
    q = robot.gencoords();
    
    
    % Create the symbolic A matrices
    for j=1:robot.n
        A{j} = robot.links(j).A(q(j));
    end
    
    switch which
        case 'left'
            left = T;
            for j=1:n
                left = inv(A{j}) * left ;
            end
            
            right = eye(4,4);
            for j=n+1:robot.n
                right = right * A{j};
            end
            
        case 'right'
            left = T;
            for j=1:n
                left = left * inv(A{robot.n-j+1});
            end
            
            right = eye(4,4);
            for j=1:(robot.n-n)
                right = right * A{j};
            end
    end
    
    %     left = simple(left);
    %     right = simple(right);
    
    if nargout == 0
        left == right
    elseif nargout == 1
        L = left;
    elseif nargout == 2
        L = left;
        R = right;
    end
end

%SOLVE_JOINT Solve a trigonometric equation
%
% S = SOLVE_JOINT(EQ, J) solves the equation EQ=0 for the joint variable qJ.
% The result is a cell array of solutions.
%
% The equations must be of the form:
%  A cos(qJ) + B sin(qJ) = 0
%  A cos(qJ) + B sin(qJ) = C
%
% where A, B, C are arbitrarily complex expressions.  qJ can be the only
% joint variable in the expression.

function s = solve_joint(eq, j)
    
    sinj = mvar('sin(q%d)', j);
    cosj = mvar('cos(q%d)', j);
    
    A = getcoef(eq, cosj);
    B = getcoef(eq, sinj);
    
    if isempty(A) || isempty(B)
        warning('don''t know how to solve this kind of equation');
    end
    
    C = -simplify(eq - A*cosj - B*sinj);  % was simple
    
    if C == 0
        % A cos(q) + B sin(q) = 0
        s(2) = atan2(A, -B);
        s(1) = atan2(-A, B);
    else
        % A cos(q) + B sin(q) = C
        r = sqrt(A^2 + B^2 - C^2);
        phi = atan2(A, B);
        
        s(2) = atan2(C, r) - phi;
        s(1) = atan2(C, -r) - phi;
    end
    if nargout == 0
        try
            eval(s)
        catch
            s
        end
    end
end

%MVAR Create a symbolic variable
%
% V = MVAR(FMT, ARGS) is a symbolic variable created using SPRINTF
%
% eg. mvar('q%d', j)
%
% The symbolic is explicitly declared to be real.

function v = mvar(fmt, varargin)

    if isempty(strfind(fmt, '('))
        % not a function
        v = sym( sprintf(fmt, varargin{:}), 'real' );
    else
        v = sym( sprintf(fmt, varargin{:}) );
        
    end
end

%HASONLY Determine if an expression contains only certain joint variables
%
% S = HASONLY(E L) is true if the joint variables (q1, q2 etc.) in the expression E
% are listed in the vector L.
%
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1 2 3]) -> true
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1]) -> false

function s = hasonly(eq, j)
    
    q = findq(eq);
    if isempty(q)
        s = false;
    else
        s = all(ismember(j, findq(eq)));
    end
end

%ISCONSTANT Determine if an expression is free of joint variables
%
% S = ISCONSTANT(E) is true if the expression E contains no joint variables such
% q1, q2 etc.

function s = isconstant(eq)
    s = isempty(findq(eq));
end

%FINDQ Find the joint variables in expression
%
% Q = FINDQ(E) returns a list of integers indicating the joint variables found
% in the expression E.  For instance an instance of 'q1' would cause a 1 to be
% returned and so on.
%
% Eg. findq('sin(q1)*cos(q2)+S3') -> [1 2]

function q = findq(s)
    
    q = [];
    
    for var=symvar(s)
        if isempty(var)
            break
        end
        varname = char(var);
        if varname(1) == 'q'
            q = [q str2num(varname(2:end))];
        end
    end
end

function coef = getcoef(eq, trig)
    z = children( collect(eq, trig) );
    z = children( z(1) );
    coef = z(1);
end

% Output a joint expression to a file

function s = gencode(Q, filename)
    
    function s = G(s, fmt, varargin)
        s = strvcat(s, sprintf(fmt, varargin{:}));
    end

    s = 'function q = xikine(T, sol)';
    s = G(s, '  if nargin < 2; sol = ones(1, %d); end', length(Q));
    s = G(s, '  px = T(1,4); py = T(2,4); pz = T(3,4);');
    
    for j=1:3
        Qj = Q{j};   % cast it to subclass
        if length(Qj) == 1
            s = G(s, '  q(%d) = %s', j, matgen2(Qj));
        elseif length(Qj) == 2
            s = G(s, '  if sol(%d) == 1', j);
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(1)));
            s = G(s, '  else');
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(2)));
            s = G(s, '  end');
            
            
        end
        
        
        s = G(s, '  S%d = sin(q(%d));', j, j);
        s = G(s, '  C%d = cos(q(%d));', j, j);
        s = G(s, ' ');
        
        
    end
    s = G(s, 'end');
    
    fp = fopen(filename, 'w');
    for i=1:numrows(s)
        fprintf(fp, '%s\n', deblank(s(i,:)));
    end
    fclose(fp);
    
end

% Generate MATLAB code from an expression
%
% Requires a bit of a hack, a subclass of sym (sym2) to do this

function s = matgen2(e)
    
    s = matgen(sym2(e));
    
    k = strfind(s, '=');
    s = deblank( s(k+2:end) );
end