function adj = meshAdjacencyMatrix(faces, varargin)
%MESHADJACENCYMATRIX Compute adjacency matrix of a mesh from set of faces.
%
%   ADJMAT = meshAdjacencyMatrix(FACES)
%   Returns a sparse NV-by-NV matrix (NV being the largest vertex index)
%   containing vertex adjacency of the mesh represented by FACES.
%   FACES is either a NF-by-3, a NF-by-4 index array, or a Nf-by-1 cell
%   array.
%
%   Example
%     [v f] = createCube;
%     adj = meshAdjacencyMatrix(f);
%
%   See also
%     meshes3d, triangulateFaces, smoothMesh

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2013-04-30,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2013 INRA - Cepia Software Platform.

% Ensures faces is a N-by-3 or N-by-4 array
if iscell(faces) || (isnumeric(faces) && size(faces, 2) > 4)
    faces = triangulateFaces(faces);
end

% forces faces to be floating point array, for sparse function
if ~isfloat(faces)
    faces = double(faces);
end
nv = max(faces(:));

% populate a sparse matrix
if size(faces, 2) == 3
    adj = sparse(...
        [faces(:,1); faces(:,1); faces(:,2); faces(:,2); faces(:,3); faces(:,3)], ...
        [faces(:,3); faces(:,2); faces(:,1); faces(:,3); faces(:,2); faces(:,1)], ...
        1.0, nv, nv);
elseif size(faces, 2) == 4
    adj = sparse(...
        [faces(:,1); faces(:,1); faces(:,2); faces(:,2); faces(:,3); faces(:,3); faces(:,4); faces(:,4)], ...
        [faces(:,4); faces(:,2); faces(:,1); faces(:,3); faces(:,2); faces(:,4); faces(:,3); faces(:,1)], ...
        1.0, nv, nv);
end

% remove double adjacencies
adj = min(adj, 1);


function results = test_fbcca(eeg, list_freqs, fs, num_harms, num_fbs)
% Steady-state visual evoked potentials (SSVEPs) detection using the filter
% bank canonical correlation analysis (FBCCA)-based method [1].
%
% function results = test_fbcca(eeg, list_freqs, fs, num_harms, num_fbs)
%
% Input:
%   eeg             : Input eeg data
%                     (# of targets, # of channels, Data length [sample])
%   list_freqs      : List for stimulus frequencies
%   fs              : Sampling frequency
%   num_harms       : # of harmonics
%   num_fbs         : # of filters in filterbank analysis
%
% Output:
%   results         : The target estimated by this method
%
% Reference:
%   [1] X. Chen, Y. Wang, S. Gao, T. -P. Jung and X. Gao,
%       "Filter bank canonical correlation analysis for implementing a
%        high-speed SSVEP-based brain-computer interface",
%       J. Neural Eng., vol.12, 046008, 2015.
%
% Masaki Nakanishi, 22-Dec-2017
% Swartz Center for Computational Neuroscience, Institute for Neural
% Computation, University of California San Diego
% E-mail: masaki@sccn.ucsd.edu

if nargin < 3
    error('stats:test_fbcca:LackOfInput', 'Not enough input arguments.');
end

if ~exist('num_harms', 'var') || isempty(num_harms), num_harms = 3; end

if ~exist('num_fbs', 'var') || isempty(num_fbs), num_fbs = 5; end

fb_coefs = [1:num_fbs].^(-1.25)+0.25;

[num_targs, ~, num_smpls] = size(eeg);
y_ref = cca_reference(list_freqs, fs, num_smpls, num_harms);
for targ_i = 1:1:num_targs
     test_tmp = squeeze(eeg(targ_i, :, :));
     for fb_i = 1:1:num_fbs
         testdata = filterbank(test_tmp, fs, fb_i);
         for class_i = 1:1:num_targs
             refdata = squeeze(y_ref(class_i, :, :));
             [~,~,r_tmp] = canoncorr(testdata', refdata');
             r(fb_i, class_i) = r_tmp(1,1);
         end % class_i
     end % fb_i
     rho = fb_coefs*r;
    [~, tau] = max(rho);
    results(targ_i) = tau;
end % targ_i

function [ y_ref ] = cca_reference(list_freqs, fs, num_smpls, num_harms)
% Generate reference signals for the canonical correlation analysis (CCA)
% -based steady-state visual evoked potentials (SSVEPs) detection [1, 2].
%
% function [ y_ref ] = cca_reference(listFreq, fs,  nSmpls, nHarms)
%
% Input:
%   listFreq        : List for stimulus frequencies
%   fs              : Sampling frequency
%   nSmpls          : # of samples in an epoch
%   nHarms          : # of harmonics
%
% Output:
%   y_ref           : Generated reference signals
%                    (# of targets, 2*# of channels, Data length [sample])
%
% Reference:
%   [1] Z. Lin, C. Zhang, W. Wu, and X. Gao,
%       "Frequency Recognition Based on Canonical Correlation Analysis for
%        SSVEP-Based BCI",
%       IEEE Trans. Biomed. Eng., 54(6), 1172-1176, 2007.
%   [2] G. Bin, X. Gao, Z. Yan, B. Hong, and S. Gao,
%       "An online multi-channel SSVEP-based brain-computer interface using
%        a canonical correlation analysis method",
%       J. Neural Eng., 6 (2009) 046002 (6pp).
%
% Masaki Nakanishi, 28-Jul-2016
% Swartz Center for Computational Neuroscience, Institute for Neural
% Computation, University of California San Diego
% E-mail: masaki@sccn.ucsd.edu

if nargin < 3
    error('stats:cca_reference:LackOfInput',...
        'Not enough input arguments.');
end

if ~exist('num_harms', 'var') || isempty(num_harms), num_harms = 3; end

num_freqs = length(list_freqs);
tidx = (1:num_smpls)/fs;
for freq_i = 1:1:num_freqs
    tmp = [];
    for harm_i = 1:1:num_harms
        stim_freq = list_freqs(freq_i);
        tmp = [tmp;...
            sin(2*pi*tidx*harm_i*stim_freq);...
            cos(2*pi*tidx*harm_i*stim_freq)];
    end % harm_i
    y_ref(freq_i, 1:2*num_harms, 1:num_smpls) = tmp;
end % freq_i


% SCRIPT TEST FOR THE UR10 ROBOT KINEMATICS

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.

close all

robot = load_robot('UR', 'UR10');
% adjust 3D view as desired
adjust_view(robot)

% test both solutions: based on the transpose an on the Moore-Penrose
robot.inversekinematic_fn = 'inverse_kinematics_jacobian(robot, T, q)';
q0 = [0.1 -pi/2 -pi/2 0.1 0.1 0.1]';
%q0 = [0.2  0.2 0.2 0.2 0.2 0.2]';
%q0 = [0.2  0.2 0.2 0.2 0.2 0.2]';

T0 = directkinematic(robot, q0)

fprintf('\nSimple test: try to reach T')
T_target = [-0.3390   -0.3390    0.8776    0.2447;
            0.8469    0.2962    0.4416   -0.4373;
            -0.4096    0.8929    0.1867    1.2568;
              0         0         0    1.0000];

qinv = inversekinematic(robot, T_target, q0)

T_reached = directkinematic(robot, qinv)
'diff T-T_target'
T_reached-T_target
% Plot manipulatiliby ellipse
drawrobot3d(robot, qinv)


function b = dgesl ( a, lda, n, ipvt, b, job )

%*****************************************************************************80
%
%% DGESL solves a real general linear system A * X = B.
%
%  Discussion:
%
%    DGESL can solve either of the systems A * X = B or A' * X = B.
%
%    The system matrix must have been factored by DGECO or DGEFA.
%
%    A division by zero will occur if the input factor contains a
%    zero on the diagonal.  Technically this indicates singularity
%    but it is often caused by improper arguments or improper
%    setting of LDA.  It will not occur if the subroutines are
%    called correctly and if DGECO has set 0.0 < RCOND
%    or DGEFA has set INFO == 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the output from DGECO or DGEFA.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix A.
%
%    Input, integer IPVT(N), the pivot vector from DGECO or DGEFA.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, integer JOB.
%    0, solve A * X = B;
%    nonzero, solve A' * X = B.
%
%    Output, real B(N), the solution vector.
%

%
%  Solve A * X = B.
%
  if ( job == 0 )

    for k = 1 : n-1

      l = ipvt(k);
      t = b(l);

      if ( l ~= k )
        b(l) = b(k);
        b(k) = t;
      end

      b(k+1:n) = daxpy ( n-k, t, a(k+1:n,k)', 1, b(k+1:n), 1 );

    end

    for k = n : -1 : 1
      b(k) = b(k) / a(k,k);
      t = -b(k);
      b(1:k-1) = daxpy ( k-1, t, a(1:k-1,k)', 1, b(1:k-1), 1 );
    end

  else
%
%  Solve A' * X = B.
%
    for k = 1 : n
      t = b(1:k-1) * a(1:k-1,k);
      b(k) = ( b(k) - t ) / a(k,k);
    end

    for k = n-1 : -1 : 1

      b(k) = b(k) + b(k+1:n) * a(k+1:n,k);
      l = ipvt(k);

      if ( l ~= k )
        [ b(l), b(k) ] = r8_swap ( b(l), b(k) );
      end

    end

  end

  return
end


function c=lpccc2cc(cc,np)
%LPCCC2PF Extrapolate complex cepstrum C=(CC)

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: lpccc2cc.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program 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 General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

p=size(cc,2);
if nargin<2 np=p; end
if np<=p
   c=cc(:,1:np);
else
   c=lpcar2cc(lpccc2ar(cc),np);
end


function [x_best, psi_best, out] = GLM_FY(mapp, lin_sym_solver, x0, options)
% GLM_FY is a Levenberg-Marquardt algorithm for solving systems of
% nonlinear equations :math:`h(x) = 0`, `x` in :math:`R^m`
% using the nonlinear unconstrained minimization :math:`\textrm{min} \psi(x) = 1/2 ||h(x)||^2`
% s.t. `x` in :math:`R^m`.
%
% USAGE:
%
%    [x_best, psi_best, out] = GLM_FY(mapp, lin_sym_solver, x0, options)
%
% INPUTS:
%    mapp:              function handle provides `h(x)` and gradient `h(x)`
%    lin_sym_solver:    function handle for solving the linear system
%    x0:                initial point
%    options:           structure including the parameteres of scheme
%
%                         * .eta - parameter of the scheme
%                         * .MaxNumIter - maximum number of iterations
%                         * .MaxNumMapEval - maximum number of function evaluations
%                         * .MaxNumGmapEval - maximum number of subgradient evaluations
%                         * .TimeLimit - maximum running time
%                         * .epsilon - accuracy parameter
%                         * .x_opt - optimizer
%                         * .psi_opt - optimum
%                         * .adaptive - update lambda adaptively
%                         * .flag_x_error - 1: saves :math:`x_{error}`, 0: do not saves :math:`x_{error}` (default)
%                         * .flag_psi_error - 1: saves :math:`\psi_{error}`, 0: do not saves :math:`\psi_{error}` (default)
%                         * .flag_time - 1: saves :math:`\psi_{error}`, 0: do not saves :math:`\psi_{error}` (default)
%                         * .Stopping_Crit - stopping criterion
%
%                           1. stop if :math:`||grad|| \leq \epsilon`
%                           2. stop if :math:`||nhxk|| \leq \epsilon`
%                           3. stop if `MaxNumIter` is reached
%                           4. stop if `MaxNumMapEval` is reached
%                           5. stop if `MaxNumGmapEval` is reached
%                           6. stop if `TimeLimit` is reached
%                           7. stop if :math:`||grad|| \leq \textrm{max}(\epsilon, \epsilon^2 * ngradx0)`
%                           8. stop if :math:`||nhxk|| \leq \textrm{max}(\epsilon, \epsilon^2 * nhx0)`
%                           9. stop if (default) :math:`||hxk|| \leq \epsilon` or `MaxNumIter` is reached
%
% OUTPUTS:
%    x_best:            the best approximation of the optimizer
%    psi_best:          the best approximation of the optimum
%    out:               structure including more output information
%
%                         * .T - running time
%                         * .Niter - total number of iterations
%                         * .Nmap - total number of mapping evaluations
%                         * .Ngmap - total number of mapping gradient evaluations
%                         * .merit_func - array including all merit function values
%                         * .x_error - relative error :math:`\textrm{norm}(x_k(:)-x_{opt}(:))/\textrm{norm}(x_{opt})`
%                         * .psi_error - relative error :math:`(\psi_k-\psi_{opt})/(\psi_0-\psi_{opt}))`
%                         * .Status - reason of termination
%
% .. REFERENCE:
% .. J. Fan, Y. Yuan, On  the  quadratic  convergence  of  the Levenberg-Marquardt method without nonsingularity assumption, Computing, 74(1), 23-39  (2005)
% .. Author: - Masoud Ahookhosh, System Biochemistry Group, Luxembourg Center for System Biomedicine, University of Luxembourg, Luxembourg
%            - Update: July 2017, M. Ahookhosh

format longG ;

% ================ Error messages for input and output =================
if nargin > 4
    error('The number of input arguments is more than what is needed');
elseif nargin < 4
    error('The number of input arguments is not enough');
end;

if isempty(mapp)
    error('the function handle mapp has to be defined');
elseif ~isa(mapp,'function_handle')
    error('mapp should be a function handle');
end

if isempty(lin_sym_solver)
    error('the function handle lin_sym_solver has to be defined');
elseif ~isa(lin_sym_solver,'function_handle')
    error('lin_sym_solver should be a function handle');
end

if isempty(x0)
    error('The starting point x0 has to be defined');
elseif ~isa(x0,'numeric')
    error('x0 should be a numeric vector');
end

% =================== initializing the parameters ======================
% ===== user has requested viewing the default values of "options" =====
[eta,epsilon,MaxNumIter,MaxNumMapEval,MaxNumGmapEval, adaptive, ...
    TimeLimit,flag_x_error,flag_psi_error,flag_time,Stopping_Crit] ...
    = Initialization(options);

if ~isa(eta,'numeric') || (eta <= 0)
    error('eta should be numeric and eta in (0,4*delta)');
end

if isfield(options,'x_opt')
    x_opt=options.x_opt;
elseif flag_x_error==1
    error('x_error requires to x_opt be specified');
end

if flag_x_error == 1
    Nxopt      = sqrt(sum(x_opt(:).^2));
    x_error(1) = sqrt(sum((x0(:)-x_opt(:)).^2))/Nxopt;
end

if flag_psi_error == 1
    psi_error(1) = 1;
end

if flag_time == 1
    Time(1) = 0;
end

xk         = x0;
Niter      = 1;
[hxk,ghxk] = mapp(x0);
Nmap       = 1;
Ngmap      = 1;
grad       = ghxk*hxk;
nhxk       = sqrt(sum(hxk.^2));
nhx0       = nhxk;
ngradx0    = sqrt(sum(grad.^2));
I          = eye(length(xk));
psik       = 0.5*nhxk^2;
merit_func = psik;
sigma      = 1e-2;
rho        = 0.2;
gamma      = 0.5; % ???
max_inner  = 5;
StopFlag   = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%% Main body of GLM_FY.m %%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T0 = tic;

% ======================= start of the main loop =======================
while ~StopFlag

    muk   = nhxk;
    Hk    = ghxk*ghxk'+muk*I;
    dk    = lin_sym_solver(Hk,grad);
    xk1   = xk+dk;
    hxk1  = mapp(xk1);
    Nmap  = Nmap+1;
    nhxk1 = norm(hxk1);
    if nhxk1 <= gamma*nhxk
        xk    = xk1;
        Niter = Niter+1;
        hxk   = mapp(xk);
        Nmap  = Nmap+1;
        nhxk  = norm(hxk);
        psik  = 0.5*nhxk^2;
    else
        psik1       = 0.5*nhxk1^2;
        alphak      = 1;
        inner_count = 0;
        while ((psik1-psik)>=sigma*alphak*(grad'*dk)&& ...
                inner_count<= max_inner)
            alphak      = rho*alphak;
            xk1         = xk+alphak*dk;
            hxk1        = mapp(xk1);
            Nmap        = Nmap+1;
            nhxk1       = norm(hxk1);
            psik1       = 0.5*nhxk1^2;
            inner_count = inner_count+1;
        end
        xk    = xk1;
        Niter = Niter+1
        nhxk  = nhxk1;
        psik  = psik1;
    end
    [hxk,ghxk] = mapp(xk);
    Nmap       = Nmap+1;
    Ngmap      = Ngmap+1;
    grad       = ghxk*hxk;

    % ================= Gathering output information ===================
    merit_func(Niter) = psik;
    if flag_time == 1
        Time(Niter+1) = toc(T0);
    end

    if flag_x_error == 1
        Nx_opt = norm(x_opt);
        x_error(Niter+1) = sqrt(sum((xk(:)-x_opt(:)).^2))/Nx_opt;
    end

    if flag_psi_error == 1
        psi_error(Niter+1) = (psik-psi_opt)/(psi0-psi_opt);
    end

    % ================== checking stopping criteria ====================
    T = toc(T0);

    [StopFlag,Status] = StopCriterion(grad,nhxk,Niter,Nmap, ...
    Ngmap,MaxNumIter,MaxNumMapEval,MaxNumGmapEval,T,TimeLimit, ...
    epsilon,nhx0,ngradx0,Stopping_Crit);

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Outputs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Status
x_best         = xk;
psi_best       = psik;
out.T          = T;
out.nhx        = nhxk;
out.merit_func = merit_func';
out.Niter      = Niter;
out.Nmap       = Nmap;
out.Ngmap      = Ngmap;
out.Status     = Status;

if flag_x_error == 1
    out.x_error = x_error;
end
if flag_psi_error == 1
    out.psi_error = psi_error;
end
if flag_time == 1
    out.Time = Time;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% End of GLM_FY.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%